far auto in development

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

@@ -56,7 +56,7 @@ public class Auto_LT_Close extends LinearOpMode {
 
     public static double intake3Time = 4.2;
 
-    public static double flywheel0Time = 3.5;
+    public static double flywheel0Time = 1.5;
     public static double pickup1Speed = 12;
     // ---- POSITION TOLERANCES ----
     public static double posXTolerance = 5.0;

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

@@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
 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.spinEndPos;
+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;
@@ -12,6 +13,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
 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.Targeting.turretInterpolate;
+import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
 
 import androidx.annotation.NonNull;
@@ -45,14 +47,9 @@ import java.util.Objects;
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Far extends LinearOpMode {
     public static double shoot0Vel = 3300, shoot0Hood = 0.48;
-    public static double autoSpinStartPos = 0.2;
-    public static double shoot0SpinSpeedIncrease = 0.015;
-    public static double shoot0XTolerance = 1.0;
     public static double redTurretShootPos = 0.05;
     public static double blueTurretShootPos = -0.05;
-    public static int fwdTime = 200, strafeTime = 2300;
     double xLeave, yLeave, hLeave;
-    public static int sleepTime = 1300;
     public int motif = 0;
     double turretShootPos = 0.0;
     Robot robot;
@@ -65,18 +62,10 @@ public class Auto_LT_Far extends LinearOpMode {
     Targeting targeting;
     Targeting.Settings targetingSettings;
     AutoActions autoActions;
-    double firstSpindexShootPos = autoSpinStartPos;
-    boolean shootForward = true;
     double xShoot, yShoot, hShoot;
-    private int driverSlotGreen = 0;
-    private int passengerSlotGreen = 0;
-    int rearSlotGreen = 0;
-    int mostGreenSlot = 0;
     double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
     double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
-    public static double firstShootTime = 0.3;
+    public static double flywheel0Time = 1.5;
-    public static double flywheel0Time = 3.5;
-    public static double shoot0Time = 2;
     boolean gatePickup = false;
     boolean stack3 = true;
     double xStackPickupA, yStackPickupA, hStackPickupA;
@@ -84,13 +73,31 @@ public class Auto_LT_Far extends LinearOpMode {
     public static int pickupStackSpeed = 12;
     int prevMotif = 0;
     public static double spindexerSpeedIncrease = 0.005;
-
+    public static double shootAllTime = 2;
+    // ---- POSITION TOLERANCES ----
+    public static double posXTolerance = 5.0;
+    public static double posYTolerance = 5.0;
+    public static double shootStackTime = 2;
+    public static double shootGateTime = 2;
+    public static double colorSenseTime = 1;
+    public static double intakeStackTime = 3.3;
+    public static double intakeGateTime = 3;
+    public static double redObeliskTurrPos1 = 0.12;
+    public static double redObeliskTurrPos2 = 0.13;
+    public static double redObeliskTurrPos3 = 0.14;
+    public static double blueObeliskTurrPos1 = -0.12;
+    public static double blueObeliskTurrPos2 = -0.13;
+    public static double blueObeliskTurrPos3 = -0.14;
+    double obeliskTurrPos1 = 0.0;
+    double obeliskTurrPos2 = 0.0;
+    double obeliskTurrPos3 = 0.0;
 
     // initialize path variables here
     TrajectoryActionBuilder leave3Ball = null;
     TrajectoryActionBuilder leaveFromShoot = null;
     TrajectoryActionBuilder pickup3 = null;
     TrajectoryActionBuilder shoot3 = null;
+    Pose2d autoStart = new Pose2d(0,0,0);
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -118,23 +125,12 @@ public class Auto_LT_Far extends LinearOpMode {
 
         turret.setTurret(turrDefault);
 
-        drive = new MecanumDrive(hardwareMap, autoStart);
+        servos.setSpinPos(spinStartPos);
-
-        autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
-
-        servos.setSpinPos(autoSpinStartPos);
 
         servos.setTransferPos(transferServo_out);
 
         while (opModeInInit()) {
 
-            // Recalibration in initialization
-            drive.updatePoseEstimate();
-            if (gamepad2.triangle) {
-                autoStart = drive.localizer.getPose(); // use this position as starting position
-                gamepad2.rumble(1000);
-            }
-
             if (gamepad2.squareWasPressed()){
                 robot.limelight.start();
                 robot.limelight.pipelineSwitch(1);
@@ -167,6 +163,10 @@ public class Auto_LT_Far extends LinearOpMode {
             if (redAlliance) {
                 robot.light.setPosition(0.28);
 
+                autoStart = new Pose2d(autoStartRX, autoStartRY, Math.toRadians(autoStartRH));
+                drive = new MecanumDrive(hardwareMap, autoStart);
+                autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
+
                 xLeave = rLeaveX;
                 yLeave = rLeaveY;
                 hLeave = rLeaveH;
@@ -183,10 +183,17 @@ public class Auto_LT_Far extends LinearOpMode {
                 yStackPickupB = rStackPickupBY;
                 hStackPickupB = rStackPickupBH;
 
+                obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
+                obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
+                obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
                 turretShootPos = turrDefault + redTurretShootPos;
             } else {
                 robot.light.setPosition(0.6);
 
+                autoStart = new Pose2d(autoStartBX, autoStartBY, Math.toRadians(autoStartBH));
+                drive = new MecanumDrive(hardwareMap, autoStart);
+                autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
+
                 xLeave = bLeaveX;
                 yLeave = bLeaveY;
                 hLeave = bLeaveH;
@@ -203,6 +210,9 @@ public class Auto_LT_Far extends LinearOpMode {
                 yStackPickupB = bStackPickupBY;
                 hStackPickupB = bStackPickupBH;
 
+                obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
+                obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
+                obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
                 turretShootPos = turrDefault + blueTurretShootPos;
             }
 
@@ -220,6 +230,8 @@ public class Auto_LT_Far extends LinearOpMode {
             shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB)))
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
 
+            limelightUsed = true;
+
             TELE.addData("Red?", redAlliance);
             TELE.addData("Turret Default", turrDefault);
             TELE.addData("Gate Cycle?", gatePickup);
@@ -239,9 +251,11 @@ public class Auto_LT_Far extends LinearOpMode {
             robot.transfer.setPower(1);
 
             startAuto();
+            shoot();
 
             if (stack3){
-                //cycleStackFar();
+                cycleStackFar();
+                shoot();
             }
 
             //TODO: insert code here for gate auto
@@ -278,6 +292,7 @@ public class Auto_LT_Far extends LinearOpMode {
                                 0.501,
                                 0.501,
                                 0.501,
+                                0.501,
                                 false
                         ),
                         autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
@@ -313,61 +328,51 @@ public class Auto_LT_Far extends LinearOpMode {
         Actions.runBlocking(leaveFromShoot.build());
     }
 
-//    void cycleStackFar(){
+    void cycleStackFar(){
-//        Actions.runBlocking(
+        Actions.runBlocking(
-//                new ParallelAction(
+                new ParallelAction(
-//                        pickup3.build(),
+                        pickup3.build(),
-//                        manageShooterAuto(
+                        autoActions.manageShooterAuto(
-//                                intake3Time,
+                                intakeStackTime,
-//                                0.501,
+                                xStackPickupB,
-//                                0.501,
+                                yStackPickupB,
-//                                0.501,
+                                posXTolerance,
-//                                0.501
+                                posYTolerance,
-//                        ),
+                                hStackPickupB,
-//                        intake(intake3Time),
+                                true
-//                        detectObelisk(
+                        ),
-//                                intake3Time,
+                        autoActions.intake(intakeStackTime),
-//                                0.501,
+                        autoActions.detectObelisk(
-//                                0.501,
+                                intakeStackTime,
-//                                0.501,
+                                xStackPickupB,
-//                                0.501,
+                                yStackPickupB,
-//                                obeliskTurrPos3
+                                posXTolerance,
-//                        )
+                                posYTolerance,
-//
+                                obeliskTurrPos3
-//                )
+                        )
-//        );
+
-//
+                )
-//        motif = turret.getObeliskID();
+        );
-//
+
-//        if (motif == 0) motif = prevMotif;
+        motif = turret.getObeliskID();
-//        prevMotif = motif;
+
-//
+        if (motif == 0) motif = 22;
-//        Actions.runBlocking(
+        prevMotif = motif;
-//                new ParallelAction(
+
-//                        manageFlywheelAuto(
+        Actions.runBlocking(
-//                                shoot3Time,
+                new ParallelAction(
-//                                0.501,
+                        autoActions.manageShooterAuto(
-//                                0.501,
+                                shootStackTime,
-//                                0.501,
+                                xShoot,
-//                                0.501
+                                yShoot,
-//                        ),
+                                posXTolerance,
-//                        shoot3.build(),
+                                posYTolerance,
-//                        prepareShootAll(colorSenseTime, shoot3Time, motif)
+                                hShoot,
-//                )
+                                false
-//        );
+                        ),
-//
+                        shoot3.build(),
-//        Actions.runBlocking(
+                        autoActions.prepareShootAll(colorSenseTime, shootStackTime, motif)
-//                new ParallelAction(
+                )
-//                        manageShooterAuto(
+        );
-//                                finalShootAllTime,
+    }
-//                                0.501,
-//                                0.501,
-//                                0.501,
-//                                0.501
-//                        ),
-//                        shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
-//                )
-//
-//        );
-//    }
 }

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

@@ -477,10 +477,6 @@ public class AutoActions{
             boolean whileIntaking
     ) {
 
-        boolean timeFallback = (time != 0.501);
-        boolean posXFallback = (posX != 0.501);
-        boolean posYFallback = (posY != 0.501);
-
         return new Action() {
 
             double stamp = 0.0;
@@ -488,6 +484,10 @@ public class AutoActions{
             int shootingTicker = 0;
             double shootingStamp = 0;
 
+            final boolean timeFallback = (time != 0.501);
+            final boolean posXFallback = (posX != 0.501);
+            final boolean posYFallback = (posY != 0.501);
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
 
@@ -539,17 +539,15 @@ public class AutoActions{
                 if (whileIntaking){
                     shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
                 } else {
-                    shouldFinish = timeDone || (xDone && yDone);
+                    shouldFinish = timeDone || (xDone && yDone) || doneShooting;
                 }
 
                 teleStart = currentPose;
 
-                if (doneShooting && shootingTicker == 0){
+                TELE.addData("Steady?", flywheel.getSteady());
-                    shootingTicker++;
+                TELE.update();
-                    shootingStamp = System.currentTimeMillis();
-                }
 
-                if (System.currentTimeMillis() - shootingStamp > 100 || shouldFinish){
+                if (shouldFinish){
                     doneShooting = false;
                     return false;
                 } else {

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

@@ -5,11 +5,11 @@ import com.acmerobotics.roadrunner.Pose2d;
 
 @Config
 public class Back_Poses {
-    public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50.1;
+    public static double rLeaveX = 90, rLeaveY = 50, rLeaveH = 50.1;
-    public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50;
+    public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50;
 
-    public static double rShootX = 95, rShootY = 85, rShootH = 90;
+    public static double rShootX = 100, rShootY = 55, rShootH = 90;
-    public static double bShootX = 95, bShootY = -85, bShootH = -90;
+    public static double bShootX = 100, bShootY = -55, bShootH = -90;
 
     public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
     public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
@@ -17,7 +17,7 @@ public class Back_Poses {
     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
+    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/utils/Flywheel.java

@@ -78,7 +78,7 @@ public class Flywheel {
 
         }
         // really should be a running average of the last 5
-        steady = (Math.abs(targetVelocity - velo) < 200.0);
+        steady = (Math.abs(commandedVelocity - velo) < 200.0);
 
         return powPID;
     }

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

@@ -89,8 +89,6 @@ public class Turret {
             robot.turr2.setPosition(1-pos);
             isFirstTurretPos = false;
         }
-        TELE.addLine("Moved Turret");
-        TELE.update();
         prevTurrPos = pos;
     }