auton progress with errors

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

@@ -1,67 +1,7 @@
 package org.firstinspires.ftc.teamcode.autonomous;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveX;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveY;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveH;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveX;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveY;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep;
-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.spinEndPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
@@ -849,6 +789,7 @@ public class Auto_LT_Close extends LinearOpMode {
             if (gamepad2.squareWasPressed()){
                 robot.limelight.start();
                 robot.limelight.pipelineSwitch(1);
+                gamepad2.rumble(500);
             }
 
             if (redAlliance) {
@@ -1052,8 +993,8 @@ public class Auto_LT_Close extends LinearOpMode {
                                 shootAllVelocity,
                                 shootAllHood,
                                 intake1Time,
-                                x2b,
+                                0.501,
-                                y2b,
+                                0.501,
                                 pickup1XTolerance,
                                 pickup1YTolerance
                         ),
@@ -1113,8 +1054,8 @@ public class Auto_LT_Close extends LinearOpMode {
                         pickup2.build(),
                         manageShooterAuto(
                                 intake2Time,
-                                x2b,
+                                0.501,
-                                y2b,
+                                0.501,
                                 pickup1XTolerance,
                                 pickup1YTolerance
                         ),
@@ -1171,8 +1112,8 @@ public class Auto_LT_Close extends LinearOpMode {
                         pickup3.build(),
                         manageShooterAuto(
                                 intake3Time,
-                                x2b,
+                                0.501,
-                                y2b,
+                                0.501,
                                 pickup1XTolerance,
                                 pickup1YTolerance
                         ),

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

@@ -1,13 +1,7 @@
 package org.firstinspires.ftc.teamcode.autonomous;
 
-import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveH;
+import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
-import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveX;
-import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveY;
-import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveH;
-import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveX;
-import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveY;
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleEnd;
 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.spinEndPos;
@@ -31,6 +25,7 @@ import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.ParallelAction;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.TranslationalVelConstraint;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -81,6 +76,12 @@ public class Auto_LT_Far extends LinearOpMode {
     public static double firstShootTime = 0.3;
     public static double flywheel0Time = 3.5;
     public static double shoot0Time = 2;
+    boolean gatePickup = false;
+    boolean stack3 = true;
+    double xStackPickupA, yStackPickupA, hStackPickupA;
+    double xStackPickupB, yStackPickupB, hStackPickupB;
+    public static int pickupStackSpeed = 15;
+    int prevMotif = 0;
 
     public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
         return new Action() {
@@ -667,6 +668,9 @@ public class Auto_LT_Far extends LinearOpMode {
 
     // initialize path variables here
     TrajectoryActionBuilder leave3Ball = null;
+    TrajectoryActionBuilder leaveFromShoot = null;
+    TrajectoryActionBuilder pickup3 = null;
+    TrajectoryActionBuilder shoot3 = null;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -694,23 +698,34 @@ public class Auto_LT_Far extends LinearOpMode {
 
         turret.setTurret(turrDefault);
 
-        drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
+        drive = new MecanumDrive(hardwareMap, autoStart);
 
         servos.setSpinPos(autoSpinStartPos);
 
         servos.setTransferPos(transferServo_out);
 
-        robot.light.setPosition(1);
-
         while (opModeInInit()) {
 
             // Recalibration in initialization
             drive.updatePoseEstimate();
-            if (gamepad2.square) {
+            if (gamepad2.triangle) {
-                teleEnd = drive.localizer.getPose(); // use this position as starting position
+                autoStart = drive.localizer.getPose(); // use this position as starting position
                 gamepad2.rumble(1000);
             }
 
+            if (gamepad2.squareWasPressed()){
+                robot.limelight.start();
+                robot.limelight.pipelineSwitch(1);
+                gamepad2.rumble(500);
+            }
+
+            if (gamepad2.leftBumperWasPressed()){
+                gatePickup = !gatePickup;
+            }
+            if (gamepad2.rightBumperWasPressed()){
+                stack3 = !stack3;
+            }
+
             turret.setTurret(turretShootPos);
 
             robot.hood.setPosition(shoot0Hood);
@@ -734,6 +749,18 @@ public class Auto_LT_Far extends LinearOpMode {
                 yLeave = rLeaveY;
                 hLeave = rLeaveH;
 
+                xShoot = rShootX;
+                yShoot = rShootY;
+                hShoot = rShootH;
+
+                xStackPickupA = rStackPickupAX;
+                yStackPickupA = rStackPickupAY;
+                hStackPickupA = rStackPickupAH;
+
+                xStackPickupB = rStackPickupBX;
+                yStackPickupB = rStackPickupBY;
+                hStackPickupB = rStackPickupBH;
+
                 turretShootPos = turrDefault + redTurretShootPos;
             } else {
                 robot.light.setPosition(0.6);
@@ -742,15 +769,40 @@ public class Auto_LT_Far extends LinearOpMode {
                 yLeave = bLeaveY;
                 hLeave = bLeaveH;
 
+                xShoot = bShootX;
+                yShoot = bShootY;
+                hShoot = bShootH;
+
+                xStackPickupA = bStackPickupAX;
+                yStackPickupA = bStackPickupAY;
+                hStackPickupA = bStackPickupAH;
+
+                xStackPickupB = bStackPickupBX;
+                yStackPickupB = bStackPickupBY;
+                hStackPickupB = bStackPickupBH;
+
                 turretShootPos = turrDefault + blueTurretShootPos;
             }
 
-            leave3Ball = drive.actionBuilder(teleEnd)
+            leave3Ball = drive.actionBuilder(autoStart)
                     .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
 
+            leaveFromShoot = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
+                    .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
+
+            pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
+                    .strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA))
+                    .strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB),
+                            new TranslationalVelConstraint(pickupStackSpeed));
+
+            shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB)))
+                    .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
+
             TELE.addData("Red?", redAlliance);
             TELE.addData("Turret Default", turrDefault);
-            TELE.addData("Start Position", teleEnd);
+            TELE.addData("Gate Cycle?", gatePickup);
+            TELE.addData("Pickup Stack?", stack3);
+            TELE.addData("Start Position", autoStart);
             TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift
             TELE.update();
         }
@@ -762,11 +814,21 @@ public class Auto_LT_Far extends LinearOpMode {
         // Currently only shoots; keep this start and modify times and then add extra paths
         if (opModeIsActive()) {
 
+            double stamp = getRuntime();
+
             robot.transfer.setPower(1);
 
             startAuto();
 
+            if (stack3){
+                cycleStackFar();
+            }
+
+            if (gatePickup || stack3){
+                leave();
+            } else {
                 leave3Ball();
+            }
 
             // Actual way to end autonomous in to find final position
             while (opModeIsActive()) {
@@ -810,4 +872,67 @@ public class Auto_LT_Far extends LinearOpMode {
         assert leave3Ball != null;
         Actions.runBlocking(leave3Ball.build());
     }
+
+    void leave(){
+        assert leaveFromShoot != null;
+        Actions.runBlocking(leaveFromShoot.build());
+    }
+
+    void cycleStackFar(){
+        Actions.runBlocking(
+                new ParallelAction(
+                        pickup3.build(),
+                        manageShooterAuto(
+                                intake3Time,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501
+                        ),
+                        intake(intake3Time),
+                        detectObelisk(
+                                intake3Time,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501,
+                                obeliskTurrPos3
+                        )
+
+                )
+        );
+
+        motif = turret.getObeliskID();
+
+        if (motif == 0) motif = prevMotif;
+        prevMotif = motif;
+
+        Actions.runBlocking(
+                new ParallelAction(
+                        manageFlywheelAuto(
+                                shoot3Time,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501
+                        ),
+                        shoot3.build(),
+                        prepareShootAll(colorSenseTime, shoot3Time, motif)
+                )
+        );
+
+        Actions.runBlocking(
+                new ParallelAction(
+                        manageShooterAuto(
+                                finalShootAllTime,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501
+                        ),
+                        shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
+                )
+
+        );
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.autonomous;
+package org.firstinspires.ftc.teamcode.autonomous.disabled;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.autonomous;
+package org.firstinspires.ftc.teamcode.autonomous.disabled;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH;

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

@@ -1,10 +1,23 @@
 package org.firstinspires.ftc.teamcode.constants;
 
 import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.Pose2d;
 
 @Config
 public class Back_Poses {
-    public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50;
+    public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50.1;
     public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50;
 
+    public static double rShootX = 95, rShootY = 85, rShootH = 90;
+    public static double bShootX = 95, bShootY = -85, bShootH = -90;
+
+    public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
+    public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
+
+    public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1;
+    public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
+
+    public static Pose2d autoStart = new Pose2d(0, 0, 0); // TODO: find this position
+
+
 }

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

@@ -43,5 +43,4 @@ public class Front_Poses {
     public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
 
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
-    public static Pose2d teleEnd = new Pose2d(0, 0, 0);
 }