Added spindexer to teleopv4

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java

@@ -1,11 +1,19 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
+import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
 
 @TeleOp
@@ -13,7 +21,10 @@ import org.firstinspires.ftc.teamcode.utilsv2.*;
 public class TeleopV4 extends LinearOpMode {
     Robot robot;
     Drivetrain drivetrain;
+    Shooter shooter;
     MultipleTelemetry TELE;
+    Follower follower;
+    SpindexerTransferIntake spindexerTransferIntake;
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -24,6 +35,15 @@ public class TeleopV4 extends LinearOpMode {
         );
 
         drivetrain = new Drivetrain(robot, TELE);
+        follower = Constants.createFollower(hardwareMap);
+        Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
+        follower.setStartingPose(start);
+
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance);
+        shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE);
+        spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+
 
         waitForStart();
 
@@ -39,6 +59,41 @@ public class TeleopV4 extends LinearOpMode {
                     gamepad1.left_stick_x
             );
 
+            shooter.update();
+            spindexerTransferIntake.update();
+
+            SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
+
+            if (gamepad1.xWasPressed() &&
+                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
+                            state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
+                            state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
+                            state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
+                            state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
+
+                spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+            }
+
+            if (gamepad1.aWasPressed() &&
+                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
+
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.HOLD_BALLS
+                );
+            }
+
+            if (gamepad1.yWasPressed()
+                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
+
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.INTAKE
+                );
+            }
+
+
+
 
 
             TELE.update();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java

@@ -45,7 +45,7 @@ public class Shooter {
     private double flywheelVelocity = 0.0;
     private double turretPosition = 0.5;
 
-    enum ShooterState {
+    public enum ShooterState {
         READ_OBELISK,
         TRACK_GOAL,
         MANUAL_FLYWHEEL_TRACK_TURR,

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java

@@ -51,6 +51,10 @@ public class SpindexerTransferIntake {
         this.mode = spindexerMode;
     }
 
+    public RapidMode getRapidState(){
+        return this.rapidMode;
+    }
+
     private long stateTime() {
         return System.currentTimeMillis() - stateStartTime;
     }