Merge remote-tracking branch 'origin/danielv5'

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java

@@ -100,13 +100,23 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
 
     public static double spindexerSpeedIncrease = 0.03;
 
-    public static double obeliskTurrPos1 = 0.52;
-    public static double obeliskTurrPos2 = 0.53;
-    public static double obeliskTurrPos3 = 0.54;
+    public static double redObeliskTurrPos1 = 0.52;
+    public static double redObeliskTurrPos2 = 0.53;
+    public static double redObeliskTurrPos3 = 0.54;
+
+    public static double blueObeliskTurrPos1 = 0.28;
+    public static double blueObeliskTurrPos2 = 0.27;
+    public static double blueObeliskTurrPos3 = 0.26;
+    double obeliskTurrPos1 = 0.0;
+    double obeliskTurrPos2 = 0.0;
+    double obeliskTurrPos3 = 0.0;
     public static double normalIntakeTime = 3.0;
     public static double shoot1Turr = 0.57;
     public static double shoot0XTolerance = 1.0;
-    public static double turretShootPos = 0.52;
+    public static double redTurretShootPos = 0.52;
+    public static double blueTurretShootPos = 0.26;
+    double turretShootPos = 0.0;
+
     public static double shootAllTime = 1.8;
     public static double shoot0Time = 1.6;
     public static double intake1Time = 3.0;
@@ -485,16 +495,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
 
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
+                    robot.limelight.pipelineSwitch(1);
                 }
 
                 ticker++;
-                robot.limelight.pipelineSwitch(1);
                 motif = turret.detectObelisk();
-                if (redAlliance){
-                    robot.limelight.pipelineSwitch(4);
-                } else {
-                    robot.limelight.pipelineSwitch(2);
-                }
 
                 robot.turr1.setPosition(turrPos);
                 robot.turr2.setPosition(1 - turrPos);
@@ -511,7 +516,16 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
                 TELE.addData("Hood", robot.hood.getPosition());
                 TELE.update();
 
-                return !shouldFinish;
+                if (shouldFinish){
+                    if (redAlliance){
+                        robot.limelight.pipelineSwitch(4);
+                    } else {
+                        robot.limelight.pipelineSwitch(2);
+                    }
+                    return false;
+                } else {
+                    return true;
+                }
 
             }
         };
@@ -795,6 +809,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
                 yShoot = rShootY;
                 hShoot = rShootH;
 
+                obeliskTurrPos1 = redObeliskTurrPos1;
+                obeliskTurrPos2 = redObeliskTurrPos2;
+                obeliskTurrPos3 = redObeliskTurrPos3;
+                turretShootPos = redTurretShootPos;
+
             } else {
                 robot.light.setPosition(0.6);
 
@@ -830,6 +849,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
                 yShoot = bShootY;
                 hShoot = bShootH;
 
+                obeliskTurrPos1 = blueObeliskTurrPos1;
+                obeliskTurrPos2 = blueObeliskTurrPos2;
+                obeliskTurrPos3 = blueObeliskTurrPos3;
+                turretShootPos = blueTurretShootPos;
+
             }
 
             shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java

@@ -16,33 +16,38 @@ public class Poses {
 
     public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1);
     public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
-    public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140.1);
+    public static double rx2b = 19, ry2b = 40, rh2b = Math.toRadians(140.1);
 
     public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140);
 
     public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
+
+    public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140);
+
     public static double rx3b = 38, ry3b = 56, rh3b = Math.toRadians(140.1);
 
-    public static double rx4a = 72, ry4a = 50, rh4a = Math.toRadians(145);
-    public static double rx4b = 42, ry4b = 80, rh4b = Math.toRadians(145.1);
+    public static double rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140);
+    public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1);
 
-    public static double bx1 = 40, by1 = 7, bh1 = 0;
-    public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140);
-    public static double bx2b = 25, by2b = -38, bh2b = Math.toRadians(-140);
+    public static double bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1);
+    public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140);
+    public static double bx2b = 23, by2b = -36, bh2b = Math.toRadians(-140.1);
     public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140);
 
-    public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140);
-    public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140);
+    public static double bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140);
+    public static double bx3b = 38, by3b = -56, bh3b = Math.toRadians(-140.1);
+    public static double bx3aG = 55, by3aG = -43, bh3aG = Math.toRadians(-140);
 
-    public static double bx4a = 72, by4a = -55, bh4a = Math.toRadians(-140);
-    public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140);
+
+    public static double bx4a = 75, by4a = -53, bh4a = Math.toRadians(-140);
+    public static double bx4b = 45, by4b = -83, bh4b = Math.toRadians(-140.1);
     public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
 
     public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50);
     public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50);
 
-    public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140);
-    public static double bxPrep = 50, byPrep = -10, bhPrep = Math.toRadians(140);
+    public static double bShootX = 40, bShootY = 7, bShootH = Math.toRadians(-50);
+    public static double bxPrep = 45, byPrep = -10, bhPrep = Math.toRadians(-50);
 
 
     public static Pose2d teleStart = new Pose2d(0, 0, 0);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java

@@ -20,6 +20,10 @@ public class Poses_V2 {
 
     public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140), rt2c  = Math.toRadians(Math.PI/2);
 
+    public static double rXGateA = 27, rYGateA = 56, rHGateA = Math.toRadians(160);
+
+    public static double rXGateB = 40, rYGateB = 43, rHGateB = Math.toRadians(159);
+
     public static double rXGate = 30, rYGate = 63, rHGate = Math.toRadians(179);
 
     public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
@@ -36,6 +40,10 @@ public class Poses_V2 {
     public static double rShootX = 40, rShootY = 7, rShootH = Math.toRadians(140);
 
     public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140);
+
+    public static double bXGateA = 33, bYGateA = 61, bHGateA = Math.toRadians(165);
+    public static double bXGateB = 33, bYGateB = 61, bHGateB = Math.toRadians(165);
+
     public static double bXGate = 25, bYGate = 69, bHGate = Math.toRadians(165);
 
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java

@@ -16,11 +16,11 @@ public class ServoPositions {
 
     public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6;
     public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4;
-    public static double spinStartPos = 0.25;
+    public static double spinStartPos = 0.22;
     public static double spinEndPos = 0.85;
 
     public static double shootAllAutoSpinStartPos = 0.2;
-    public static double shootAllSpindexerSpeedIncrease = 0.04;
+    public static double shootAllSpindexerSpeedIncrease = 0.014;
     public static double shootAllTime = 1.8;
 
     public static double transferServo_out = 0.15;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java

@@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
+import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -39,10 +40,12 @@ public class TeleopV3 extends LinearOpMode {
     public double vel = 3000;
     public boolean autoVel = true;
     public boolean targetingHood = true;
-    public double manualOffset = 0.0;
+    public static double manualOffset = 0.0;
     public boolean autoHood = true;
 
     public double shootStamp = 0.0;
+    public static double hoodSpeedOffset = 0.01;
+    public static double turretSpeedOffset = 0.01;
 
     boolean fixedTurret = false;
     Robot robot;
@@ -117,7 +120,7 @@ public class TeleopV3 extends LinearOpMode {
 
         while (opModeIsActive()) {
 
-            TELE.addData("Is limelight on?", robot.limelight.getStatus());
+            //TELE.addData("Is limelight on?", robot.limelight.getStatus());
 
             // LIGHT COLORS
             spindexer.ballCounterLight();
@@ -191,43 +194,40 @@ public class TeleopV3 extends LinearOpMode {
             //HOOD:
 
             if (targetingHood) {
-                robot.hood.setPosition(targetingSettings.hoodAngle);
+                robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset);
             } else {
                 robot.hood.setPosition(hoodDefaultPos + hoodOffset);
             }
 
-            if (gamepad2.dpadUpWasPressed() || gamepad1.dpadUpWasPressed()) {
-                hoodOffset -= 0.03;
-                autoHoodOffset -= 0.02;
-
-            } else if (gamepad2.dpadDownWasPressed() || gamepad1.dpadDownWasPressed()) {
-                hoodOffset += 0.03;
-                autoHoodOffset += 0.02;
+            if (gamepad2.dpadUpWasPressed()) {
+                hoodOffset -= hoodSpeedOffset;
+                autoHoodOffset -= hoodSpeedOffset;
+                gamepad2.rumble(80);
 
+            } else if (gamepad2.dpadDownWasPressed()) {
+                hoodOffset += hoodSpeedOffset;
+                autoHoodOffset += hoodSpeedOffset;
+                gamepad2.rumble(80);
             }
 
-            if (gamepad2.cross) {
-                manualOffset = 0;
-                overrideTurr = true;
+            if (gamepad2.dpadLeftWasPressed()){
+                manualOffset -= turretSpeedOffset;
+                gamepad2.rumble(80);
+            } else if (gamepad2.dpadRightWasPressed()){
+                manualOffset += turretSpeedOffset;
+                gamepad2.rumble(80);
             }
 
-            if (gamepad2.squareWasPressed()) {
-                drive = new MecanumDrive(hardwareMap, new Pose2d(20, 0, 0));
-                sleep(1500);
+            if (gamepad2.rightBumperWasPressed()){
+                limelightUsed = true;
+                gamepad2.rumble(80);
+            } else if (gamepad2.leftBumperWasPressed()){
+                limelightUsed = false;
+                gamepad2.rumble(80);
             }
 
-            if (gamepad2.triangle) {
-                autoHood = false;
-                hoodOffset = 0;
-            }
-
-            if (gamepad2.circleWasPressed()) {
-                xOffset = robotX;
-                yOffset = robotY;
-                headingOffset = robotHeading;
-
-                autoHood = true;
-                fixedTurret = false;
+            if (gamepad2.crossWasPressed()) {
+                drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
             }
 
             if (enableSpindexerManager) {
@@ -259,21 +259,21 @@ public class TeleopV3 extends LinearOpMode {
 
                     if (shooterTicker == 0) {
                         spindexer.prepareShootAllContinous();
-                        TELE.addLine("preparing to shoot");
+                        //TELE.addLine("preparing to shoot");
 //                    } else if (shooterTicker == 2) {
 //                        //robot.transferServo.setPosition(transferServo_in);
 //                        spindexer.shootAll();
 //                        TELE.addLine("starting to shoot");
                     } else if (!spindexer.shootAllComplete()) {
                         robot.transferServo.setPosition(transferServo_in);
-                        TELE.addLine("shoot");
+                        //TELE.addLine("shoot");
                     } else {
                         robot.transferServo.setPosition(transferServo_out);
                         //spindexPos = spindexer_intakePos1;
                         shootAll = false;
                         spindexer.resetSpindexer();
                         //spindexer.processIntake();
-                        TELE.addLine("stop shooting");
+                        //TELE.addLine("stop shooting");
                     }
                     shooterTicker++;
                     //spindexer.processIntake();
@@ -311,11 +311,11 @@ public class TeleopV3 extends LinearOpMode {
 //            TELE.addData("oddColor", oddBallColor);
 //
 //            // Spindexer Debug
-            TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1));
-            TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition);
-            TELE.addData("spinIntakeState", spindexer.currentIntakeState);
-            TELE.addData("spinTestCounter", spindexer.counter);
-            TELE.addData("autoSpintake", autoSpintake);
+//            TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1));
+//            TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition);
+//            TELE.addData("spinIntakeState", spindexer.currentIntakeState);
+//            TELE.addData("spinTestCounter", spindexer.counter);
+//            TELE.addData("autoSpintake", autoSpintake);
 //
 //            TELE.addData("shootall commanded", shootAll);
 //            // Targeting Debug
@@ -324,14 +324,12 @@ public class TeleopV3 extends LinearOpMode {
 //            TELE.addData("robotInchesX", targeting.robotInchesX);
 //            TELE.addData( "robotInchesY", targeting.robotInchesY);
 //            TELE.addData("Targeting Interpolate", turretInterpolate);
-            TELE.addData("Targeting GridX", targeting.robotGridX);
-            TELE.addData("Targeting GridY", targeting.robotGridY);
-            TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
-            TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
+//            TELE.addData("Targeting GridX", targeting.robotGridX);
+//            TELE.addData("Targeting GridY", targeting.robotGridY);
+//            TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
+//            TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
 //            TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
 
-            TELE.update();
-
             ticker++;
         }
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java

@@ -49,16 +49,25 @@ public class Targeting {
         KNOWNTARGETING[1][0] = new Settings (2300.0, 0.93);
         KNOWNTARGETING[1][1] = new Settings (2300.0, 0.93);
         KNOWNTARGETING[1][2] = new Settings (2600.0, 0.78);
-        KNOWNTARGETING[1][3] = new Settings (2800.0, 0.62);
-        KNOWNTARGETING[1][4] = new Settings (3000.0, 0.55);
+//        KNOWNTARGETING[1][3] = new Settings (2800.0, 0.62);
+//        KNOWNTARGETING[1][4] = new Settings (3000.0, 0.55);
+//        KNOWNTARGETING[1][5] = new Settings (3200.0, 0.50);
+        KNOWNTARGETING[1][3] = new Settings (2800.0, 0.68);  // Real settings replaced with (0,3) new Settings (2800.0, 0.62);
+        KNOWNTARGETING[1][4] = new Settings (3000.0, 0.58);  // Real setting replaced with (0,4)  new Settings (3000.0, 0.55);
         KNOWNTARGETING[1][5] = new Settings (3200.0, 0.50);
         // ROW 2
+//        KNOWNTARGETING[2][0] = new Settings (2500.0, 0.78);
+//        KNOWNTARGETING[2][1] = new Settings (2500.0, 0.78);
+//        KNOWNTARGETING[2][2] = new Settings (2700.0, 0.60);
+//        KNOWNTARGETING[2][3] = new Settings (2900.0, 0.53);
+//        KNOWNTARGETING[2][4] = new Settings (3100.0, 0.50);
+//        KNOWNTARGETING[2][5] = new Settings (3100.0, 0.50);
         KNOWNTARGETING[2][0] = new Settings (2500.0, 0.78);
         KNOWNTARGETING[2][1] = new Settings (2500.0, 0.78);
         KNOWNTARGETING[2][2] = new Settings (2700.0, 0.60);
-        KNOWNTARGETING[2][3] = new Settings (2900.0, 0.53);
-        KNOWNTARGETING[2][4] = new Settings (3100.0, 0.50);
-        KNOWNTARGETING[2][5] = new Settings (3100.0, 0.50);
+        KNOWNTARGETING[2][3] = new Settings (2800.0, 0.62);  // Real settings replaced with (1,3) new Settings (2900.0, 0.53);
+        KNOWNTARGETING[2][4] = new Settings (3000.0, 0.55);  // real settings replaces with (1,4) new Settings (3100.0, 0.50);
+        KNOWNTARGETING[2][5] = new Settings (3200.0, 0.50);  // real settings replaced with (1,5) new Settings (3100.0, 0.50);
         // ROW 3
         KNOWNTARGETING[3][0] = new Settings (2900.0, 0.50);
         KNOWNTARGETING[3][1] = new Settings (2900.0, 0.50);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.utils;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
+import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.manualOffset;
 
 import static java.lang.Math.abs;
 
@@ -26,6 +27,7 @@ public class Turret {
     public static double turrDefault = 0.4;
     public static double turrMin = 0.15;
     public static double turrMax = 0.85;
+    public static boolean limelightUsed = true;
 
     public static double visionCorrectionGain = 0.08;  // Single tunable gain
     public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
@@ -54,7 +56,7 @@ public class Turret {
     LLResult result;
 
     private PIDController bearingPID;
-    public static double B_PID_P = 0.3, B_PID_I = 0.0, B_PID_D = 0.05;
+    public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
     boolean bearingAligned = false;
 
     public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
@@ -84,11 +86,6 @@ public class Turret {
     }
 
     private void limelightRead() { // only for tracking purposes, not general reads
-        if (redAlliance) {
-            webcam.pipelineSwitch(4);
-        } else {
-            webcam.pipelineSwitch(2);
-        }
 
         result = webcam.getLatestResult();
         if (result != null) {
@@ -223,7 +220,7 @@ public class Turret {
 
         limelightRead();
         // Active correction if we see the target
-        if (result.isValid() && !lockOffset) {
+        if (result.isValid() && !lockOffset && limelightUsed) {
             currentTrackOffset += bearingAlign(result);
             currentTrackCount++;
 //            double bearingError = Math.abs(tagBearingDeg);
@@ -282,21 +279,21 @@ public class Turret {
         }
 
         // Set servo positions
-        robot.turr1.setPosition(turretPos);
-        robot.turr2.setPosition(1.0 - turretPos);
+        robot.turr1.setPosition(turretPos + manualOffset);
+        robot.turr2.setPosition(1.0 - turretPos - manualOffset);
 
 
         /* ---------------- TELEMETRY ---------------- */
 
-        TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg);
-        TELE.addData("Target Pos", "%.3f", targetTurretPos);
-        TELE.addData("Current Pos", "%.3f", currentPos);
-        TELE.addData("Commanded Pos", "%.3f", turretPos);
-        TELE.addData("LL Valid", result.isValid());
-        TELE.addData("LL getTx", result.getTx());
-        TELE.addData("LL Offset", offset);
+//        TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg);
+//        TELE.addData("Target Pos", "%.3f", targetTurretPos);
+//        TELE.addData("Current Pos", "%.3f", currentPos);
+//        TELE.addData("Commanded Pos", "%.3f", turretPos);
+//        TELE.addData("LL Valid", result.isValid());
+//        TELE.addData("LL getTx", result.getTx());
+//        TELE.addData("LL Offset", offset);
 //        TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET");
-        TELE.addData("Learned Offset", "%.2f", offset);
+//        TELE.addData("Learned Offset", "%.2f", offset);
     }
 
 }