front gate auto in progress

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

@@ -40,7 +40,11 @@ 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.61;
+
+    public static double velGate0End = 2700, hoodGate0End = 0.4;
+
+    public static double spindexerSpeedIncrease = 0.025;
 
     double obeliskTurrPos1 = 0.0;
     double obeliskTurrPos2 = 0.0;
@@ -63,12 +67,10 @@ 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 xShoot0 = 40, yShoot0 = 0.1, hShoot0 = 0.1;
+    public static double waitToShoot0 = 0.6;
-    public static double waitToShoot0 = 1.1;
+    public static double waitToPickupGate2 = 0.1;
-    public static double waitToPickupGate2 = 0.5;
+    public static double pickupStackGateSpeed = 50;
-    public static double pickupStackGateSpeed = 23;
     public static double intake2TimeGate = 6;
-    public static double xShootGate = 40, yShootGate = 0.2, hShootGate = 0.2;
 
     Robot robot;
     MultipleTelemetry TELE;
@@ -94,9 +96,11 @@ public class Auto_LT_Close extends LinearOpMode {
     double x4b, y4b, h4b;
 
     double xShoot, yShoot, hShoot;
+    double xShoot0, yShoot0, hShoot0;
     double xGate, yGate, hGate;
-    double xPrep, yPrep, hPrep;
+    double xShootGate, yShootGate, hShootGate;
     double xLeave, yLeave, hLeave;
+    double xLeaveGate, yLeaveGate, hLeaveGate;
 
     int ballCycles = 3;
     int prevMotif = 0;
@@ -206,9 +210,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;
@@ -216,6 +218,16 @@ 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;
+
                 obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
@@ -247,9 +259,6 @@ public class Auto_LT_Close extends LinearOpMode {
                 y4b = by4b;
                 h4b = bh4b;
 
-                xPrep = bxPrep;
-                yPrep = byPrep;
-                hPrep = bhPrep;
                 xShoot = bShootX;
                 yShoot = bShootY;
                 hShoot = bShootH;
@@ -257,6 +266,16 @@ 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;
+
                 obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
@@ -265,12 +284,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 {
@@ -283,12 +312,15 @@ public class Auto_LT_Close extends LinearOpMode {
                     .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
                             new TranslationalVelConstraint(pickup1Speed));
 
-            if (ballCycles < 3 || 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){
                 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)))
@@ -543,14 +575,16 @@ public class Auto_LT_Close extends LinearOpMode {
                         new SequentialAction(
                                 new ParallelAction(
                                         autoActions.Wait(waitToShoot0),
-                                        autoActions.manageShooterAuto(
+                                        autoActions.manageShooterManual(
                                                 waitToShoot0,
-                                                xShoot0,
+                                                velGate0Start,
-                                                yShoot0,
+                                                hoodGate0Start,
-                                                hShoot0
+                                                velGate0Start,
+                                                hoodGate0Start,
+                                                0.501
                                         )
                                 ),
-                                autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease),
+                                autoActions.shootAllManual(shootAllTime, spindexerSpeedIncrease, velGate0Start, hoodGate0Start, velGate0End, hoodGate0End,0.501),
                                 autoActions.intake(
                                         intake2TimeGate,
                                         xShootGate,
@@ -561,4 +595,5 @@ public class Auto_LT_Close extends LinearOpMode {
                 )
         );
     }
+
 }

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

@@ -276,6 +276,75 @@ public class AutoActions{
         };
     }
 
+    public Action shootAllManual(double shootTime, 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, 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 || shooterTicker < Spindexer.waitFirstBallTicks+1)) {
+
+                    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){
+                            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,
@@ -305,7 +374,12 @@ 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;
+                }
             }
         };
     }
@@ -409,7 +483,7 @@ public class AutoActions{
                 if (posX != 0.501) {
                     deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
                 } else {
-                    deltaPose = new Pose2d(robotX, robotY, robotHeading);
+                    deltaPose = new Pose2d(dx, dy, robotHeading);
                 }
 
 //                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
@@ -456,6 +530,72 @@ public class AutoActions{
             }
         };
     }
+
+    public Action manageShooterManual(
+            double time,
+            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);
+
+            @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) * ((System.currentTimeMillis() - stamp)/(time*1000)));
+                double vel = velStart + (velEnd-velStart) * ((System.currentTimeMillis() - stamp)/(time*1000));
+
+                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;
+
+                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,14 @@ 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 = 40, rShootGateY = 0.2, rShootGateH = 90;
+    public static double bShootGateX = 40, bShootGateY = -0.2, bShootGateH = -90;
+
+    public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55;
+    public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55;
+
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
 }

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

@@ -16,7 +16,7 @@ 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.3;
+    public static double spinStartPos = 0.25;
     public static double spinEndPos = 0.85;
 
     public static double shootAllSpindexerSpeedIncrease = 0.014;