sorting auto is done

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

@@ -18,11 +18,10 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
-import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
 import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
-import org.firstinspires.ftc.teamcode.utilsv2.Robot;
-import org.firstinspires.ftc.teamcode.utilsv2.Turret;
 
 import java.util.List;
 
@@ -33,45 +32,52 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
     MultipleTelemetry TELE;
     Follower follower;
     MeasuringLoopTimes loopTimes;
+    Shooter shooter;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+    SpindexerTransferIntake spindexer;
 
     // Wait Times
-    public static double shootTime = 2;
+    public static double sortedShootTime = 2;
-    public static double openGateTime = 1.5;
+    public static double rapidWaitTime = 0.25;
+    public static double rapidShootTime = 1;
+    public static double openGateTime = 2.5;
+    public static double pushTime = 2;
 
     // Extra Variables
-    public static double intakePower = 0.3;
+    public static double intakePower = 0.5;
     double shootX, shootY, shootH;
 
     // Initialize path state machine
     private enum PathState {
         PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
-        PICKUP1, DRIVE_OPENGATE, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
+        PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
         DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
         DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
     }
     PathState pathState = PathState.PUSHBOT;
 
     // Poses
-    public static double startPoseX = 84, startPoseY = 7, startPoseH = 90;
+    public static double startPoseX = 91.5, startPoseY = 15, startPoseH = 90;
-    public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
+    public static double pushBotX = 97, pushBotY = 18, pushBotH = 100;
-    public static double shoot0ControlX = 88.29667812142038, shoot0ControlY = 52.03493699885454;
+    public static double shoot0ControlX = 94.29667812142038, shoot0ControlY = 55.03493699885454;
-    public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
+    public static double shoot0X = 95, shoot0Y = 83, shoot0H = 0;
-    public static double pickup1ControlX = 109.29381443298968, pickup1ControlY = 82.70618556701031;
     public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0;
     public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165;
-    public static double openGateX = 129, openGateY = 74, openGateH = 0;
+    public static double openGateX = 131, openGateY = 74, openGateH = 0;
     public static double shoot1ControlX = 112, shoot1ControlY = 75;
-    public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12;
+    public static double shoot1X = 95, shoot1Y = 83, shoot1H = 0;
-    public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0;
+    public static double drivePickup2X = 98, drivePickup2Y = 58.5, drivePickup2H = 0;
     public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0;
     public static double shoot2ControlX = 102, shoot2ControlY = 63;
-    public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50;
+    public static double shoot2X = 95, shoot2Y = 83, shoot2H = 0;
-    public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0;
+    public static double drivePickup3X = 98, drivePickup3Y = 34.5, drivePickup3H = 0;
     public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0;
     public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446;
-    public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80;
+    public static double shoot3X = 84, shoot3Y = 120, shoot3H = -90;
     Pose startPose, pushBot, shoot0Control, shoot0,
-            pickup1Control, pickup1, openGateControl, openGate, shoot1Control, shoot1,
+            pickup1, openGateControl, openGate, shoot1Control, shoot1,
             drivePickup2, pickup2, shoot2Control, shoot2,
             drivePickup3, pickup3, shoot3Control, shoot3;
     private void initializePoses(){
@@ -79,7 +85,6 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
         pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH));
         shoot0Control = new Pose(shoot0ControlX, shoot0ControlY);
         shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
-        pickup1Control = new Pose(pickup1ControlX, pickup1ControlY);
         pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H));
         openGateControl = new Pose(openGateControlX, openGateControlY);
         openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH));
@@ -112,7 +117,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                 .build();
 
         shoot0_pickup1 = follower.pathBuilder()
-                .addPath(new BezierCurve(shoot0, pickup1Control, pickup1))
+                .addPath(new BezierLine(shoot0, pickup1))
                 .setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading())
                 .build();
 
@@ -164,42 +169,46 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
         double currentTime = (double) System.currentTimeMillis() / 1000;
         switch(pathState){
             case PUSHBOT:
+                spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+                shooter.setFlywheelVelocity(2400);
+                robot.setHoodPos(0.64);
                 if (startAuto){
-                    follower.followPath(startPose_pushBot, true);
+                    follower.followPath(startPose_pushBot, false);
                     startAuto = false;
+                    timeStamp = currentTime;
                     shootX = shoot0X;
                     shootY = shoot0Y;
                     shootH = shoot0H;
                 }
-                if (!follower.isBusy()){
+                if (!follower.isBusy() || currentTime - timeStamp > pushTime){
                     follower.followPath(pushBot_shoot0, true);
                     pathState = PathState.DRIVE_SHOOT0;
+                    timeStamp = currentTime;
                 }
                 break;
             case DRIVE_SHOOT0:
-                if (!follower.isBusy()){
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
                     timeStamp = currentTime;
                     pathState = PathState.WAIT_SHOOT0;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
                 }
                 break;
             case WAIT_SHOOT0:
-                if (currentTime - timeStamp > shootTime){
+                if (currentTime - timeStamp > rapidShootTime &&
+                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
+                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
                     follower.followPath(shoot0_pickup1, intakePower, false);
                     pathState = PathState.PICKUP1;
+                    spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SORTED);
                 }
                 break;
             case PICKUP1:
                 if (!follower.isBusy()){
-                    follower.followPath(pickup1_openGate, true);
+                    follower.followPath(pickup1_openGate, false);
                     pathState = PathState.OPENGATE;
                     shootX = shoot1X;
                     shootY = shoot1Y;
                     shootH = shoot1H;
-                }
-                break;
-            case DRIVE_OPENGATE:
-                if (!follower.isBusy()){
-                    pathState = PathState.OPENGATE;
                     timeStamp = currentTime;
                 }
                 break;
@@ -213,10 +222,11 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                 if (!follower.isBusy()){
                     pathState = PathState.WAIT_SHOOT1;
                     timeStamp = currentTime;
+                    spindexer.startSortedShoot();
                 }
                 break;
             case WAIT_SHOOT1:
-                if (currentTime - timeStamp > shootTime){
+                if (currentTime - timeStamp > sortedShootTime){
                     follower.followPath(shoot1_drivePickup2, true);
                     pathState = PathState.DRIVE_PICKUP2;
                 }
@@ -240,16 +250,19 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                 if (!follower.isBusy()){
                     pathState = PathState.WAIT_SHOOT2;
                     timeStamp = currentTime;
+                    spindexer.startSortedShoot();
                 }
                 break;
             case WAIT_SHOOT2:
-                if (currentTime - timeStamp > shootTime){
+                if (currentTime - timeStamp > sortedShootTime){
                     follower.followPath(shoot2_drivePickup3, true);
                     pathState = PathState.DRIVE_PICKUP3;
                 }
                 break;
             case DRIVE_PICKUP3:
                 if (!follower.isBusy()){
+                    shooter.setFlywheelVelocity(2300);
+                    robot.setHoodPos(0.68);
                     follower.followPath(drivePickup3_pickup3, intakePower, false);
                     pathState = PathState.PICKUP3;
                 }
@@ -266,6 +279,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
             case DRIVE_SHOOT3:
                 if (!follower.isBusy()){
                     pathState = PathState.WAIT_SHOOT3;
+                    spindexer.startSortedShoot();
                 }
                 break;
             case WAIT_SHOOT3:
@@ -296,19 +310,33 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
         }
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
         follower = Constants.createFollower(hardwareMap);
+        sleep(1000);
         follower.setStartingPose(new Pose(72,72,0));
         loopTimes = new MeasuringLoopTimes();
         loopTimes.init();
+        turret = new Turret(robot);
+        flywheel = new Flywheel(robot);
+        commander = new VelocityCommander();
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
+        spindexer = new SpindexerTransferIntake(robot, TELE, commander);
+        ParkTilter park = new ParkTilter(robot);
 
         boolean initializeRobot = false;
         while (opModeInInit()){
             follower.update();
 
+            if (gamepad1.squareWasPressed()){
+                robot.setSpinPos(ServoPositions.spindexer_A2);
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
+                robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
+            }
+
             if (gamepad1.crossWasPressed() && !initializeRobot){
                 Color.redAlliance = !Color.redAlliance;
+                shooter.setRedAlliance(Color.redAlliance);
 
                 double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X,
-                        pickup1ControlX, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
+                        pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
                         drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
                         drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
 
@@ -326,14 +354,26 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                 initializePoses();
                 follower.setPose(startPose);
                 buildPaths();
-//                turret.switchPipeline(Turret.PipelineMode.OBELISK);
-                robot.limelight.start();
                 sleep(2000);
+                turret.switchPipeline(Turret.PipelineMode.OBELISK);
+                robot.limelight.start();
+                limelightUsed = true;
+                park.unpark();
+            }
+
+            if (initializeRobot){
+                //add obelisk read here
+                shooter.setState(Shooter.ShooterState.READ_OBELISK);
+                int ID = turret.getObeliskID();
+                spindexer.setDesiredPatternAuto(ID);
+                TELE.addData("ID", ID);
+                shooter.update(robot.voltage.getVoltage());
             }
 
             TELE.addData("Red Alliance?", Color.redAlliance);
             TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot);
             TELE.addData("Start Pose", follower.getPose());
+            TELE.addData("Current LL Pipeline", turret.pipeline());
             TELE.update();
         }
 
@@ -341,9 +381,10 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
 
         if (isStopRequested()) return;
 
-        limelightUsed = false;
-
         while (opModeIsActive()){
+            shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            shooter.update(robot.voltage.getVoltage());
+
             follower.update();
             pathStateMachine();
             Pose currentPose = follower.getPose();
@@ -351,6 +392,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
             teleStartPoseY = currentPose.getY();
             teleStartPoseH = Math.toDegrees(currentPose.getHeading());
 
+            spindexer.update();
+
             for (LynxModule hub : allHubs) {
                 hub.clearBulkCache();
             }

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.35;
+    public static double rapidFireBlocker_Closed = 0.32;
     public static double rapidFireBlocker_Open = 0.5;
 
     public static double spindexBlocker_Closed = 0.31;
@@ -34,9 +34,9 @@ public class ServoPositions {
 
     public static double shootAllSpindexerSpeedIncrease = 0.01;
 
-    public static double transferServo_out = 0.57;
+    public static double transferServo_out = 0.28;
 
-    public static double transferServo_in = 0.77;
+    public static double transferServo_in = 0.54;
 
     public static double hoodAuto = 0.27;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java

@@ -5,7 +5,7 @@ import com.pedropathing.control.FilteredPIDFCoefficients;
 import com.pedropathing.control.PIDFCoefficients;
 import com.pedropathing.follower.Follower;
 import com.pedropathing.follower.FollowerConstants;
-import com.pedropathing.ftc.FollowerBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.FollowerBuilder;
 import com.pedropathing.ftc.drivetrains.MecanumConstants;
 import com.pedropathing.ftc.localization.constants.PinpointConstants;
 import com.pedropathing.paths.PathConstraints;
@@ -22,7 +22,7 @@ public class Constants {
             .lateralZeroPowerAcceleration(-60.876)
             .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.015, 0.02))
             .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02))
-            .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.02, 0, 0.00001, 0.6, 0.015))
+            .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.013, 0, 0.00001, 0.6, 0.015))
             .centripetalScaling(0.0005);
 
     public static MecanumConstants driveConstants = new MecanumConstants()
@@ -51,10 +51,16 @@ public class Constants {
             .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD);
 
     public static Follower createFollower(HardwareMap hardwareMap) {
-        return new FollowerBuilder(followerConstants, hardwareMap)
+        FollowerBuilder followerBuilder;
+        followerBuilder = new FollowerBuilder(followerConstants, hardwareMap)
                 .pathConstraints(pathConstraints)
                 .mecanumDrivetrain(driveConstants)
-                .pinpointLocalizer(localizerConstants)
+                .pinpointLocalizer(localizerConstants);
-                .build();
+
+        followerBuilder.resetPinpoint();
+        followerBuilder.recalibrateIMU();
+
+        return followerBuilder.build();
+
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/FollowerBuilder.java

@@ -0,0 +1,106 @@
+package org.firstinspires.ftc.teamcode.pedroPathing;
+import com.pedropathing.drivetrain.Drivetrain;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.follower.FollowerConstants;
+import com.pedropathing.ftc.drivetrains.*;
+import com.pedropathing.ftc.localization.constants.DriveEncoderConstants;
+import com.pedropathing.ftc.localization.constants.OTOSConstants;
+import com.pedropathing.ftc.localization.constants.PinpointConstants;
+import com.pedropathing.ftc.localization.constants.ThreeWheelConstants;
+import com.pedropathing.ftc.localization.constants.ThreeWheelIMUConstants;
+import com.pedropathing.ftc.localization.constants.TwoWheelConstants;
+import com.pedropathing.ftc.localization.localizers.DriveEncoderLocalizer;
+import com.pedropathing.ftc.localization.localizers.OTOSLocalizer;
+import com.pedropathing.ftc.localization.localizers.PinpointLocalizer;
+import com.pedropathing.ftc.localization.localizers.ThreeWheelIMULocalizer;
+import com.pedropathing.ftc.localization.localizers.ThreeWheelLocalizer;
+import com.pedropathing.ftc.localization.localizers.TwoWheelLocalizer;
+import com.pedropathing.localization.Localizer;
+import com.pedropathing.paths.PathConstraints;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+/** This is the FollowerBuilder.
+ * It is used to create Followers with a specific drivetrain + localizer without having to use a full constructor
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ */
+public class FollowerBuilder {
+    private final FollowerConstants constants;
+    private PathConstraints constraints;
+    private final HardwareMap hardwareMap;
+    private Localizer localizer;
+    private Drivetrain drivetrain;
+
+    public FollowerBuilder(FollowerConstants constants, HardwareMap hardwareMap) {
+        this.constants = constants;
+        this.hardwareMap = hardwareMap;
+        constraints = PathConstraints.defaultConstraints;
+    }
+
+    public FollowerBuilder setLocalizer(Localizer localizer) {
+        this.localizer = localizer;
+        return this;
+    }
+
+    public FollowerBuilder driveEncoderLocalizer(DriveEncoderConstants lConstants) {
+        return setLocalizer(new DriveEncoderLocalizer(hardwareMap, lConstants));
+    }
+
+    public FollowerBuilder OTOSLocalizer(OTOSConstants lConstants) {
+        return setLocalizer(new OTOSLocalizer(hardwareMap, lConstants));
+    }
+
+    PinpointLocalizer pinpointLocalizer;
+    public FollowerBuilder pinpointLocalizer(PinpointConstants lConstants) {
+        pinpointLocalizer = new PinpointLocalizer(hardwareMap, lConstants);
+        return setLocalizer(pinpointLocalizer);
+    }
+
+    public void resetPinpoint(){
+        pinpointLocalizer.resetIMU();
+    }
+
+    public void recalibrateIMU(){
+        pinpointLocalizer.recalibrate();
+    }
+
+    public FollowerBuilder threeWheelIMULocalizer(ThreeWheelIMUConstants lConstants) {
+        return setLocalizer(new ThreeWheelIMULocalizer(hardwareMap, lConstants));
+    }
+
+    public FollowerBuilder threeWheelLocalizer(ThreeWheelConstants lConstants) {
+        return setLocalizer(new ThreeWheelLocalizer(hardwareMap, lConstants));
+    }
+
+    public FollowerBuilder twoWheelLocalizer(TwoWheelConstants lConstants) {
+        return setLocalizer(new TwoWheelLocalizer(hardwareMap, lConstants));
+    }
+
+    public FollowerBuilder setDrivetrain(Drivetrain drivetrain) {
+        this.drivetrain = drivetrain;
+        return this;
+    }
+
+    public FollowerBuilder mecanumDrivetrain(MecanumConstants mecanumConstants) {
+        return setDrivetrain(new Mecanum(hardwareMap, mecanumConstants));
+    }
+
+    @Deprecated
+    public FollowerBuilder mecanumExDrivetrain(MecanumConstants mecanumConstants) {
+        return setDrivetrain(new MecanumEx(hardwareMap, mecanumConstants));
+    }
+
+    public FollowerBuilder swerveDrivetrain(SwerveConstants swerveConstants, SwervePod... pods) {
+        return setDrivetrain(new Swerve(hardwareMap, swerveConstants, pods));
+    }
+
+    public FollowerBuilder pathConstraints(PathConstraints pathConstraints) {
+        this.constraints = pathConstraints;
+        PathConstraints.setDefaultConstraints(pathConstraints);
+        return this;
+    }
+
+    public Follower build() {
+        return new Follower(constants, localizer, drivetrain, constraints);
+    }
+}

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

@@ -161,6 +161,8 @@ public class TeleopV4 extends LinearOpMode {
 
             TELE.addData("Current Position", currentPose);
 
+            TELE.addData("Current LL Pipeline", turret.pipeline());
+
             TELE.update();
         }
 

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

@@ -34,7 +34,7 @@ public class NewShooterTest extends LinearOpMode {
 
     public static int flywheelVelo = 0;
     public static double hoodPos = 0.5;
-    public static double transferPower = -0.7;
+    public static double transferPower = -0.8;
 //    public static double turretPos = 0.51;
 
     @Override

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

@@ -140,11 +140,9 @@ public class Shooter {
                 break;
             case READ_OBELISK:
                 manualFlywheel = false;
-                turr.trackObelisk(
+                robot.setTurretPos(Turret.neutralPosition);
-                        (obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+
-                        (obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
+                turr.detectObelisk();
-                        follow.getHeading()
-                );
 
                 commander.getVeloPredictive(
                         (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
@@ -158,7 +156,7 @@ public class Shooter {
 
                 flywheelVelocity = commander.getPredictedRPM();
 
-                fly.manageFlywheel(flywheelVelocity);
+                fly.manageFlywheel(0);
                 fly.setF(voltage);
                 break;
 

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

@@ -216,6 +216,16 @@ public class SpindexerTransferIntake {
         desiredPattern = pattern;
     }
 
+    public void setDesiredPatternAuto(int id) {
+        if (id == 22){
+            desiredPattern = DesiredPattern.PGP;
+        } else if (id == 23){
+            desiredPattern = DesiredPattern.PPG;
+        } else {
+            desiredPattern = DesiredPattern.GPP;
+        }
+    }
+
     public void startSortedShoot() {
 
         shootOrder = buildShootOrder(
@@ -404,6 +414,8 @@ public class SpindexerTransferIntake {
 
             case SORTED:
 
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
+
                 switch (sortedIntakeStates) {
                     case NOTHING:
                         break;
@@ -424,14 +436,14 @@ public class SpindexerTransferIntake {
                                 ServoPositions.transferServo_out
                         );
                         robot.setIntakePower(1);
-                        robot.setTransferPower(-1);
+                        robot.setTransferPower(-0.7);
                         if (sortedStateTime() > 200) {
                             setSortedIntakeMode(SortedIntakeStates.INTAKE_1);
                         }
                         break;
                     case INTAKE_1:
                         robot.setIntakePower(1);
-                        robot.setTransferPower(-1);
+                        robot.setTransferPower(-0.7);
                         if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
                             NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
                             if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
@@ -458,8 +470,6 @@ public class SpindexerTransferIntake {
                         }
                         break;
                     case INTAKE_2:
-                        robot.setIntakePower(1);
-                        robot.setTransferPower(-1);
                         robot.setIntakePower(1);
                         robot.setTransferPower(-1);
                         if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
@@ -470,9 +480,9 @@ public class SpindexerTransferIntake {
                             ballTicks++;
                             if (ballTicks > 15) {
                                 if (greenTicks > 10){
-                                    ballColors[0] = BallStates.GREEN;
+                                    ballColors[1] = BallStates.GREEN;
                                 } else {
-                                    ballColors[0] = BallStates.PURPLE;
+                                    ballColors[1] = BallStates.PURPLE;
                                 }
                                 robot.setSpinPos(ServoPositions.spindexer_A3);
                                 setSortedIntakeMode(SortedIntakeStates.DELAY_2);
@@ -505,9 +515,9 @@ public class SpindexerTransferIntake {
                             ballTicks++;
                             if (ballTicks > 15) {
                                 if (greenTicks > 10){
-                                    ballColors[0] = BallStates.GREEN;
+                                    ballColors[2] = BallStates.GREEN;
                                 } else {
-                                    ballColors[0] = BallStates.PURPLE;
+                                    ballColors[2] = BallStates.PURPLE;
                                 }
                                 setSortedIntakeMode(SortedIntakeStates.REVERSE);
                                 ballTicks = 0;
@@ -525,7 +535,12 @@ public class SpindexerTransferIntake {
                 break;
             case SHOOT_SORTED:
 
-                robot.setTransferPower(commander.getTransferPow());
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
+                if (Shooter.manualFlywheel) {
+                    robot.setTransferPower(NewShooterTest.transferPower);
+                } else {
+                    robot.setTransferPower(commander.getTransferPow());
+                }
 
 
                 switch (shootState) {
@@ -697,7 +712,7 @@ public class SpindexerTransferIntake {
                                 ServoPositions.transferServo_out
                         );
                         robot.setIntakePower(1);
-                        robot.setTransferPower(-1);
+                        robot.setTransferPower(-0.7);
 
                         if (shootStateTime() > 250) {
 

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

@@ -16,7 +16,7 @@ public class Turret {
     Robot robot;
 
     private final double servoTicksPer180 = 0.58;
-    private final double neutralPosition = 0.51;
+    public static double neutralPosition = 0.51;
     private final double turretMin = 0.05;
     private final double turretMax = 0.95;
     public static boolean limelightUsed = true;
@@ -83,11 +83,12 @@ public class Turret {
                 pipeline = 2;
             }
         }
-        if (pipeline == 0){prevPipeline = 0;}
         if (pipeline != prevPipeline){
             robot.limelight.pipelineSwitch(pipeline);
         }
+        prevPipeline = pipeline;
     }
+    public int pipeline(){return prevPipeline;}
 
     public void trackObelisk(double dx, double dy, double h) {
 
@@ -107,7 +108,6 @@ public class Turret {
 
         robot.setTurretPos(servoAngle);
 
-
         detectObelisk();
 
     }
@@ -116,20 +116,14 @@ public class Turret {
         return obeliskID;
     }
 
-    private int detectObelisk() {
+    public void detectObelisk() {
-        switchPipeline(PipelineMode.OBELISK);
         result = robot.limelight.getLatestResult();
         if (result != null && result.isValid()) {
             List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
-            double prevTx  = -1000;
             for (LLResultTypes.FiducialResult fiducial : fiducials) {
-                double currentTx = fiducial.getTargetXDegrees();
+                obeliskID = fiducial.getFiducialId();
-                if (currentTx > prevTx){
-                    obeliskID = fiducial.getFiducialId();
-                }
             }
         }
-        return obeliskID;
     }
 
     public void manual (double pos) {