stash for dany

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

@@ -18,6 +18,7 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.ParallelAction;
 import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.SequentialAction;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.TranslationalVelConstraint;
 import com.acmerobotics.roadrunner.Vector2d;
@@ -39,13 +40,13 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Close extends LinearOpMode {
     public static double shoot0Vel = 2300, shoot0Hood = 0.93;
-    public static double spindexerSpeedIncrease = 0.014;
+    public static double velGate0Start = 2700, hoodGate0Start = 0.6;
 
-    double obeliskTurrPos1 = 0.0;
-    double obeliskTurrPos2 = 0.0;
-    double obeliskTurrPos3 = 0.0;
+    public static double velGate0End = 2700, hoodGate0End = 0.35;
+    public static double hood0MoveTime = 2;
+    public static double spindexerSpeedIncrease = 0.016;
 
-    public static double shootAllTime = 2;
+    public static double shootAllTime = 4;
     public static double intake1Time = 3.3;
     public static double intake2Time = 3.8;
 
@@ -61,7 +62,19 @@ public class Auto_LT_Close extends LinearOpMode {
     public static double shoot2Time = 2.5;
     public static double shoot3Time = 2.5;
     public static double colorSenseTime = 1.2;
-    public int motif = 0;
+    public static double waitToShoot0 = 0.5;
+    public static double waitToPickupGate2 = 0.3;
+    public static double pickupStackGateSpeed = 30;
+    public static double intake2TimeGate = 3;
+    public static double shoot2GateTime = 1.7;
+    public static double endGateTime = 22;
+    public static double waitToPickupGateWithPartner = 0.7;
+    public static double waitToPickupGateSolo = 0.01;
+    public static double intakeGateTime = 8;
+    public static double shootGateTime = 1.7;
+    public static double shoot1GateTime = 1.7;
+    public static double intake1GateTime = 3.3;
+    public static double lastShootTime = 27;
 
     Robot robot;
     MultipleTelemetry TELE;
@@ -74,8 +87,9 @@ public class Auto_LT_Close extends LinearOpMode {
     Targeting.Settings targetingSettings;
     AutoActions autoActions;
     Light light;
-    double x1, y1, h1;
 
+    int motif = 0;
+    double x1, y1, h1;
     double x2a, y2a, h2a, t2a;
 
     double x2b, y2b, h2b, t2b;
@@ -87,13 +101,21 @@ public class Auto_LT_Close extends LinearOpMode {
     double x4b, y4b, h4b;
 
     double xShoot, yShoot, hShoot;
-    double xGate, yGate, hGate;
-    double xPrep, yPrep, hPrep;
+    double xShoot0, yShoot0, hShoot0;
+    double pickupGateAX, pickupGateAY, pickupGateAH;
+    double pickupGateBX, pickupGateBY, pickupGateBH;
+    double xShootGate, yShootGate, hShootGate;
     double xLeave, yLeave, hLeave;
+    double xLeaveGate, yLeaveGate, hLeaveGate;
 
     int ballCycles = 3;
     int prevMotif = 0;
-
+    boolean gateCycle = true;
+    boolean withPartner = true;
+    double obeliskTurrPos1 = 0.0;
+    double obeliskTurrPos2 = 0.0;
+    double obeliskTurrPos3 = 0.0;
+    double waitToPickupGate = 0;
 
     // initialize path variables here
     TrajectoryActionBuilder shoot0 = null;
@@ -103,6 +125,9 @@ public class Auto_LT_Close extends LinearOpMode {
     TrajectoryActionBuilder shoot2 = null;
     TrajectoryActionBuilder pickup3 = null;
     TrajectoryActionBuilder shoot3 = null;
+    TrajectoryActionBuilder shoot0ToPickup2 = null;
+    TrajectoryActionBuilder gateCyclePickup = null;
+    TrajectoryActionBuilder gateCycleShoot = null;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -136,13 +161,18 @@ public class Auto_LT_Close extends LinearOpMode {
 
         servos.setTransferPos(transferServo_out);
 
-        limelightUsed = false;
+        limelightUsed = true;
 
         robot.light.setPosition(1);
 
         while (opModeInInit()) {
 
+            if (gateCycle){
+                servos.setHoodPos(hoodGate0Start);
+            } else {
                 servos.setHoodPos(shoot0Hood);
+            }
+
             turret.setTurret(turrDefault);
 
             if (gamepad2.crossWasPressed()) {
@@ -165,7 +195,13 @@ public class Auto_LT_Close extends LinearOpMode {
             }
 
             if (gamepad2.squareWasPressed()){
+                if (!gateCycle){
                     turret.pipelineSwitch(1);
+                } else if (redAlliance){
+                    turret.pipelineSwitch(4);
+                } else {
+                    turret.pipelineSwitch(2);
+                }
                 robot.limelight.start();
                 gamepad2.rumble(500);
             }
@@ -197,9 +233,7 @@ public class Auto_LT_Close extends LinearOpMode {
                 x4b = rx4b;
                 y4b = ry4b;
                 h4b = rh4b;
-                xPrep = rxPrep;
-                yPrep = ryPrep;
-                hPrep = rhPrep;
+
                 xShoot = rShootX;
                 yShoot = rShootY;
                 hShoot = rShootH;
@@ -207,6 +241,23 @@ public class Auto_LT_Close extends LinearOpMode {
                 yLeave = rLeaveY;
                 hLeave = rLeaveH;
 
+                xShoot0 = rShoot0X;
+                yShoot0 = rShoot0Y;
+                hShoot0 = rShoot0H;
+                xShootGate = rShootGateX;
+                yShootGate = rShootGateY;
+                hShootGate = rShootGateH;
+                xLeaveGate = rLeaveGateX;
+                yLeaveGate = rLeaveGateY;
+                hLeaveGate = rLeaveGateH;
+
+                pickupGateAX = rPickupGateAX;
+                pickupGateAY = rPickupGateAY;
+                pickupGateAH = rPickupGateAH;
+                pickupGateBX = rPickupGateBX;
+                pickupGateBY = rPickupGateBY;
+                pickupGateBH = rPickupGateBH;
+
                 obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
@@ -238,9 +289,6 @@ public class Auto_LT_Close extends LinearOpMode {
                 y4b = by4b;
                 h4b = bh4b;
 
-                xPrep = bxPrep;
-                yPrep = byPrep;
-                hPrep = bhPrep;
                 xShoot = bShootX;
                 yShoot = bShootY;
                 hShoot = bShootH;
@@ -248,6 +296,23 @@ public class Auto_LT_Close extends LinearOpMode {
                 yLeave = bLeaveY;
                 hLeave = bLeaveH;
 
+                xShoot0 = bShoot0X;
+                yShoot0 = bShoot0Y;
+                hShoot0 = bShoot0H;
+                xShootGate = bShootGateX;
+                yShootGate = bShootGateY;
+                hShootGate = bShootGateH;
+                xLeaveGate = bLeaveGateX;
+                yLeaveGate = bLeaveGateY;
+                hLeaveGate = bLeaveGateH;
+
+                pickupGateAX = bPickupGateAX;
+                pickupGateAY = bPickupGateAY;
+                pickupGateAH = bPickupGateAH;
+                pickupGateBX = bPickupGateBX;
+                pickupGateBY = bPickupGateBY;
+                pickupGateBH = bPickupGateBH;
+
                 obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
@@ -256,12 +321,22 @@ public class Auto_LT_Close extends LinearOpMode {
             shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                     .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
 
+            if (gateCycle){
+                pickup1 = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate)))
+                        .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
+                        .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
+                                new TranslationalVelConstraint(pickupStackGateSpeed));
+            } else {
                 pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
                         .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
                         .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
                                 new TranslationalVelConstraint(pickup1Speed));
+            }
 
-            if (ballCycles < 2){
+            if (gateCycle){
+                shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
+                        .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
+            } else if (ballCycles < 2){
                 shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
                         .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
             } else {
@@ -274,12 +349,15 @@ public class Auto_LT_Close extends LinearOpMode {
                     .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
                             new TranslationalVelConstraint(pickup1Speed));
 
-            if (ballCycles < 3){
+            if (gateCycle){
+                shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
+                        .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
+            } else if (ballCycles < 3){
                 shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
                         .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
             } else {
                 shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
-                        .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hLeave));
+                        .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
             }
 
             pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
@@ -290,6 +368,27 @@ public class Auto_LT_Close extends LinearOpMode {
             shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, Math.toRadians(h4b)))
                     .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
 
+            shoot0ToPickup2 = drive.actionBuilder(new Pose2d(0,0,0))
+                    .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0))
+                    .waitSeconds(waitToPickupGate2)
+                    .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
+                    .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
+                            new TranslationalVelConstraint(pickupStackGateSpeed));
+
+            if (withPartner){
+                waitToPickupGate = waitToPickupGateWithPartner;
+            } else {
+                waitToPickupGate = waitToPickupGateSolo;
+            }
+
+            gateCyclePickup = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate)))
+                    .strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH))
+                    .waitSeconds(waitToPickupGate)
+                    .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH));
+
+            gateCycleShoot = drive.actionBuilder(new Pose2d(pickupGateBX, pickupGateBY, Math.toRadians(pickupGateBH)))
+                    .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
+
             TELE.addData("Red?", redAlliance);
             TELE.addData("Turret Default", turrDefault);
             TELE.addData("Ball Cycles", ballCycles);
@@ -303,8 +402,27 @@ public class Auto_LT_Close extends LinearOpMode {
 
         if (opModeIsActive()) {
 
+            double stamp = getRuntime();
+
             robot.transfer.setPower(1);
 
+            if (gateCycle){
+                shoot0Gate();
+                cycleStackMiddleGate();
+
+                while (getRuntime() - stamp < endGateTime){
+                    cycleGateIntake();
+                    if (getRuntime() - stamp < lastShootTime){
+                        cycleGateShoot();
+                    }
+                }
+                cycleStackCloseIntakeGate();
+
+                if (getRuntime() - stamp < lastShootTime){
+                    cycleStackCloseShootGate();
+                }
+
+            } else {
                 startAuto();
                 shoot();
 
@@ -322,6 +440,7 @@ public class Auto_LT_Close extends LinearOpMode {
                     cycleStackFar();
                     shoot();
                 }
+            }
 
             while (opModeIsActive()) {
 
@@ -359,7 +478,8 @@ public class Auto_LT_Close extends LinearOpMode {
                                 flywheel0Time,
                                 x1,
                                 y1,
-                                h1
+                                h1,
+                                false
                         )
                 )
         );
@@ -515,4 +635,131 @@ public class Auto_LT_Close extends LinearOpMode {
                 )
         );
     }
+
+    void shoot0Gate(){
+        Actions.runBlocking(
+                new ParallelAction(
+                        shoot0ToPickup2.build(),
+                        new SequentialAction(
+                                new ParallelAction(
+                                        autoActions.manageShooterManual(
+                                                waitToShoot0,
+                                                0.501,
+                                                velGate0Start,
+                                                hoodGate0Start,
+                                                velGate0Start,
+                                                hoodGate0Start,
+                                                0.501
+                                        )
+                                ),
+                                autoActions.shootAllManual(
+                                        shootAllTime,
+                                        hood0MoveTime,
+                                        spindexerSpeedIncrease,
+                                        velGate0Start,
+                                        hoodGate0Start,
+                                        velGate0End,
+                                        hoodGate0End,
+                                        0.501),
+                                autoActions.intake(
+                                        intake2TimeGate,
+                                        xShootGate,
+                                        yShootGate,
+                                        hShootGate
+                                )
+                        )
+                )
+        );
+    }
+
+    void cycleStackMiddleGate(){
+        servos.setSpinPos(spinStartPos);
+        Actions.runBlocking(
+                new ParallelAction(
+                        shoot2.build(),
+                        new SequentialAction(
+                                new ParallelAction(
+                                        autoActions.manageShooterAuto(
+                                                shoot2GateTime,
+                                                xShootGate,
+                                                yShootGate,
+                                                hShootGate,
+                                                false
+                                        )
+                                ),
+                                autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                        )
+                )
+        );
+    }
+
+    void cycleGateIntake(){
+        Actions.runBlocking(
+                new ParallelAction(
+                        gateCyclePickup.build(),
+                        autoActions.intake(
+                                intakeGateTime,
+                                xShootGate,
+                                yShootGate,
+                                hShootGate
+                        )
+                )
+        );
+    }
+
+    void cycleGateShoot(){
+        servos.setSpinPos(spinStartPos);
+        Actions.runBlocking(
+                new ParallelAction(
+                        gateCycleShoot.build(),
+                        new SequentialAction(
+                                new ParallelAction(
+                                        autoActions.manageShooterAuto(
+                                                shootGateTime,
+                                                xShootGate,
+                                                yShootGate,
+                                                hShootGate,
+                                                false
+                                        )
+                                ),
+                                autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                        )
+                )
+        );
+    }
+
+    void cycleStackCloseIntakeGate(){
+        Actions.runBlocking(
+                new ParallelAction(
+                        pickup1.build(),
+                        autoActions.intake(
+                                intake1GateTime,
+                                xShootGate,
+                                yShootGate,
+                                hShootGate
+                        )
+                )
+        );
+    }
+
+    void cycleStackCloseShootGate(){
+        servos.setSpinPos(spinStartPos);
+        Actions.runBlocking(
+                new ParallelAction(
+                        shoot1.build(),
+                        new SequentialAction(
+                                new ParallelAction(
+                                        autoActions.manageShooterAuto(
+                                                shoot1GateTime,
+                                                xLeaveGate,
+                                                yLeaveGate,
+                                                hLeaveGate,
+                                                false
+                                        )
+                                ),
+                                autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                        )
+                )
+        );
+    }
 }

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

@@ -327,7 +327,8 @@ public class Auto_LT_Far extends LinearOpMode {
                                 flywheel0Time,
                                 0.501,
                                 0.501,
-                                0.501
+                                0.501,
+                                true
                         )
 
                 )

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.autonomous.actions;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
@@ -47,7 +48,7 @@ public class AutoActions{
     private int passengerSlotGreen = 0;
     private int rearSlotGreen = 0;
     private int mostGreenSlot = 0;
-    private double firstSpindexShootPos = spindexer_outtakeBall1;
+    private double firstSpindexShootPos = spinStartPos;
     private boolean shootForward = true;
     public int motif = 0;
     double spinEndPos = ServoPositions.spinEndPos;
@@ -112,11 +113,12 @@ public class AutoActions{
             }
 
             Action manageShooter = null;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
-                    manageShooter = manageShooterAuto(time, posX, posY, posH);
+                    manageShooter = manageShooterAuto(time, posX, posY, posH, false);
                 }
                 ticker++;
                 servos.setTransferPos(transferServo_out);
@@ -215,6 +217,7 @@ public class AutoActions{
 
             int shooterTicker = 0;
             Action manageShooter = null;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 drive.updatePoseEstimate();
@@ -228,7 +231,7 @@ public class AutoActions{
 
                 if (ticker == 1) {
                     stamp = System.currentTimeMillis();
-                    manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501);
+                    manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501, false);
 
                 }
                 ticker++;
@@ -247,7 +250,6 @@ public class AutoActions{
                 if (System.currentTimeMillis() - stamp < shootTime * 1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks + 1)) {
 
                     if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) {
-                        servos.setTransferPos(transferServo_out);
                         servos.setSpinPos(firstSpindexShootPos);
                     } else {
                         servos.setTransferPos(transferServo_in);
@@ -276,6 +278,77 @@ public class AutoActions{
         };
     }
 
+    public Action shootAllManual(
+            double shootTime,
+            double hoodMoveTime, //Set to 0.501 to show that you are not using, but you must set hoodPoses equal
+            double spindexSpeed,
+            double velStart,
+            double hoodStart,
+            double velEnd,
+            double hoodEnd,
+            double turr) {
+        return new Action() {
+            int ticker = 1;
+
+            double stamp = 0.0;
+
+            int shooterTicker = 0;
+            Action manageShooter = null;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
+                spindexer.setIntakePower(-0.1);
+
+                light.setState(StateEnums.LightState.BALL_COLOR);
+                light.update();
+
+                if (ticker == 1) {
+                    stamp = System.currentTimeMillis();
+                    manageShooter = manageShooterManual(shootTime, hoodMoveTime, velStart, hoodStart, velEnd, hoodEnd, turr);
+
+                }
+                ticker++;
+
+                manageShooter.run(telemetryPacket);
+
+                double prevSpinPos = servos.getSpinCmdPos();
+
+                boolean end;
+                if (shootForward) {
+                    end = prevSpinPos > spinEndPos;
+                } else {
+                    end = prevSpinPos < spinEndPos;
+                }
+
+                if (System.currentTimeMillis() - stamp < shootTime * 1000 && !end) {
+                    servos.setTransferPos(transferServo_in);
+                    shooterTicker++;
+                    Spindexer.whileShooting = true;
+                    if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) {
+                        servos.setSpinPos(prevSpinPos + spindexSpeed);
+                    } else if (shooterTicker > Spindexer.waitFirstBallTicks) {
+                        servos.setSpinPos(prevSpinPos - spindexSpeed);
+                    }
+
+                    return true;
+
+                } else {
+                    servos.setTransferPos(transferServo_out);
+                    Spindexer.whileShooting = false;
+                    spindexer.resetSpindexer();
+                    spindexer.processIntake();
+
+                    return false;
+
+                }
+            }
+        };
+    }
+
     public Action intake(
             double time,
             double posX,
@@ -286,11 +359,12 @@ public class AutoActions{
             double stamp = 0.0;
             int ticker = 0;
             Action manageShooter = null;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
-                    manageShooter = manageShooterAuto(time, posX, posY, posH);
+                    manageShooter = manageShooterAuto(time, posX, posY, posH, false);
                 }
                 ticker++;
 
@@ -305,12 +379,18 @@ public class AutoActions{
 
                 manageShooter.run(telemetryPacket);
 
-                return ((System.currentTimeMillis() - stamp) < (time * 1000)) && !spindexer.isFull();
+                if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()) {
+                    spindexer.setIntakePower(-0.1);
+                    return false;
+                } else {
+                    return true;
+                }
             }
         };
     }
 
     private boolean detectingObelisk = false;
+
     public Action detectObelisk(
             double time,
             double posX,
@@ -373,7 +453,8 @@ public class AutoActions{
             double time,
             double posX,
             double posY,
-            double posH
+            double posH,
+            boolean flywheelSensor
     ) {
 
         return new Action() {
@@ -408,8 +489,10 @@ public class AutoActions{
                 Pose2d deltaPose;
                 if (posX != 0.501) {
                     deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
+                    Turret.limelightUsed = false;
                 } else {
-                    deltaPose = new Pose2d(robotX, robotY, robotHeading);
+                    deltaPose = new Pose2d(dx, dy, robotHeading);
+                    Turret.limelightUsed = true;
                 }
 
 //                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
@@ -428,7 +511,7 @@ public class AutoActions{
                 flywheel.manageFlywheel(targetingSettings.flywheelRPM);
 
                 boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
-                boolean shouldFinish = timeDone || flywheel.getSteady();
+                boolean shouldFinish = timeDone || (flywheel.getSteady() && flywheelSensor);
 
                 teleStart = currentPose;
 
@@ -439,6 +522,91 @@ public class AutoActions{
             }
         };
     }
+
+    public Action Wait(double time) {
+        return new Action() {
+            boolean ticker = false;
+            double stamp = 0.0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                if (!ticker) {
+                    stamp = System.currentTimeMillis();
+                    ticker = true;
+                }
+
+                return (System.currentTimeMillis() - stamp < time * 1000);
+
+            }
+        };
+    }
+
+    public Action manageShooterManual(
+            double maxTime,
+            double hoodMoveTime, //Set to 0.501 to show that you are not using, but you must set hoodPoses equal
+            double velStart,
+            double hoodStart,
+            double velEnd,
+            double hoodEnd,
+            double turr
+    ) {
+        return new Action() {
+
+            double stamp = 0.0;
+            int ticker = 0;
+
+            final boolean timeFallback = (maxTime != 0.501);
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+
+                drive.updatePoseEstimate();
+                Pose2d currentPose = drive.localizer.getPose();
+
+                if (ticker == 0) {
+                    stamp = System.currentTimeMillis();
+                }
+
+                ticker++;
+
+                double robotX = currentPose.position.x;
+                double robotY = currentPose.position.y;
+
+                double robotHeading = currentPose.heading.toDouble();
+
+                double goalX = -15;
+                double goalY = 0;
+
+                double dx = robotX - goalX;  // delta x from robot to goal
+                double dy = robotY - goalY;  // delta y from robot to goal
+                Pose2d deltaPose;
+                if (turr == 0.501) {
+                    deltaPose = new Pose2d(dx, dy, robotHeading);
+                    if (!detectingObelisk) {
+                        turret.trackGoal(deltaPose);
+                    }
+                } else {
+                    turret.setTurret(turr);
+                }
+
+                servos.setHoodPos(hoodStart + ((hoodEnd - hoodStart) * Math.min(((System.currentTimeMillis() - stamp) / (hoodMoveTime * 1000)), 1)));
+                double vel = velStart + (velEnd - velStart) * Math.min(((System.currentTimeMillis() - stamp) / (hoodMoveTime * 1000)), 1);
+
+                double voltage = robot.voltage.getVoltage();
+                flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
+                flywheel.manageFlywheel(vel);
+
+                boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > maxTime * 1000;
+
+                teleStart = currentPose;
+
+                TELE.addData("Steady?", flywheel.getSteady());
+                TELE.update();
+
+                return !timeDone;
+            }
+        };
+    }
 }
 
 

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

@@ -42,5 +42,20 @@ public class Front_Poses {
     public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55;
     public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55;
 
+    public static double rShoot0X = 40, rShoot0Y = 0.1, rShoot0H = 0.1;
+    public static double bShoot0X = 40, bShoot0Y = -0.1, bShoot0H = -0.1;
+
+    public static double rShootGateX = 50, rShootGateY = 10, rShootGateH = 90;
+    public static double bShootGateX = 40, bShootGateY = 1, bShootGateH = -90;
+
+    public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55;
+    public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55;
+
+    public static double rPickupGateAX = 24, rPickupGateAY = 50, rPickupGateAH = 140;
+    public static double bPickupGateAX = 36, bPickupGateAY = -50, bPickupGateAH = -140;
+
+    public static double rPickupGateBX = 38, rPickupGateBY = 68, rPickupGateBH = 180;
+    public static double bPickupGateBX = 46, bPickupGateBY = -60, bPickupGateBH = -180;
+
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
 }

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

@@ -5,19 +5,19 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos1 = 0.06; //0.13;
+    public static double spindexer_intakePos1 = 0.13; //0.13;
 
-    public static double spindexer_intakePos2 = 0.25; //0.33;//0.5;
+    public static double spindexer_intakePos2 = 0.32; //0.33;//0.5;
 
-    public static double spindexer_intakePos3 = 0.43; //0.53;//0.66;
+    public static double spindexer_intakePos3 = 0.5; //0.53;//0.66;
 
-    public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24;
-    public static double spindexer_outtakeBall3b = 0.15; //0.65; //0.24;
+    public static double spindexer_outtakeBall3 = 0.78; //0.65; //0.24;
+    public static double spindexer_outtakeBall3b = 0.22; //0.65; //0.24;
 
-    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.3;
-    public static double spinEndPos = 0.85;
+    public static double spindexer_outtakeBall2 = 0.7; //0.46; //0.6;
+    public static double spindexer_outtakeBall1 = 0.42; //0.27; //0.4;
+    public static double spinStartPos = 0.05;
+    public static double spinEndPos = 0.95;
 
     public static double shootAllSpindexerSpeedIncrease = 0.014;
 
@@ -27,7 +27,7 @@ public class ServoPositions {
 
     public static double hoodAuto = 0.27;
 
-    public static double hoodOffset = -0.05;
+    public static double hoodOffset = -0.04; // offset from 0.93
 
     public static double turret_redClose = 0;
     public static double turret_blueClose = 0;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java

@@ -201,18 +201,18 @@ public final class MecanumDrive {
         public double kA = 0.00008;
 
         // path profile parameters (in inches)
-        public double maxWheelVel = 90;
+        public double maxWheelVel = 70;
         public double minProfileAccel = -40;
-        public double maxProfileAccel = 90;
+        public double maxProfileAccel = 70;
 
         // turn profile parameters (in radians)
-        public double maxAngVel = Math.PI; // shared with path
-        public double maxAngAccel = Math.PI;
+        public double maxAngVel = 2 *Math.PI; // shared with path
+        public double maxAngAccel = 2 * Math.PI;
 
         // path controller gains
-        public double axialGain = 4;
-        public double lateralGain = 4;
-        public double headingGain = 4; // shared with turn
+        public double axialGain = 6;
+        public double lateralGain = 6;
+        public double headingGain = 6; // shared with turn
 
         public double axialVelGain = 0.0;
         public double lateralVelGain = 0.0;
@@ -345,7 +345,16 @@ public final class MecanumDrive {
                 t = Actions.now() - beginTs;
             }
 
-            if (t >= timeTrajectory.duration) {
+            Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
+
+            PoseVelocity2d robotVelRobot = updatePoseEstimate();
+
+            Pose2d error = txWorldTarget.value().minusExp(localizer.getPose());
+
+            if ((t >= timeTrajectory.duration && error.position.norm() < 2
+                    && robotVelRobot.linearVel.norm() < 0.5)
+                    || t >= timeTrajectory.duration + 0.5) {
                 leftFront.setPower(0);
                 leftBack.setPower(0);
                 rightBack.setPower(0);
@@ -354,10 +363,6 @@ public final class MecanumDrive {
                 return false;
             }
 
-            Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
-            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
-
-            PoseVelocity2d robotVelRobot = updatePoseEstimate();
 
             PoseVelocity2dDual<Time> command = new HolonomicController(
                     PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
@@ -388,7 +393,6 @@ public final class MecanumDrive {
             p.put("y", localizer.getPose().position.y);
             p.put("heading (deg)", Math.toDegrees(localizer.getPose().heading.toDouble()));
 
-            Pose2d error = txWorldTarget.value().minusExp(localizer.getPose());
             p.put("xError", error.position.x);
             p.put("yError", error.position.y);
             p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));

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

@@ -65,6 +65,7 @@ public class TeleopV3 extends LinearOpMode {
     boolean reject = false;
     double xOffset = 0.0;
     double yOffset = 0.0;
+    double hOffset = 0.0;
 //    double headingOffset = 0.0;
     int ticker = 0;
 
@@ -75,6 +76,7 @@ public class TeleopV3 extends LinearOpMode {
 
     int intakeTicker = 0;
     private boolean shootAll = false;
+    boolean relocalize = false;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -111,7 +113,6 @@ public class TeleopV3 extends LinearOpMode {
         light.setState(StateEnums.LightState.MANUAL);
         limelightUsed = true;
 
-        robot.light.setPosition(1);
         while (opModeInInit()) {
             robot.limelight.start();
             if (redAlliance) {
@@ -122,6 +123,7 @@ public class TeleopV3 extends LinearOpMode {
                 light.setManualLightColor(Color.LightBlue);
 
             }
+            robot.light.setPosition(1);
 
             light.update();
         }
@@ -165,10 +167,11 @@ public class TeleopV3 extends LinearOpMode {
 
             double robX = drive.localizer.getPose().position.x;
             double robY = drive.localizer.getPose().position.y;
+            double robH = drive.localizer.getPose().heading.toDouble();
 
-            double robotX = robX - xOffset;
-            double robotY = robY - yOffset;
-            double robotHeading = drive.localizer.getPose().heading.toDouble();
+            double robotX = robX + xOffset;
+            double robotY = robY + yOffset;
+            double robotHeading = robH + hOffset;
 
             double goalX = -15;
             double goalY = 0;
@@ -182,7 +185,21 @@ public class TeleopV3 extends LinearOpMode {
             targetingSettings = targeting.calculateSettings
                     (robotX, robotY, robotHeading, 0.0, turretInterpolate);
 
+            //RELOCALIZATION
+
+            if (gamepad2.squareWasPressed()){
+                relocalize = !relocalize;
+                gamepad2.rumble(500);
+            }
+
+            if (relocalize){
+                turret.relocalize();
+                xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
+                yOffset = (turret.getLimelightX() * 39.3701) - robY;
+                hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
+            } else {
                 turret.trackGoal(deltaPose);
+            }
 
             //VELOCITY AUTOMATIC
             if (autoVel) {
@@ -339,18 +356,22 @@ public class TeleopV3 extends LinearOpMode {
             // Targeting Debug
             TELE.addData("robotX", robotX);
             TELE.addData("robotY", robotY);
-            TELE.addData("robotInchesX", targeting.robotInchesX);
-            TELE.addData("robotInchesY", targeting.robotInchesY);
-            TELE.addData("Targeting Interpolate", turretInterpolate);
+            TELE.addData("robot H", robotHeading);
+//            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("timeSinceStamp", getRuntime() - shootStamp);
-            TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub)
+//            TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
+//            TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
+//            TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
+//            TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub)
             TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
             TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
             TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
+            TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
+            TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
+            TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
 
             TELE.update();
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.tests;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
@@ -128,9 +129,9 @@ public class ShooterTest extends LinearOpMode {
 
             if (hoodPos != 0.501) {
                 if (enableHoodAutoOpen) {
-                    robot.hood.setPosition(hoodPos+(hoodAdjustFactor*(flywheel.getVelo()/Velocity)));
+                    robot.hood.setPosition(hoodPos+(hoodAdjustFactor*(flywheel.getVelo()/Velocity)) + hoodOffset);
                 } else {
-                    robot.hood.setPosition(hoodPos);
+                    robot.hood.setPosition(hoodPos + hoodOffset);
                 }
             }
 

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

@@ -187,7 +187,7 @@ public class Spindexer {
         distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
 
         // Position 1
-        if (distanceRearCenter < 60) {
+        if (distanceRearCenter < 65) {
 
             // Mark Ball Found
             newPos1Detection = true;

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

@@ -82,13 +82,19 @@ public class Targeting {
     public Targeting() {
     }
 
+    double cos54 = Math.cos(Math.toRadians(-54));
+    double sin54 = Math.sin(Math.toRadians(-54));
+
     public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) {
         Settings recommendedSettings = new Settings(0.0, 0.0);
-
-        double cos45 = Math.cos(Math.toRadians(-45));
-        double sin45 = Math.sin(Math.toRadians(-45));
-        double rotatedY = (robotX + cancelOffsetX) * sin45 + (robotY + cancelOffsetY) * cos45;
-        double rotatedX = (robotX + cancelOffsetX) * cos45 - (robotY + cancelOffsetY) * sin45;
+        if (!redAlliance){
+            sin54 = Math.sin(Math.toRadians(54));
+        } else {
+            sin54 = Math.sin(Math.toRadians(-54));
+        }
+        // TODO: test these values determined from the fmap
+        double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
+        double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
 
         // Convert robot coordinates to inches
         robotInchesX = rotatedX * unitConversionFactor;
@@ -99,7 +105,17 @@ public class Targeting {
         int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
 
         int remX = Math.floorMod((int) robotInchesX, tileSize);
-        int remY = Math.floorMod((int) robotInchesX, tileSize);
+        int remY = Math.floorMod((int) robotInchesY, tileSize);
+
+        //clamp
+
+        //if (redAlliance) {
+            robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
+            robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
+        //} else {
+//            robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
+//            robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
+        //}
 
         // Determine if we need to interpolate based on tile position.
         // if near upper or lower quarter or tile interpolate with next tile.
@@ -172,16 +188,6 @@ public class Targeting {
             interpolate = false;
         }
 
-        //clamp
-
-        if (redAlliance) {
-            robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
-            robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
-        } else {
-            robotGridY = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
-            robotGridX = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
-        }
-
         // basic search
         if (true) { //!interpolate) {
             if ((robotGridY < 6) && (robotGridX < 6)) {

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

@@ -22,12 +22,12 @@ public class Turret {
 
     public static double turretTolerance = 0.02;
     public static double turrPosScalar = 0.00011264432;
-    public static double turret180Range = 0.4;
-    public static double turrDefault = 0.37;
-    public static double turrMin = 0.15;
-    public static double turrMax = 0.8;
+    public static double turret180Range = 0.54;
+    public static double turrDefault = 0.35;
+    public static double turrMin = 0;
+    public static double turrMax = 1;
     public static boolean limelightUsed = true;
-
+    public static double limelightPosOffset = 5;
     public static double manualOffset = 0.0;
 
 //    public static double visionCorrectionGain = 0.08;  // Single tunable gain
@@ -99,8 +99,12 @@ public class Turret {
         return Math.abs(pos - this.getTurrPos()) < turretTolerance;
     }
 
+    public static double alphaPosConstant = 0.3;
     private void limelightRead() { // only for tracking purposes, not general reads
-
+        Double xPos = null;
+        Double yPos = null;
+        Double zPos = null;
+        Double hPos = null;
         result = webcam.getLatestResult();
         if (result != null) {
             if (result.isValid()) {
@@ -112,9 +116,28 @@ public class Turret {
                     limelightPosX = botpose.getPosition().x;
                     limelightPosY = botpose.getPosition().y;
                 }
+                List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+                for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                    limelightTagPose = fiducial.getRobotPoseTargetSpace();
+                    if (limelightTagPose != null){
+                        xPos = limelightTagPose.getPosition().x;
+                        yPos = limelightTagPose.getPosition().y;
+                        zPos = limelightTagPose.getPosition().z;
+                        hPos = limelightTagPose.getOrientation().getYaw();
+                    }
+                }
 
             }
         }
+        if (xPos != null ){
+            if (zPos>0) {
+
+                limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
+                limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
+                limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ);
+                limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH);
+            }
+        }
     }
 
     public double getBearing() {
@@ -127,12 +150,21 @@ public class Turret {
         return ty;
     }
 
+    Pose3D limelightTagPose;
+    double limelightTagX = 0.0;
+    double limelightTagY = 0.0;
+    double limelightTagZ = 0.0;
+    double limelightTagH = 0.0;
     public double getLimelightX() {
-        return limelightPosX;
+        return limelightTagX;
     }
+    public double getLimelightY() {return limelightTagY;}
+    public double getLimelightZ(){return limelightTagZ;}
+    public double getLimelightH(){return limelightTagH;}
 
-    public double getLimelightY() {
-        return limelightPosY;
+    public void relocalize(){
+        setTurret(turrDefault);
+        limelightRead();
     }
 
     public int detectObelisk() {