lights added to auto

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

@@ -35,6 +35,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;
@@ -62,6 +63,7 @@ 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;
@@ -117,6 +119,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 +165,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;
@@ -192,7 +198,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;

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/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;