back auto gate cycling in progress

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

@@ -22,6 +22,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Light;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 import org.firstinspires.ftc.teamcode.utils.Spindexer;
@@ -78,6 +79,7 @@ public class Auto_LT_Close extends LinearOpMode {
     Targeting targeting;
     Targeting.Settings targetingSettings;
     AutoActions autoActions;
+    Light light;
     double x1, y1, h1;
 
     double x2a, y2a, h2a, t2a;
@@ -132,7 +134,11 @@ public class Auto_LT_Close extends LinearOpMode {
 
         drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
 
-        autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
+        light = Light.getInstance();
+
+        light.init(robot.light, spindexer, turret);
+
+        autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light);
 
         servos.setSpinPos(spinStartPos);
 

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

@@ -3,26 +3,14 @@ package org.firstinspires.ftc.teamcode.autonomous;
 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;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
-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;
-
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.ParallelAction;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
@@ -35,14 +23,13 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Light;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
 import org.firstinspires.ftc.teamcode.utils.Turret;
 
-import java.util.Objects;
-
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Far extends LinearOpMode {
@@ -62,15 +49,16 @@ public class Auto_LT_Far extends LinearOpMode {
     Targeting targeting;
     Targeting.Settings targetingSettings;
     AutoActions autoActions;
+    Light light;
     double xShoot, yShoot, hShoot;
     double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
-    double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
     public static double flywheel0Time = 1.5;
-    boolean gatePickup = false;
+    boolean gatePickup = true;
     boolean stack3 = true;
     double xStackPickupA, yStackPickupA, hStackPickupA;
     double xStackPickupB, yStackPickupB, hStackPickupB;
     public static int pickupStackSpeed = 12;
+    public static int pickupGateSpeed = 25;
     int prevMotif = 0;
     public static double spindexerSpeedIncrease = 0.008;
     public static double shootAllTime = 2;
@@ -80,8 +68,8 @@ public class Auto_LT_Far extends LinearOpMode {
     public static double shootStackTime = 2;
     public static double shootGateTime = 2;
     public static double colorSenseTime = 1;
-    public static double intakeStackTime = 3.3;
+    public static double intakeStackTime = 3;
-    public static double intakeGateTime = 3;
+    public static double intakeGateTime = 2.5;
     public static double redObeliskTurrPos1 = 0.12;
     public static double redObeliskTurrPos2 = 0.13;
     public static double redObeliskTurrPos3 = 0.14;
@@ -91,12 +79,15 @@ public class Auto_LT_Far extends LinearOpMode {
     double obeliskTurrPos1 = 0.0;
     double obeliskTurrPos2 = 0.0;
     double obeliskTurrPos3 = 0.0;
+    public static double endAutoTime = 25;
 
     // initialize path variables here
     TrajectoryActionBuilder leave3Ball = null;
     TrajectoryActionBuilder leaveFromShoot = null;
     TrajectoryActionBuilder pickup3 = null;
     TrajectoryActionBuilder shoot3 = null;
+    TrajectoryActionBuilder pickupGate = null;
+    TrajectoryActionBuilder shootGate = null;
     Pose2d autoStart = new Pose2d(0,0,0);
 
     @Override
@@ -117,6 +108,10 @@ public class Auto_LT_Far extends LinearOpMode {
 
         servos = new Servos(hardwareMap);
 
+        light = Light.getInstance();
+
+        light.init(robot.light, spindexer, turret);
+
         robot.limelight.start();
 
         robot.limelight.pipelineSwitch(1);
@@ -159,7 +154,7 @@ public class Auto_LT_Far extends LinearOpMode {
 
                 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);
+                autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light);
 
                 xLeave = rLeaveX;
                 yLeave = rLeaveY;
@@ -177,6 +172,10 @@ public class Auto_LT_Far extends LinearOpMode {
                 yStackPickupB = rStackPickupBY;
                 hStackPickupB = rStackPickupBH;
 
+                pickupGateX = rPickupGateX;
+                pickupGateY = rPickupGateY;
+                pickupGateH = rPickupGateH;
+
                 obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
@@ -192,7 +191,7 @@ public class Auto_LT_Far extends LinearOpMode {
 
                 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);
+                autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light);
 
                 xLeave = bLeaveX;
                 yLeave = bLeaveY;
@@ -210,6 +209,10 @@ public class Auto_LT_Far extends LinearOpMode {
                 yStackPickupB = bStackPickupBY;
                 hStackPickupB = bStackPickupBH;
 
+                pickupGateX = bPickupGateX;
+                pickupGateY = bPickupGateY;
+                pickupGateH = bPickupGateH;
+
                 obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
@@ -236,6 +239,13 @@ 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));
 
+            pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
+                    .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH),
+                            new TranslationalVelConstraint(pickupGateSpeed));
+
+            shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH)))
+                    .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
+
             limelightUsed = true;
 
             TELE.addData("Red?", redAlliance);
@@ -253,6 +263,7 @@ 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);
 
@@ -264,7 +275,10 @@ public class Auto_LT_Far extends LinearOpMode {
                 shoot();
             }
 
-            //TODO: insert code here for gate auto
+            while (gatePickup && getRuntime() - stamp < endAutoTime){
+                cycleGatePickup();
+                shoot();
+            }
 
             if (gatePickup || stack3){
                 leave();
@@ -382,4 +396,51 @@ public class Auto_LT_Far extends LinearOpMode {
                 )
         );
     }
+
+    void cycleGatePickup(){
+        Actions.runBlocking(
+                new ParallelAction(
+                        pickupGate.build(),
+                        autoActions.manageShooterAuto(
+                                intakeGateTime,
+                                pickupGateX,
+                                pickupGateY,
+                                posXTolerance,
+                                posYTolerance,
+                                pickupGateH,
+                                true
+                        ),
+                        autoActions.intake(intakeStackTime),
+                        autoActions.detectObelisk(
+                                intakeGateTime,
+                                pickupGateX,
+                                pickupGateY,
+                                posXTolerance,
+                                posYTolerance,
+                                obeliskTurrPos3
+                        )
+                )
+        );
+
+        motif = turret.getObeliskID();
+
+        if (motif == 0) motif = prevMotif;
+        prevMotif = motif;
+
+        Actions.runBlocking(
+                new ParallelAction(
+                        autoActions.manageShooterAuto(
+                                shootGateTime,
+                                xShoot,
+                                yShoot,
+                                posXTolerance,
+                                posYTolerance,
+                                hShoot,
+                                false
+                        ),
+                        shootGate.build(),
+                        autoActions.prepareShootAll(colorSenseTime, shootGateTime, motif)
+                )
+        );
+    }
 }

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

@@ -21,8 +21,10 @@ import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.Pose2d;
 
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.constants.StateEnums;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Light;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 import org.firstinspires.ftc.teamcode.utils.Spindexer;
@@ -41,6 +43,7 @@ public class AutoActions{
     Spindexer spindexer;
     Targeting targeting;
     Targeting.Settings targetingSettings;
+    Light light;
     Turret turret;
     private int driverSlotGreen = 0;
     private int passengerSlotGreen = 0;
@@ -52,7 +55,7 @@ public class AutoActions{
     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){
+    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;
@@ -62,6 +65,7 @@ public class AutoActions{
         this.targeting = tar;
         this.targetingSettings = tS;
         this.turret = tur;
+        this.light = lig;
     }
 
     public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
@@ -112,6 +116,8 @@ public class AutoActions{
                 servos.setTransferPos(transferServo_out);
                 drive.updatePoseEstimate();
 
+                light.setState(StateEnums.LightState.GOAL_LOCK);
+
                 teleStart = drive.localizer.getPose();
 
                 if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
@@ -215,6 +221,9 @@ public class AutoActions{
 
                 spindexer.setIntakePower(-0.1);
 
+                light.setState(StateEnums.LightState.BALL_COLOR);
+                light.update();
+
                 if (ticker == 1) {
                     stamp = System.currentTimeMillis();
                 }
@@ -276,8 +285,9 @@ public class AutoActions{
 
                 spindexer.processIntake();
                 spindexer.setIntakePower(1);
+                light.setState(StateEnums.LightState.BALL_COUNT);
+                light.update();
 
-                spindexer.ballCounterLight();
                 drive.updatePoseEstimate();
 
                 teleStart = drive.localizer.getPose();

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

@@ -14,9 +14,12 @@ public class Back_Poses {
     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 rStackPickupBX = 55, rStackPickupBY = 83, rStackPickupBH = 140.1;
     public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
 
+    public static double rPickupGateX = 65, rPickupGateY = 80, rPickupGateH = 140;
+    public static double bPickupGateX = 60, bPickupGateY = -88, 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/utils/Light.java

@@ -12,7 +12,7 @@ public final class Light {
 
     private static Light instance;
     public static  double ballColorCycleTime = 1000; //in ms
-    public static double restingTime = 150; //in ms
+    public static double restingTime = 125; //in ms
 
     private Servo lightServo;
     private LightState state = LightState.DISABLED;