front gate auto still in progress

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

@@ -40,17 +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 velGate0Start = 2700, hoodGate0Start = 0.61;
+    public static double velGate0Start = 2700, hoodGate0Start = 0.6;
 
-    public static double velGate0End = 2700, hoodGate0End = 0.4;
+    public static double velGate0End = 2700, hoodGate0End = 0.35;
+    public static double hood0MoveTime = 2;
+    public static double spindexerSpeedIncrease = 0.02;
 
-    public static double spindexerSpeedIncrease = 0.025;
+    public static double shootAllTime = 4;
-
-    double obeliskTurrPos1 = 0.0;
-    double obeliskTurrPos2 = 0.0;
-    double obeliskTurrPos3 = 0.0;
-
-    public static double shootAllTime = 2;
     public static double intake1Time = 3.3;
     public static double intake2Time = 3.8;
 
@@ -67,10 +63,16 @@ public class Auto_LT_Close extends LinearOpMode {
     public static double shoot3Time = 2.5;
     public static double colorSenseTime = 1.2;
     public int motif = 0;
-    public static double waitToShoot0 = 0.6;
+    public static double waitToShoot0 = 0.5;
-    public static double waitToPickupGate2 = 0.1;
+    public static double waitToPickupGate2 = 0.3;
     public static double pickupStackGateSpeed = 50;
-    public static double intake2TimeGate = 6;
+    public static double intake2TimeGate = 3;
+    public static double shoot2GateTime = 3;
+    public static double endGateTime = 25;
+    public static double waitToPickupGateWithPartner = 2;
+    public static double waitToPickupGateSolo = 1;
+    public static double intakeGateTime = 5;
+    public static double shootGateTime = 3;
 
     Robot robot;
     MultipleTelemetry TELE;
@@ -97,7 +99,8 @@ public class Auto_LT_Close extends LinearOpMode {
 
     double xShoot, yShoot, hShoot;
     double xShoot0, yShoot0, hShoot0;
-    double xGate, yGate, hGate;
+    double pickupGateAX, pickupGateAY, pickupGateAH;
+    double pickupGateBX, pickupGateBY, pickupGateBH;
     double xShootGate, yShootGate, hShootGate;
     double xLeave, yLeave, hLeave;
     double xLeaveGate, yLeaveGate, hLeaveGate;
@@ -105,7 +108,11 @@ public class Auto_LT_Close extends LinearOpMode {
     int ballCycles = 3;
     int prevMotif = 0;
     boolean gateCycle = true;
-
+    boolean withPartner = false;
+    double obeliskTurrPos1 = 0.0;
+    double obeliskTurrPos2 = 0.0;
+    double obeliskTurrPos3 = 0.0;
+    double waitToPickupGate = 0;
 
     // initialize path variables here
     TrajectoryActionBuilder shoot0 = null;
@@ -116,6 +123,8 @@ public class Auto_LT_Close extends LinearOpMode {
     TrajectoryActionBuilder pickup3 = null;
     TrajectoryActionBuilder shoot3 = null;
     TrajectoryActionBuilder shoot0ToPickup2 = null;
+    TrajectoryActionBuilder gateCyclePickup = null;
+    TrajectoryActionBuilder gateCycleShoot = null;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -155,7 +164,12 @@ public class Auto_LT_Close extends LinearOpMode {
 
         while (opModeInInit()) {
 
+            if (gateCycle){
+                servos.setHoodPos(hoodGate0Start);
+            } else {
                 servos.setHoodPos(shoot0Hood);
+            }
+
             turret.setTurret(turrDefault);
 
             if (gamepad2.crossWasPressed()) {
@@ -228,6 +242,13 @@ public class Auto_LT_Close extends LinearOpMode {
                 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;
@@ -276,6 +297,13 @@ public class Auto_LT_Close extends LinearOpMode {
                 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;
@@ -338,6 +366,20 @@ public class Auto_LT_Close extends LinearOpMode {
                     .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);
@@ -350,11 +392,19 @@ public class Auto_LT_Close extends LinearOpMode {
         if (isStopRequested()) return;
 
         if (opModeIsActive()) {
+            double stamp = getRuntime();
 
             robot.transfer.setPower(1);
 
             if (gateCycle){
                 shoot0Gate();
+                cycleStackMiddleGate();
+
+                while (getRuntime() - stamp < endGateTime){
+                    cycleGateIntake();
+                    cycleGateShoot();
+                }
+
             } else {
                 startAuto();
                 shoot();
@@ -574,9 +624,9 @@ public class Auto_LT_Close extends LinearOpMode {
                         shoot0ToPickup2.build(),
                         new SequentialAction(
                                 new ParallelAction(
-                                        autoActions.Wait(waitToShoot0),
                                         autoActions.manageShooterManual(
                                                 waitToShoot0,
+                                                0.501,
                                                 velGate0Start,
                                                 hoodGate0Start,
                                                 velGate0Start,
@@ -584,7 +634,15 @@ public class Auto_LT_Close extends LinearOpMode {
                                                 0.501
                                         )
                                 ),
-                                autoActions.shootAllManual(shootAllTime, spindexerSpeedIncrease, velGate0Start, hoodGate0Start, velGate0End, hoodGate0End,0.501),
+                                autoActions.shootAllManual(
+                                        shootAllTime,
+                                        hood0MoveTime,
+                                        spindexerSpeedIncrease,
+                                        velGate0Start,
+                                        hoodGate0Start,
+                                        velGate0End,
+                                        hoodGate0End,
+                                        0.501),
                                 autoActions.intake(
                                         intake2TimeGate,
                                         xShootGate,
@@ -596,4 +654,59 @@ public class Auto_LT_Close extends LinearOpMode {
         );
     }
 
+    void cycleStackMiddleGate(){
+        servos.setSpinPos(spinStartPos);
+        Actions.runBlocking(
+                new ParallelAction(
+                        shoot2.build(),
+                        new SequentialAction(
+                                new ParallelAction(
+                                        autoActions.manageShooterAuto(
+                                                shoot2GateTime,
+                                                xShootGate,
+                                                yShootGate,
+                                                hShootGate
+                                        ),
+                                        autoActions.Wait(shoot2GateTime)
+                                ),
+                                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
+                                        ),
+                                        autoActions.Wait(shootGateTime)
+                                ),
+                                autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                        )
+                )
+        );
+    }
 }

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;
@@ -32,7 +33,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 import java.util.Objects;
 
 @Config
-public class AutoActions{
+public class AutoActions {
     Robot robot;
     MultipleTelemetry TELE;
     Servos servos;
@@ -47,12 +48,12 @@ 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;
 
-    public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig){
+    public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig) {
         this.robot = rob;
         this.drive = dri;
         this.TELE = tel;
@@ -81,37 +82,38 @@ public class AutoActions{
 
             boolean decideGreenSlot = false;
 
-            void spin1PosFirst(){
+            void spin1PosFirst() {
                 firstSpindexShootPos = spindexer_outtakeBall1;
                 shootForward = true;
                 spinEndPos = spindexer_outtakeBall3 + 0.1;
             }
 
-            void spin2PosFirst(){
+            void spin2PosFirst() {
                 firstSpindexShootPos = spindexer_outtakeBall2;
                 shootForward = false;
                 spinEndPos = spindexer_outtakeBall3b - 0.1;
             }
 
-            void reverseSpin2PosFirst(){
+            void reverseSpin2PosFirst() {
                 firstSpindexShootPos = spindexer_outtakeBall2;
                 shootForward = true;
                 spinEndPos = 0.95;
             }
 
-            void spin3PosFirst(){
+            void spin3PosFirst() {
                 firstSpindexShootPos = spindexer_outtakeBall3;
                 shootForward = false;
                 spinEndPos = spindexer_outtakeBall1 - 0.1;
             }
 
-            void oddSpin3PosFirst(){
+            void oddSpin3PosFirst() {
                 firstSpindexShootPos = spindexer_outtakeBall3b;
                 shootForward = true;
                 spinEndPos = spindexer_outtakeBall2 + 0.1;
             }
 
             Action manageShooter = null;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
@@ -215,6 +217,7 @@ public class AutoActions{
 
             int shooterTicker = 0;
             Action manageShooter = null;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 drive.updatePoseEstimate();
@@ -238,16 +241,15 @@ public class AutoActions{
                 double prevSpinPos = servos.getSpinCmdPos();
 
                 boolean end;
-                if (shootForward){
+                if (shootForward) {
                     end = prevSpinPos > spinEndPos;
                 } else {
                     end = prevSpinPos < spinEndPos;
                 }
 
-                if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) {
+                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);
@@ -255,7 +257,7 @@ public class AutoActions{
                         Spindexer.whileShooting = true;
                         if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) {
                             servos.setSpinPos(prevSpinPos + spindexSpeed);
-                        } else if (shooterTicker > Spindexer.waitFirstBallTicks){
+                        } else if (shooterTicker > Spindexer.waitFirstBallTicks) {
                             servos.setSpinPos(prevSpinPos - spindexSpeed);
                         }
 
@@ -276,7 +278,15 @@ public class AutoActions{
         };
     }
 
-    public Action shootAllManual(double shootTime, double spindexSpeed, double velStart, double hoodStart, double velEnd, double hoodEnd, double turr) {
+    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;
 
@@ -284,6 +294,7 @@ public class AutoActions{
 
             int shooterTicker = 0;
             Action manageShooter = null;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 drive.updatePoseEstimate();
@@ -297,7 +308,7 @@ public class AutoActions{
 
                 if (ticker == 1) {
                     stamp = System.currentTimeMillis();
-                    manageShooter = manageShooterManual(shootTime, velStart, hoodStart, velEnd, hoodEnd, turr);
+                    manageShooter = manageShooterManual(shootTime, hoodMoveTime, velStart, hoodStart, velEnd, hoodEnd, turr);
 
                 }
                 ticker++;
@@ -307,29 +318,22 @@ public class AutoActions{
                 double prevSpinPos = servos.getSpinCmdPos();
 
                 boolean end;
-                if (shootForward){
+                if (shootForward) {
                     end = prevSpinPos > spinEndPos;
                 } else {
                     end = prevSpinPos < spinEndPos;
                 }
 
-                if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) {
+                if (System.currentTimeMillis() - stamp < shootTime * 1000 && !end) {
-
-                    if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) {
-                        servos.setTransferPos(transferServo_out);
-                        servos.setSpinPos(firstSpindexShootPos);
-                    } else {
                     servos.setTransferPos(transferServo_in);
                     shooterTicker++;
                     Spindexer.whileShooting = true;
                     if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) {
                         servos.setSpinPos(prevSpinPos + spindexSpeed);
-                        } else if (shooterTicker > Spindexer.waitFirstBallTicks){
+                    } else if (shooterTicker > Spindexer.waitFirstBallTicks) {
                         servos.setSpinPos(prevSpinPos - spindexSpeed);
                     }
 
-                    }
-
                     return true;
 
                 } else {
@@ -355,6 +359,7 @@ public class AutoActions{
             double stamp = 0.0;
             int ticker = 0;
             Action manageShooter = null;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
@@ -374,7 +379,7 @@ public class AutoActions{
 
                 manageShooter.run(telemetryPacket);
 
-                if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()){
+                if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()) {
                     spindexer.setIntakePower(-0.1);
                     return false;
                 } else {
@@ -385,6 +390,7 @@ public class AutoActions{
     }
 
     private boolean detectingObelisk = false;
+
     public Action detectObelisk(
             double time,
             double posX,
@@ -427,8 +433,8 @@ public class AutoActions{
 
                 teleStart = currentPose;
 
-                if (shouldFinish){
+                if (shouldFinish) {
-                    if (redAlliance){
+                    if (redAlliance) {
                         turret.pipelineSwitch(4);
                     } else {
                         turret.pipelineSwitch(2);
@@ -514,13 +520,14 @@ public class AutoActions{
         };
     }
 
-    public Action Wait(double time){
+    public Action Wait(double time) {
         return new Action() {
             boolean ticker = false;
             double stamp = 0.0;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                if (!ticker){
+                if (!ticker) {
                     stamp = System.currentTimeMillis();
                     ticker = true;
                 }
@@ -532,19 +539,20 @@ public class AutoActions{
     }
 
     public Action manageShooterManual(
-            double time,
+            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 = (time != 0.501);
+            final boolean timeFallback = (maxTime != 0.501);
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -578,14 +586,14 @@ public class AutoActions{
                     turret.setTurret(turr);
                 }
 
-                servos.setHoodPos(hoodStart + (hoodEnd-hoodStart) * ((System.currentTimeMillis() - stamp)/(time*1000)));
+                servos.setHoodPos(hoodStart + ((hoodEnd - hoodStart) * Math.min(((System.currentTimeMillis() - stamp) / (hoodMoveTime * 1000)), 1)));
-                double vel = velStart + (velEnd-velStart) * ((System.currentTimeMillis() - stamp)/(time*1000));
+                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) > time * 1000;
+                boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > maxTime * 1000;
 
                 teleStart = currentPose;
 

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

@@ -51,5 +51,11 @@ public class Front_Poses {
     public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55;
     public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55;
 
+    public static double rPickupGateAX = 36, rPickupGateAY = 50, rPickupGateAH = 140;
+    public static double bPickupGateAX = 36, bPickupGateAY = -50, bPickupGateAH = -140;
+
+    public static double rPickupGateBX = 46, rPickupGateBY = 60, rPickupGateBH = 140;
+    public static double bPickupGateBX = 46, bPickupGateBY = -60, bPickupGateBH = -140;
+
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
 }

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

@@ -16,8 +16,8 @@ public class ServoPositions {
 
     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.25;
+    public static double spinStartPos = 0.05;
-    public static double spinEndPos = 0.85;
+    public static double spinEndPos = 0.95;
 
     public static double shootAllSpindexerSpeedIncrease = 0.014;