put in poses for blue

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

@@ -167,7 +167,7 @@ public class Auto_LT_Close extends LinearOpMode {
 
         while (opModeInInit()) {
 
-            if (gateCycle){
+            if (gateCycle) {
                 servos.setHoodPos(hoodGate0Start);
             } else {
                 servos.setHoodPos(shoot0Hood);
@@ -187,17 +187,17 @@ public class Auto_LT_Close extends LinearOpMode {
                 turrDefault += 0.01;
             }
 
-            if (gamepad2.rightBumperWasPressed()){
+            if (gamepad2.rightBumperWasPressed()) {
                 ballCycles++;
             }
-            if (gamepad2.leftBumperWasPressed()){
+            if (gamepad2.leftBumperWasPressed()) {
                 ballCycles--;
             }
 
-            if (gamepad2.squareWasPressed()){
-                if (!gateCycle){
+            if (gamepad2.squareWasPressed()) {
+                if (!gateCycle) {
                     turret.pipelineSwitch(1);
-                } else if (redAlliance){
+                } else if (redAlliance) {
                     turret.pipelineSwitch(4);
                 } else {
                     turret.pipelineSwitch(2);
@@ -318,10 +318,15 @@ public class Auto_LT_Close extends LinearOpMode {
                 obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
             }
 
-            shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
-                    .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
+            if (gateCycle) {
+                shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                        .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
+            } else {
+                shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                        .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0));
+            }
 
-            if (gateCycle){
+            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),
@@ -333,10 +338,10 @@ public class Auto_LT_Close extends LinearOpMode {
                                 new TranslationalVelConstraint(pickup1Speed));
             }
 
-            if (gateCycle){
+            if (gateCycle) {
                 shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
                         .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
-            } else if (ballCycles < 2){
+            } else if (ballCycles < 2) {
                 shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
                         .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
             } else {
@@ -344,15 +349,22 @@ public class Auto_LT_Close extends LinearOpMode {
                         .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
             }
 
-            pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
-                    .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
-                    .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
-                            new TranslationalVelConstraint(pickup1Speed));
+            if (gateCycle) {
+                pickup2 = drive.actionBuilder(new Pose2d(xShoot0, yShoot0, Math.toRadians(hShoot0)))
+                        .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
+                        .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
+                                new TranslationalVelConstraint(pickupStackGateSpeed));
+            } else {
+                pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
+                        .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
+                        .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
+                                new TranslationalVelConstraint(pickup1Speed));
+            }
 
-            if (gateCycle){
+            if (gateCycle) {
                 shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
                         .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
-            } else if (ballCycles < 3){
+            } else if (ballCycles < 3) {
                 shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
                         .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
             } else {
@@ -368,14 +380,7 @@ 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){
+            if (withPartner) {
                 waitToPickupGate = waitToPickupGateWithPartner;
             } else {
                 waitToPickupGate = waitToPickupGateSolo;
@@ -406,19 +411,21 @@ public class Auto_LT_Close extends LinearOpMode {
 
             robot.transfer.setPower(1);
 
-            if (gateCycle){
-                shoot0Gate();
+            if (gateCycle) {
+                startAutoGate();
+                shoot();
                 cycleStackMiddleGate();
+                shoot();
 
-                while (getRuntime() - stamp < endGateTime){
+                while (getRuntime() - stamp < endGateTime) {
                     cycleGateIntake();
-                    if (getRuntime() - stamp < lastShootTime){
+                    if (getRuntime() - stamp < lastShootTime) {
                         cycleGateShoot();
                     }
                 }
                 cycleStackCloseIntakeGate();
 
-                if (getRuntime() - stamp < lastShootTime){
+                if (getRuntime() - stamp < lastShootTime) {
                     cycleStackCloseShootGate();
                 }
 
@@ -426,17 +433,17 @@ public class Auto_LT_Close extends LinearOpMode {
                 startAuto();
                 shoot();
 
-                if (ballCycles > 0){
+                if (ballCycles > 0) {
                     cycleStackClose();
                     shoot();
                 }
 
-                if (ballCycles > 1){
+                if (ballCycles > 1) {
                     cycleStackMiddle();
                     shoot();
                 }
 
-                if (ballCycles > 2){
+                if (ballCycles > 2) {
                     cycleStackFar();
                     shoot();
                 }
@@ -459,13 +466,8 @@ public class Auto_LT_Close extends LinearOpMode {
 
     }
 
-    void shoot(){
-        Actions.runBlocking(
-                new ParallelAction(
-                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
-                )
-
-        );
+    void shoot() {
+        Actions.runBlocking(autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease));
     }
 
     void startAuto() {
@@ -485,7 +487,24 @@ public class Auto_LT_Close extends LinearOpMode {
         );
     }
 
-    void cycleStackClose(){
+    void startAutoGate() {
+        assert shoot0 != null;
+
+        Actions.runBlocking(
+                new ParallelAction(
+                        shoot0.build(),
+                        autoActions.manageShooterAuto(
+                                flywheel0Time,
+                                xShoot0,
+                                yShoot0,
+                                hShoot0,
+                                false
+                        )
+                )
+        );
+    }
+
+    void cycleStackClose() {
         Actions.runBlocking(
                 new ParallelAction(
                         pickup1.build(),
@@ -515,7 +534,7 @@ public class Auto_LT_Close extends LinearOpMode {
         double posX;
         double posY;
         double posH;
-        if (ballCycles > 1){
+        if (ballCycles > 1) {
             posX = xShoot;
             posY = yShoot;
             posH = hShoot;
@@ -540,7 +559,7 @@ public class Auto_LT_Close extends LinearOpMode {
         );
     }
 
-    void cycleStackMiddle(){
+    void cycleStackMiddle() {
         Actions.runBlocking(
                 new ParallelAction(
                         pickup2.build(),
@@ -570,7 +589,7 @@ public class Auto_LT_Close extends LinearOpMode {
         double posX;
         double posY;
         double posH;
-        if (ballCycles > 2){
+        if (ballCycles > 2) {
             posX = xShoot;
             posY = yShoot;
             posH = hShoot;
@@ -594,7 +613,7 @@ public class Auto_LT_Close extends LinearOpMode {
         );
     }
 
-    void cycleStackFar(){
+    void cycleStackFar() {
         Actions.runBlocking(
                 new ParallelAction(
                         pickup3.build(),
@@ -636,64 +655,35 @@ public class Auto_LT_Close extends LinearOpMode {
         );
     }
 
-    void shoot0Gate(){
+    void cycleStackMiddleGate() {
         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
-                                )
+                        pickup2.build(),
+                        autoActions.intake(
+                                intake2Time,
+                                x3b,
+                                y3b,
+                                h3b
                         )
                 )
         );
-    }
 
-    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)
-                        )
+                        autoActions.prepareShootAll(
+                                colorSenseTime,
+                                shoot2Time,
+                                motif,
+                                xShootGate,
+                                yShootGate,
+                                hShootGate)
                 )
         );
     }
 
-    void cycleGateIntake(){
+    void cycleGateIntake() {
         Actions.runBlocking(
                 new ParallelAction(
                         gateCyclePickup.build(),
@@ -707,28 +697,23 @@ public class Auto_LT_Close extends LinearOpMode {
         );
     }
 
-    void cycleGateShoot(){
+    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)
+                        autoActions.manageShooterAuto(
+                                shootGateTime,
+                                xShootGate,
+                                yShootGate,
+                                hShootGate,
+                                false
                         )
                 )
         );
     }
 
-    void cycleStackCloseIntakeGate(){
+    void cycleStackCloseIntakeGate() {
         Actions.runBlocking(
                 new ParallelAction(
                         pickup1.build(),
@@ -747,17 +732,12 @@ public class Auto_LT_Close extends LinearOpMode {
         Actions.runBlocking(
                 new ParallelAction(
                         shoot1.build(),
-                        new SequentialAction(
-                                new ParallelAction(
-                                        autoActions.manageShooterAuto(
-                                                shoot1GateTime,
-                                                xLeaveGate,
-                                                yLeaveGate,
-                                                hLeaveGate,
-                                                false
-                                        )
-                                ),
-                                autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                        autoActions.manageShooterAuto(
+                                shoot1GateTime,
+                                xLeaveGate,
+                                yLeaveGate,
+                                hLeaveGate,
+                                false
                         )
                 )
         );

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

@@ -14,10 +14,10 @@ public class Back_Poses {
     public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
 
     public static double rStackPickupBX = 55, rStackPickupBY = 73, rStackPickupBH = 140.1;
-    public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
+    public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1;
 
     public static double rPickupGateX = 70, rPickupGateY = 90, rPickupGateH = 140;
-    public static double bPickupGateX = 60, bPickupGateY = -88, bPickupGateH = -140;
+    public static double bPickupGateX = 70, bPickupGateY = -90, bPickupGateH = -140;
 
     public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;
     public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;

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

@@ -8,33 +8,33 @@ public class Front_Poses {
 
 
     public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
-    public static double bx1 = 20, by1 = 0.5, bh1 = 0.1;
+    public static double bx1 = 20, by1 = -0.5, bh1 = -0.1;
 
     public static double rx2a = 41, ry2a = 18, rh2a = 140;
     public static double bx2a = 41, by2a = -18, bh2a = -140;
 
     public static double rx2b = 23, ry2b = 36, rh2b = 140.1;
-    public static double bx2b = 19, by2b = -40, bh2b = -140.1;
+    public static double bx2b = 23, by2b = -36, bh2b = -140.1;
 
     public static double rx3a = 55, ry3a = 39, rh3a = 140;
     public static double bx3a = 55, by3a = -39, bh3a = -140;
 
     public static double rx3aG = 60, ry3aG = 34, rh3aG = 140;
-    public static double bx3aG = 55, by3aG = -43, bh3aG = -140;
+    public static double bx3aG = 60, by3aG = -34, bh3aG = -140;
 
     public static double rx3b = 36, ry3b = 58, rh3b = 140.1;
-    public static double bx3b = 41, by3b = -59, bh3b = -140.1;
+    public static double bx3b = 36, by3b = -58, bh3b = -140.1;
 
     public static double rx4a = 75, ry4a = 53, rh4a = 140;
     public static double bx4a = 75, by4a = -53, bh4a = -140;
 
     public static double rx4b = 50, ry4b = 78, rh4b = 140.1;
-    public static double bx4b = 47, by4b = -85, bh4b = -140.1;
+    public static double bx4b = 50, by4b = -78, bh4b = -140.1;
 
     public static double rfx1 = 10, rfy1 = 0, rfh1 = 0;
 
     public static double rShootX = 40, rShootY = 10, rShootH = 50;
-    public static double bShootX = 40, bShootY = 0, bShootH = -50;
+    public static double bShootX = 40, bShootY = -10, bShootH = -50;
 
     public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
     public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
@@ -46,16 +46,16 @@ public class Front_Poses {
     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 bShootGateX = 50, bShootGateY = -10, 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 bPickupGateAX = 24, bPickupGateAY = -50, bPickupGateAH = -140;
 
     public static double rPickupGateBX = 38, rPickupGateBY = 68, rPickupGateBH = 180;
-    public static double bPickupGateBX = 46, bPickupGateBY = -60, bPickupGateBH = -180;
+    public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180;
 
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
 }

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

@@ -129,9 +129,8 @@ public class Turret {
 
             }
         }
-        if (xPos != null ){
+        if (xPos != null){
             if (zPos>0) {
-
                 limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
                 limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
                 limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ);