shoooooottteeer test

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

@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double rapidFireBlocker_Closed = 0.3;
+    public static double rapidFireBlocker_Closed = 0.35;
     public static double rapidFireBlocker_Open = 0.5;
 
     public static double spindexBlocker_Closed = 0.31;

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

@@ -25,6 +25,7 @@ public class TeleopV4 extends LinearOpMode {
     MultipleTelemetry TELE;
     Follower follower;
     SpindexerTransferIntake spindexerTransferIntake;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -49,7 +50,7 @@ public class TeleopV4 extends LinearOpMode {
 
         if (isStopRequested()) return;
 
-        while (opModeIsActive()){
+        while (opModeIsActive()) {
 
             //Drivetrain
 
@@ -93,9 +94,6 @@ public class TeleopV4 extends LinearOpMode {
             }
 
 
-
-
-
             TELE.update();
         }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java

@@ -0,0 +1,126 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
+import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
+
+@Config
+@TeleOp
+public class NewShooterTest extends LinearOpMode {
+
+    Robot robot;
+
+    public static boolean intake = true;
+    public static boolean shoot = false;
+    public static double intakePower = 1.0;
+    public static double transferShootPower = -1;
+    public static double transferIntakePower = -1.0;
+    public static double turretPos = 0.51;
+    public static double hoodPos = 0.51;
+    public static double flywheel = 0;
+
+    private enum ShootState {
+        IDLE,
+        WAIT_GATE,
+        WAIT_PUSH
+    }
+
+    private ShootState shootState = ShootState.IDLE;
+    private long timestamp = 0;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        Robot.resetInstance();
+
+        robot = Robot.getInstance(hardwareMap);
+
+        Shooter shooter = new Shooter(
+                robot,
+                new MultipleTelemetry(
+                        telemetry,
+                        FtcDashboard.getInstance().getTelemetry()
+                ),
+                Constants.createFollower(hardwareMap),
+                true
+        );
+
+        shooter.setState(Shooter.ShooterState.MANUAL);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+            robot.setHoodPos(hoodPos);
+            shooter.setTurretPosition(turretPos);
+            shooter.setFlywheelVelocity(flywheel);
+            robot.setSpinPos(ServoPositions.spindexer_A2);
+
+            if (intake && !shoot) {
+
+                shootState = ShootState.IDLE;
+
+                robot.setRapidFireBlockerPos(
+                        ServoPositions.rapidFireBlocker_Closed);
+
+                robot.setTransferPower(transferIntakePower);
+                robot.setIntakePower(intakePower);
+                robot.setTransferServoPos(
+                        ServoPositions.transferServo_out);
+            }
+
+            else if (shoot) {
+                robot.setIntakePower(intakePower);
+
+
+                switch (shootState) {
+
+                    case IDLE:
+
+                        robot.setTransferPower(transferShootPower);
+
+                        timestamp = System.currentTimeMillis();
+                        shootState = ShootState.WAIT_GATE;
+
+                        break;
+
+                    case WAIT_GATE:
+
+                        if (System.currentTimeMillis() - timestamp >= 300) {
+
+                            robot.setRapidFireBlockerPos(
+                                    ServoPositions.rapidFireBlocker_Open);
+
+                            timestamp = System.currentTimeMillis();
+                            shootState = ShootState.WAIT_PUSH;
+                        }
+
+                        break;
+
+                    case WAIT_PUSH:
+
+                        if (System.currentTimeMillis() - timestamp >= 100) {
+
+                            robot.setTransferServoPos(
+                                    ServoPositions.transferServo_in);
+
+                            shootState = ShootState.IDLE;
+                        }
+
+                        break;
+                }
+            }
+
+            shooter.update();
+        }
+    }
+}

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

@@ -125,7 +125,7 @@ public class Robot {
         spin2 = hardwareMap.get(Servo.class, "spin2");
 
         transfer = hardwareMap.get(DcMotorEx.class, "transfer");
-        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
+
 
         transferServo = hardwareMap.get(Servo.class, "transferServo");