stash

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

@@ -485,16 +485,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 +506,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;
+                }
 
             }
         };

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

@@ -20,7 +20,7 @@ public class ServoPositions {
     public static double spinEndPos = 0.85;
 
     public static double shootAllAutoSpinStartPos = 0.2;
-    public static double shootAllSpindexerSpeedIncrease = 0.02;
+    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;
@@ -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()) {
+            if (gamepad2.dpadUpWasPressed()) {
-                hoodOffset -= 0.03;
+                hoodOffset -= hoodSpeedOffset;
-                autoHoodOffset -= 0.02;
+                autoHoodOffset -= hoodSpeedOffset;
-
+                gamepad2.rumble(80);
-            } else if (gamepad2.dpadDownWasPressed() || gamepad1.dpadDownWasPressed()) {
-                hoodOffset += 0.03;
-                autoHoodOffset += 0.02;
 
+            } else if (gamepad2.dpadDownWasPressed()) {
+                hoodOffset += hoodSpeedOffset;
+                autoHoodOffset += hoodSpeedOffset;
+                gamepad2.rumble(80);
             }
 
-            if (gamepad2.cross) {
+            if (gamepad2.dpadLeftWasPressed()){
-                manualOffset = 0;
+                manualOffset -= turretSpeedOffset;
-                overrideTurr = true;
+                gamepad2.rumble(80);
+            } else if (gamepad2.dpadRightWasPressed()){
+                manualOffset += turretSpeedOffset;
+                gamepad2.rumble(80);
             }
 
-            if (gamepad2.squareWasPressed()) {
+            if (gamepad2.rightBumperWasPressed()){
-                drive = new MecanumDrive(hardwareMap, new Pose2d(20, 0, 0));
+                limelightUsed = true;
-                sleep(1500);
+                gamepad2.rumble(80);
+            } else if (gamepad2.leftBumperWasPressed()){
+                limelightUsed = false;
+                gamepad2.rumble(80);
             }
 
-            if (gamepad2.triangle) {
+            if (gamepad2.crossWasPressed()){
-                autoHood = false;
-                hoodOffset = 0;
-            }
 
-            if (gamepad2.circleWasPressed()) {
-                xOffset = robotX;
-                yOffset = robotY;
-                headingOffset = robotHeading;
-
-                autoHood = true;
-                fixedTurret = false;
             }
 
             if (enableSpindexerManager) {
@@ -330,8 +330,6 @@ public class TeleopV3 extends LinearOpMode {
 //            TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
 //            TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
 
-//            TELE.update();
-
             ticker++;
         }
     }

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.15, B_PID_I = 0.0, B_PID_D = 0.025;
+    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,8 +279,8 @@ public class Turret {
         }
 
         // Set servo positions
-        robot.turr1.setPosition(turretPos);
+        robot.turr1.setPosition(turretPos + manualOffset);
-        robot.turr2.setPosition(1.0 - turretPos);
+        robot.turr2.setPosition(1.0 - turretPos - manualOffset);
 
 
         /* ---------------- TELEMETRY ---------------- */