refactored pedro into libs

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java

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

@@ -0,0 +1,441 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
+import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
+import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
+
+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.BezierCurve;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.PathChain;
+import com.qualcomm.hardware.lynx.LynxModule;
+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.libs.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utils.Flywheel;
+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 com.pedropathing.util.Timer;
+
+import java.util.List;
+
+@Config
+@Autonomous (preselectTeleOp = "TeleopV3")
+public class Auto12BallPedroPathing extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    Flywheel flywheel;
+    Targeting targeting;
+    Targeting.Settings targetingSettings;
+    Follower follower;
+    Turret turret;
+    Spindexer spindexer;
+    Servos servos;
+    Timer opModeTimer, shootingTimer;
+
+    // Wait Times
+    public static double shootTime = 2;
+
+    // Extra Variables
+    public static double intakePower = 0.3;
+
+    // Initialize path state machine
+    private enum PathState {
+        DRIVE_SHOOT0,
+        WAIT_SHOOT0,
+        DRIVE_PICKUP1,
+        PICKUP1,
+        DRIVE_SHOOT1,
+        WAIT_SHOOT1,
+        DRIVE_PICKUP2,
+        PICKUP2,
+        DRIVE_SHOOT2,
+        WAIT_SHOOT2,
+        DRIVE_PICKUP3,
+        PICKUP3,
+        DRIVE_SHOOT3,
+        WAIT_SHOOT3
+    }
+    PathState pathState = PathState.DRIVE_SHOOT0;
+
+    // Poses
+    public static double startPoseX = 112, startPoseY = 132.5, startPoseH = -90;
+    public static double shoot0X = 106, shoot0Y = 106, shoot0H = -40;
+    public static double drivePickup1X = 102, drivePickup1Y = 82, drivePickup1H = 0;
+    public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0;
+    public static double shoot1X = 86, shoot1Y = 82, shoot1H = -80;
+    public static double drivePickup2ControlX = 91.69828844730904, drivePickup2ControlY = 66.724457099909;
+    public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0;
+    public static double pickup2X = 132, pickup2Y = 57, pickup2H = 0;
+    public static double shoot2ControlX = 86, shoot2ControlY = 57;
+    public static double shoot2X = 86, shoot2Y = 82, shoot2H = -90;
+    public static double drivePickup3ControlX = 97.97800291788306, drivePickup3ControlY = 50.10765863138859;
+    public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0;
+    public static double pickup3X = 132, pickup3Y = 34.5, pickup3H = 0;
+    public static double shoot3ControlX = 86, shoot3ControlY = 34.5;
+    public static double shoot3X = 84, shoot3Y = 102, shoot3H = -90;
+    Pose startPose;
+    Pose shoot0;
+    Pose drivePickup1;
+    Pose pickup1;
+    Pose shoot1;
+    Pose drivePickup2Control;
+    Pose drivePickup2;
+    Pose pickup2;
+    Pose shoot2Control;
+    Pose shoot2;
+    Pose drivePickup3Control;
+    Pose drivePickup3;
+    Pose pickup3;
+    Pose shoot3Control;
+    Pose shoot3;
+    private void initializePoses(){
+        startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH));
+        shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
+        drivePickup1 = new Pose(drivePickup1X, drivePickup1Y, Math.toRadians(drivePickup1H));
+        pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H));
+        shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H));
+        drivePickup2Control = new Pose(drivePickup2ControlX, drivePickup2ControlY);
+        drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H));
+        pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H));
+        shoot2Control = new Pose(shoot2ControlX, shoot2ControlY);
+        shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H));
+        drivePickup3Control = new Pose(drivePickup3ControlX, drivePickup3ControlY);
+        drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H));
+        pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H));
+        shoot3Control = new Pose(shoot3ControlX, shoot3ControlY);
+        shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H));
+    } // add poses to void
+
+    //Building Paths
+    PathChain startPose_shoot0;
+    PathChain shoot0_drivePickup1;
+    PathChain drivePickup1_pickup1;
+    PathChain pickup1_shoot1;
+    PathChain shoot1_drivePickup2;
+    PathChain drivePickup2_pickup2;
+    PathChain pickup2_shoot2;
+    PathChain shoot2_drivePickup3;
+    PathChain drivePickup3_pickup3;
+    PathChain pickup3_shoot3;
+    private void buildPaths(){
+        startPose_shoot0 = follower.pathBuilder()
+                .addPath(new BezierLine(startPose, shoot0))
+                .setLinearHeadingInterpolation(startPose.getHeading(), shoot0.getHeading())
+                .build();
+
+        shoot0_drivePickup1 = follower.pathBuilder()
+                .addPath(new BezierLine(shoot0, drivePickup1))
+                .setLinearHeadingInterpolation(shoot0.getHeading(), drivePickup1.getHeading())
+                .build();
+
+        drivePickup1_pickup1 = follower.pathBuilder()
+                .addPath(new BezierLine(drivePickup1, pickup1))
+                .setTangentHeadingInterpolation()
+                .build();
+
+        pickup1_shoot1 = follower.pathBuilder()
+                .addPath(new BezierLine(pickup1, shoot1))
+                .setLinearHeadingInterpolation(pickup1.getHeading(), shoot1.getHeading())
+                .build();
+
+        shoot1_drivePickup2 = follower.pathBuilder()
+                .addPath(new BezierCurve(shoot1, drivePickup2Control, drivePickup2))
+                .setLinearHeadingInterpolation(shoot1.getHeading(), drivePickup2.getHeading())
+                .build();
+
+        drivePickup2_pickup2 = follower.pathBuilder()
+                .addPath(new BezierLine(drivePickup2, pickup2))
+                .setConstantHeadingInterpolation(pickup2.getHeading())
+                .build();
+
+        pickup2_shoot2 = follower.pathBuilder()
+                .addPath(new BezierCurve(pickup2, shoot2Control, shoot2))
+                .setLinearHeadingInterpolation(pickup2.getHeading(), shoot2.getHeading())
+                .build();
+
+        shoot2_drivePickup3 = follower.pathBuilder()
+                .addPath(new BezierCurve(shoot2, drivePickup3Control, drivePickup3))
+                .setLinearHeadingInterpolation(shoot2.getHeading(), drivePickup3.getHeading())
+                .build();
+
+        drivePickup3_pickup3 = follower.pathBuilder()
+                .addPath(new BezierLine(drivePickup3, pickup3))
+                .setTangentHeadingInterpolation()
+                .build();
+
+        pickup3_shoot3 = follower.pathBuilder()
+                .addPath(new BezierCurve(pickup3, shoot3Control, shoot3))
+                .setLinearHeadingInterpolation(pickup3.getHeading(), shoot3.getHeading())
+                .build();
+    }
+
+    //Path State Machine
+    private boolean startAuto = true;
+    private double timeStamp = 0;
+    private void pathStateMachine(){
+        double currentTime = (double) System.currentTimeMillis() / 1000;
+        switch(pathState){
+            case DRIVE_SHOOT0:
+                if (startAuto){
+                    follower.followPath(startPose_shoot0, true);
+                    startAuto = false;
+                }
+                if (!follower.isBusy()){
+                    pathState = PathState.WAIT_SHOOT0;
+                    timeStamp = currentTime;
+//                    spindexer.prepareShootAllContinous();
+                }
+                break;
+            case WAIT_SHOOT0:
+                if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){
+//                    spindexer.resetSpindexer();
+                    pathState = PathState.DRIVE_PICKUP1;
+                    follower.followPath(shoot0_drivePickup1, true);
+                }
+                break;
+            case DRIVE_PICKUP1:
+                if (!follower.isBusy()){
+                    pathState = PathState.PICKUP1;
+                    follower.followPath(drivePickup1_pickup1, intakePower, false);
+                }
+                break;
+            case PICKUP1:
+                if (!follower.isBusy()){
+                    pathState = PathState.DRIVE_SHOOT1;
+                    follower.followPath(pickup1_shoot1, true);
+                }
+                break;
+            case DRIVE_SHOOT1:
+                if (!follower.isBusy()){
+                    pathState = PathState.WAIT_SHOOT1;
+                    timeStamp = currentTime;
+//                    spindexer.prepareShootAllContinous();
+                }
+                break;
+            case WAIT_SHOOT1:
+                if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){
+//                    spindexer.resetSpindexer();
+                    pathState = PathState.DRIVE_PICKUP2;
+                    follower.followPath(shoot1_drivePickup2, true);
+                }
+                break;
+            case DRIVE_PICKUP2:
+                if (!follower.isBusy()){
+                    pathState = PathState.PICKUP2;
+                    follower.followPath(drivePickup2_pickup2, intakePower, false);
+                }
+                break;
+            case PICKUP2:
+                if (!follower.isBusy()){
+                    pathState = PathState.DRIVE_SHOOT2;
+                    follower.followPath(pickup2_shoot2, true);
+                }
+                break;
+            case DRIVE_SHOOT2:
+                if (!follower.isBusy()){
+                    pathState = PathState.WAIT_SHOOT2;
+                    timeStamp = currentTime;
+//                    spindexer.prepareShootAllContinous();
+                }
+                break;
+            case WAIT_SHOOT2:
+                if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){
+//                    spindexer.resetSpindexer();
+                    pathState = PathState.DRIVE_PICKUP3;
+                    follower.followPath(shoot2_drivePickup3, true);
+                }
+                break;
+            case DRIVE_PICKUP3:
+                if (!follower.isBusy()){
+                    pathState = PathState.PICKUP3;
+                    follower.followPath(drivePickup3_pickup3, intakePower, false);
+                }
+                break;
+            case PICKUP3:
+                if (!follower.isBusy()){
+                    pathState = PathState.DRIVE_SHOOT3;
+                    follower.followPath(pickup3_shoot3, true);
+                }
+                break;
+            case DRIVE_SHOOT3:
+                if (!follower.isBusy()){
+                    pathState = PathState.WAIT_SHOOT3;
+//                    spindexer.prepareShootAllContinous();
+                }
+                break;
+            case WAIT_SHOOT3:
+                if (spindexer.shootAllComplete()){
+//                    spindexer.resetSpindexer();
+                    TELE.addLine("Done Auto");
+                    TELE.update();
+                }
+                break;
+            default:
+                break;
+        }
+        TELE.update();
+    }
+
+    private boolean driveToShoot(){
+        return pathState == PathState.DRIVE_SHOOT0 ||
+                pathState == PathState.DRIVE_SHOOT1 ||
+                pathState == PathState.DRIVE_SHOOT2 ||
+                pathState == PathState.DRIVE_SHOOT3;
+    }
+
+    private double adjustXPoseBasedOnAlliance(double pose){
+        return (144-pose);
+    }
+
+    private double adjustHeadingBasedOnAlliance(double heading){
+        heading = 180 - heading;
+        while (heading > 180) {heading-=360;}
+        while (heading <= -180) {heading+=360;}
+        return heading;
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+        List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        flywheel = new Flywheel(hardwareMap);
+        targeting = new Targeting();
+        targetingSettings = new Targeting.Settings(0,0);
+        follower = Constants.createFollower(hardwareMap);
+        follower.setStartingPose(new Pose(72,72,0));
+        turret = new Turret(robot, TELE, robot.limelight);
+        spindexer = new Spindexer(hardwareMap);
+        servos = new Servos(hardwareMap);
+        opModeTimer = new Timer();
+        shootingTimer = new Timer();
+
+        robot.light.setPosition(Color.LightRed);
+
+        boolean initializeRobot = false;
+        while (opModeInInit()){
+            follower.update();
+
+            if (gamepad1.crossWasPressed() && !initializeRobot){
+                Color.redAlliance = !Color.redAlliance;
+                if (Color.redAlliance){
+                    robot.light.setPosition(Color.LightRed);
+                } else {
+                    robot.light.setPosition(Color.LightBlue);
+                }
+
+                startPoseX = adjustXPoseBasedOnAlliance(startPoseX);
+                startPoseH = adjustHeadingBasedOnAlliance(startPoseH);
+
+                shoot0X = adjustXPoseBasedOnAlliance(shoot0X);
+                shoot0H = adjustHeadingBasedOnAlliance(shoot0H);
+
+                drivePickup1X = adjustXPoseBasedOnAlliance(drivePickup1X);
+                drivePickup1H = adjustHeadingBasedOnAlliance(drivePickup1H);
+
+                pickup1X = adjustXPoseBasedOnAlliance(pickup1X);
+                pickup1H = adjustHeadingBasedOnAlliance(pickup1H);
+
+                shoot1X = adjustXPoseBasedOnAlliance(shoot1X);
+                shoot1H = adjustHeadingBasedOnAlliance(shoot1H);
+
+                drivePickup2ControlX = adjustXPoseBasedOnAlliance(drivePickup2ControlX);
+
+                drivePickup2X = adjustXPoseBasedOnAlliance(drivePickup2X);
+                drivePickup2H = adjustHeadingBasedOnAlliance(drivePickup2H);
+
+                pickup2X = adjustXPoseBasedOnAlliance(pickup2X);
+                pickup2H = adjustHeadingBasedOnAlliance(pickup2H);
+
+                shoot2ControlX = adjustXPoseBasedOnAlliance(shoot2ControlX);
+
+                shoot2X = adjustXPoseBasedOnAlliance(shoot2X);
+                shoot2H = adjustHeadingBasedOnAlliance(shoot2H);
+
+                drivePickup3ControlX = adjustXPoseBasedOnAlliance(drivePickup3ControlX);
+
+                drivePickup3X = adjustXPoseBasedOnAlliance(drivePickup3X);
+                drivePickup3H = adjustHeadingBasedOnAlliance(drivePickup3H);
+
+                pickup3X = adjustXPoseBasedOnAlliance(pickup3X);
+                pickup3H = adjustHeadingBasedOnAlliance(pickup3H);
+
+                shoot3ControlX = adjustXPoseBasedOnAlliance(shoot3ControlX);
+
+                shoot3X = adjustXPoseBasedOnAlliance(shoot3X);
+                shoot3H = adjustHeadingBasedOnAlliance(shoot3H);
+            }
+
+            if (gamepad1.triangleWasPressed()){
+                initializeRobot = true;
+            }
+
+            if (initializeRobot){
+                initializePoses();
+                follower.setPose(startPose);
+                buildPaths();
+                sleep(2000);
+
+                turret.setTurret(turrDefault);
+                servos.setSpinPos(spinStartPos);
+            }
+
+            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.update();
+        }
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        robot.transfer.setPower(1);
+        limelightUsed = false;
+        opModeTimer.resetTimer();
+
+        while (opModeIsActive()){
+            follower.update();
+            pathStateMachine();
+            Pose currentPose = follower.getPose();
+//            teleStartPoseX = currentPose.getX();
+//            teleStartPoseY = currentPose.getY();
+//            teleStartPoseH = Math.toDegrees(currentPose.getHeading());
+
+//            turret.trackGoal(currentPose);
+//            targetingSettings = targeting.calculateSettings(currentPose.getX(), currentPose.getY(), currentPose.getHeading(), 0, turretInterpolate);
+//
+//            double voltage = robot.voltage.getVoltage();
+//            flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
+//            flywheel.manageFlywheel(targetingSettings.flywheelRPM);
+//            servos.setHoodPos(targetingSettings.hoodAngle);
+//
+//            if (driveToShoot()){
+//                servos.setSpinPos(spinStartPos);
+//            } else {
+//                spindexer.processIntake();
+//            }
+
+//            TELE.addData("X:", currentPose.getX());
+//            TELE.addData("Y:", currentPose.getY());
+//            TELE.addData("H:", currentPose.getHeading());
+//            TELE.update();
+        }
+    }
+}

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

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.autonomous;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos0;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3;
@@ -89,10 +90,6 @@ public class Auto_LT_Close extends LinearOpMode {
     public static double intake1GateTime = 3.3;
     public static double lastShootTime = 27;
 
-    public static double openGateX = 26;
-    public static double openGateY = 48;
-    public  static double openGateH = Math.toRadians(155);
-
     Robot robot;
     MultipleTelemetry TELE;
     MecanumDrive drive;
@@ -124,6 +121,8 @@ public class Auto_LT_Close extends LinearOpMode {
     double xShootGate, yShootGate, hShootGate;
     double xLeave, yLeave, hLeave;
     double xLeaveGate, yLeaveGate, hLeaveGate;
+    double openGateCloseX = 0, openGateCloseY = 0, openGateCloseH = 0;
+    double openGateMiddleX = 0, openGateMiddleY = 0, openGateMiddleH = 0;
 
     int ballCycles = 3;
     int prevMotif = 0;
@@ -134,6 +133,7 @@ public class Auto_LT_Close extends LinearOpMode {
     double obeliskTurrPos3 = 0.0;
     double waitToPickupGate = 0;
     double obeliskTurrPosAutoStart = 0;
+    boolean limelightStart = false;
 
     // initialize path variables here
     TrajectoryActionBuilder shoot0 = null;
@@ -179,13 +179,14 @@ public class Auto_LT_Close extends LinearOpMode {
 
         servos.setTransferPos(transferServo_out);
         limelightUsed = false;
+//        Spindexer.teleop = false;
 
         robot.light.setPosition(1);
 
         hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU();
 
         while (opModeInInit()) {
-            if (limelightUsed && !gateCycle){
+            if (limelightUsed && !gateCycle && limelightStart){
                 Actions.runBlocking(
                         autoActions.detectObelisk(
                                 0.1,
@@ -207,14 +208,6 @@ public class Auto_LT_Close extends LinearOpMode {
                 }
             }
 
-            if (!gateCycle) {
-                turret.pipelineSwitch(1);
-            } else if (redAlliance) {
-                turret.pipelineSwitch(4);
-            } else {
-                turret.pipelineSwitch(2);
-            }
-
             if (gateCycle) {
                 servos.setHoodPos(hoodGate0Start);
             } else {
@@ -249,8 +242,10 @@ public class Auto_LT_Close extends LinearOpMode {
             if (gamepad2.squareWasPressed()) {
 
                 drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
+                sleep(100);
                 robot.limelight.start();
                 limelightUsed = true;
+                limelightStart = true;
 
                 gamepad2.rumble(500);
             }
@@ -258,6 +253,12 @@ public class Auto_LT_Close extends LinearOpMode {
             if (redAlliance) {
                 robot.light.setPosition(0.28);
 
+                if (gateCycle){
+                    turret.pipelineSwitch(1);
+                } else {
+                    turret.pipelineSwitch(4);
+                }
+
                 // ---- FIRST SHOT ----
                 x1 = rx1;
                 y1 = ry1;
@@ -307,6 +308,14 @@ public class Auto_LT_Close extends LinearOpMode {
                 pickupGateBY = rPickupGateBY;
                 pickupGateBH = rPickupGateBH;
 
+                openGateCloseX = rOpenGateCloseX;
+                openGateCloseY = rOpenGateCloseY;
+                openGateCloseH = rOpenGateCloseH;
+
+                openGateMiddleX = rOpenGateMiddleX;
+                openGateMiddleY = rOpenGateMiddleY;
+                openGateMiddleH = rOpenGateMiddleH;
+
                 obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0;
                 obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
@@ -314,6 +323,12 @@ public class Auto_LT_Close extends LinearOpMode {
             } else {
                 robot.light.setPosition(0.6);
 
+                if (gateCycle){
+                    turret.pipelineSwitch(5);
+                } else {
+                    turret.pipelineSwitch(2);
+                }
+
                 // ---- FIRST SHOT ----
                 x1 = bx1;
                 y1 = by1;
@@ -363,25 +378,34 @@ public class Auto_LT_Close extends LinearOpMode {
                 pickupGateBY = bPickupGateBY;
                 pickupGateBH = bPickupGateBH;
 
-                obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0;
+                openGateCloseX = bOpenGateCloseX;
+                openGateCloseY = bOpenGateCloseY;
+                openGateCloseH = bOpenGateCloseH;
+
+                openGateMiddleX = bOpenGateMiddleX;
+                openGateMiddleY = bOpenGateMiddleY;
+                openGateMiddleH = bOpenGateMiddleH;
+
+                obeliskTurrPosAutoStart = turrDefault + blueObeliskTurrPos0;
                 obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
             }
 
-            if (gateCycle) {
-                shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
-                        .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0));
-            } else {
+//            if (gateCycle) {
+//                shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+//                        .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0));
+//            } else {
                 shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                         .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
-            }
+//            }
 
             if (gateCycle) {
                 pickup2 = shoot0.endTrajectory().fresh()
                         .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
                         .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
-                                new TranslationalVelConstraint(pickupStackGateSpeed));
+                                new TranslationalVelConstraint(pickupStackGateSpeed))
+                        .strafeToLinearHeading(new Vector2d(openGateMiddleX, openGateMiddleY), Math.toRadians(openGateMiddleH));
             } else {
                 pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
                         .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
@@ -389,13 +413,14 @@ public class Auto_LT_Close extends LinearOpMode {
                                 new TranslationalVelConstraint(pickup1Speed));
             }
 
-            if (gateCycle&& withPartner) {
+//            if (gateCycle && withPartner) {
+//                shoot2 = pickup2.endTrajectory().fresh()
+//                        .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH))
+//                        .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH));
+//            } else
+            if (gateCycle) {
                 shoot2 = pickup2.endTrajectory().fresh()
-                        .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH))
-                        .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH));
-            } else if (gateCycle) {
-                shoot2 = pickup2.endTrajectory().fresh()
-                        .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
+                        .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
             } else if (ballCycles < 3) {
                 shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
                         .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
@@ -417,10 +442,11 @@ public class Auto_LT_Close extends LinearOpMode {
 
 
             if (gateCycle) {
-                pickup1 = gateCycleShoot.endTrajectory().fresh()
+                pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
                         .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
                         .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
-                                new TranslationalVelConstraint(pickupStackGateSpeed));
+                                new TranslationalVelConstraint(pickupStackGateSpeed))
+                        .strafeToLinearHeading(new Vector2d(openGateCloseX, openGateCloseY), Math.toRadians(openGateCloseH));
             } else {
                 pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
                         .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
@@ -431,7 +457,7 @@ public class Auto_LT_Close extends LinearOpMode {
 
             if (gateCycle) {
                 shoot1 = pickup1.endTrajectory().fresh()
-                        .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
+                        .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
             } else if (ballCycles < 2) {
                 shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
                         .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
@@ -477,30 +503,13 @@ public class Auto_LT_Close extends LinearOpMode {
 
 
             if (gateCycle) {
-                startAutoGate();
+                startAuto();
                 shoot(0.501, 0.501, 0.501);
-                cycleStackMiddleGate();
+                cycleStackClose();
                 shoot(0.501,0.501, 0.501);
-
-                while (getRuntime() - stamp < endGateTime) {
-                    cycleGateIntake();
-                    if (getRuntime() - stamp < lastShootTime) {
-                        cycleGateShoot();
-                        shoot(0.501, 0.501, 0.501);
-                    }
-                }
-                cycleStackCloseIntakeGate();
-
-                if (getRuntime() - stamp < lastShootTime) {
-                    cycleStackCloseShootGate();
-                }
-
+                cycleStackMiddle();
                 shoot(0.501, 0.501, 0.501);
-
             } else {
-
-
-
                 startAuto();
                 shoot(0.501, 0.501,0.501);
 

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

@@ -1,6 +1,11 @@
 package org.firstinspires.ftc.teamcode.autonomous;
 
 import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
+import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarAH;
+import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarAY;
+import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBH;
+import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBX;
+import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBY;
 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.blueObeliskTurrPos1;
@@ -63,8 +68,8 @@ public class Auto_LT_Far extends LinearOpMode {
     boolean stack3 = true;
     double xStackPickupA, yStackPickupA, hStackPickupA;
     double xStackPickupB, yStackPickupB, hStackPickupB;
-    public static int pickupStackSpeed = 17;
-    public static int pickupGateSpeed = 25;
+    public static int pickupStackSpeed = 12;
+    public static int pickupGateSpeed = 30;
     int prevMotif = 0;
     public static double spindexerSpeedIncrease = 0.014;
     public static double shootAllTime = 2;
@@ -74,8 +79,8 @@ public class Auto_LT_Far extends LinearOpMode {
     public static double shootStackTime = 2;
     public static double shootGateTime = 2.5;
     public static double colorSenseTime = 1;
-    public static double intakeStackTime = 4.5;
-    public static double intakeGateTime = 8;
+    public static double intakeStackTime = 5;
+    public static double intakeGateTime = 3.75;
     double obeliskTurrPos1 = 0.0;
     double obeliskTurrPos2 = 0.0;
     double obeliskTurrPos3 = 0.0;
@@ -117,12 +122,10 @@ public class Auto_LT_Far extends LinearOpMode {
         robot.limelight.pipelineSwitch(1);
 
         turret = new Turret(robot, TELE, robot.limelight);
+        limelightUsed = false;
 
-        turret.setTurret(turrDefault);
 
-        servos.setSpinPos(spinStartPos);
-
-        servos.setTransferPos(transferServo_out);
+//        Spindexer.teleop = false;
 
         while (opModeInInit()) {
 
@@ -164,17 +167,17 @@ public class Auto_LT_Far extends LinearOpMode {
                 yShoot = rShootY;
                 hShoot = rShootH;
 
-                xStackPickupA = rStackPickupAX;
-                yStackPickupA = rStackPickupAY;
-                hStackPickupA = rStackPickupAH;
+                xStackPickupA = rStackPickupFarAX;
+                yStackPickupA = rStackPickupFarAY;
+                hStackPickupA = rStackPickupFarAH;
 
-                xStackPickupB = rStackPickupBX;
-                yStackPickupB = rStackPickupBY;
-                hStackPickupB = rStackPickupBH;
+                xStackPickupB = rStackPickupFarBX;
+                yStackPickupB = rStackPickupFarBY;
+                hStackPickupB = rStackPickupFarBH;
 
-                pickupGateX = rPickupGateX;
-                pickupGateY = rPickupGateY;
-                pickupGateH = rPickupGateH;
+                pickupGateX = rPickupGateXA;
+                pickupGateY = rPickupGateYA;
+                pickupGateH = rPickupGateHA;
 
                 obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
@@ -184,9 +187,14 @@ public class Auto_LT_Far extends LinearOpMode {
                 if (gamepad2.squareWasPressed()){
                     turret.pipelineSwitch(4);
                     robot.limelight.start();
-                    drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
-
                     gamepad2.rumble(500);
+                    sleep(1000);
+                    turret.setTurret(turrDefault);
+
+                    servos.setSpinPos(spinStartPos);
+                    limelightUsed = true;
+
+                    servos.setTransferPos(transferServo_out);
                 }
             } else {
                 robot.light.setPosition(0.6);
@@ -203,17 +211,17 @@ public class Auto_LT_Far extends LinearOpMode {
                 yShoot = bShootY;
                 hShoot = bShootH;
 
-                xStackPickupA = bStackPickupAX;
-                yStackPickupA = bStackPickupAY;
-                hStackPickupA = bStackPickupAH;
+                xStackPickupA = bStackPickupFarAX;
+                yStackPickupA = bStackPickupFarAY;
+                hStackPickupA = bStackPickupFarAH;
 
-                xStackPickupB = bStackPickupBX;
-                yStackPickupB = bStackPickupBY;
-                hStackPickupB = bStackPickupBH;
+                xStackPickupB = bStackPickupFarBX;
+                yStackPickupB = bStackPickupFarBY;
+                hStackPickupB = bStackPickupFarBH;
 
-                pickupGateX = bPickupGateX;
-                pickupGateY = bPickupGateY;
-                pickupGateH = bPickupGateH;
+                pickupGateX = bPickupGateXA;
+                pickupGateY = bPickupGateYA;
+                pickupGateH = bPickupGateHA;
 
                 obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
@@ -223,9 +231,14 @@ public class Auto_LT_Far extends LinearOpMode {
                 if (gamepad2.squareWasPressed()){
                     turret.pipelineSwitch(2);
                     robot.limelight.start();
-                    drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
-
                     gamepad2.rumble(500);
+                    sleep(1000);
+                    turret.setTurret(turrDefault);
+                    limelightUsed = true;
+
+                    servos.setSpinPos(spinStartPos);
+
+                    servos.setTransferPos(transferServo_out);
                 }
             }
 
@@ -244,19 +257,16 @@ public class Auto_LT_Far extends LinearOpMode {
                     .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))
-                    .waitSeconds(0.2)
-                    .strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB))
-                    .strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC),
+                    .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);
             TELE.addData("Turret Default", turrDefault);
+            TELE.addData("Limelight On?",limelightUsed);
             TELE.addData("Gate Cycle?", gatePickup);
             TELE.addData("Pickup Stack?", stack3);
             TELE.addData("Start Position", autoStart);
@@ -275,11 +285,17 @@ public class Auto_LT_Far extends LinearOpMode {
             robot.transfer.setPower(1);
 
             startAuto();
-            shoot();
+            if (redAlliance){
+                shoot(autoStartRX, autoStartRY, autoStartRH);
+
+            } else {
+                shoot(autoStartBX, autoStartBY, autoStartBH);
+
+            }
 
             if (stack3){
                 cycleStackFar();
-                shoot();
+                shoot(xShoot, yShoot, hShoot);
             }
 
             while (gatePickup && getRuntime() - stamp < endAutoTime){
@@ -288,10 +304,7 @@ public class Auto_LT_Far extends LinearOpMode {
                     break;
                 }
                 cycleGatePrepareShoot();
-                if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){
-                    break;
-                }
-                shoot();
+                shoot(xShoot, yShoot, hShoot);
             }
 
             if (gatePickup || stack3){
@@ -318,28 +331,46 @@ public class Auto_LT_Far extends LinearOpMode {
 
     }
 
-    void shoot(){
+    void shoot(double x, double y, double z){
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, 0.501, 0.501, 0.501)
+                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, x, y, z)
                 )
 
         );
     }
 
     void startAuto(){
-        Actions.runBlocking(
-                new ParallelAction(
-                        autoActions.manageShooterAuto(
-                                flywheel0Time,
-                                0.501,
-                                0.501,
-                                0.501,
-                                true
-                        )
 
-                )
-        );
+        if (redAlliance){
+            Actions.runBlocking(
+                    new ParallelAction(
+                            autoActions.manageShooterAuto(
+                                    flywheel0Time,
+                                    autoStartRX,
+                                    autoStartRY,
+                                    autoStartRH,
+                                    true
+                            )
+
+                    )
+            );
+
+        } else {
+            Actions.runBlocking(
+                    new ParallelAction(
+                            autoActions.manageShooterAuto(
+                                    flywheel0Time,
+                                    autoStartBX,
+                                    autoStartBY,
+                                    autoStartBH,
+                                    true
+                            )
+
+                    )
+            );
+        }
+
     }
 
     void leave3Ball(){
@@ -361,34 +392,17 @@ public class Auto_LT_Far extends LinearOpMode {
                                 xStackPickupB,
                                 yStackPickupB,
                                 hStackPickupB
-                        ),
-                        autoActions.detectObelisk(
-                                intakeStackTime,
-                                xStackPickupB,
-                                yStackPickupB,
-                                posXTolerance,
-                                posYTolerance,
-                                obeliskTurrPos3
                         )
-
                 )
         );
-
-        motif = turret.getObeliskID();
-
-        if (motif == 0) motif = 22;
-        prevMotif = motif;
-
+        servos.setSpinPos(spinStartPos);
+        spindexer.setIntakePower(-0.1);
         Actions.runBlocking(
                 new ParallelAction(
                         shoot3.build(),
-                        autoActions.prepareShootAll(
-                                colorSenseTime,
-                                shootStackTime,
-                                motif,
-                                xShoot,
-                                yShoot,
-                                hShoot)
+                        autoActions.manageShooterAuto(
+                                shootStackTime,xShoot, yShoot, hShoot, false
+                        )
                 )
         );
     }
@@ -398,39 +412,26 @@ public class Auto_LT_Far extends LinearOpMode {
                 new ParallelAction(
                         pickupGate.build(),
                         autoActions.intake(
-                                intakeStackTime,
-                                pickupGateX,
-                                pickupGateY,
-                                pickupGateH
-                        ),
-                        autoActions.detectObelisk(
                                 intakeGateTime,
                                 pickupGateX,
                                 pickupGateY,
-                                posXTolerance,
-                                posYTolerance,
-                                obeliskTurrPos3
+                                pickupGateH
                         )
                 )
         );
-
-        motif = turret.getObeliskID();
-
-        if (motif == 0) motif = prevMotif;
-        prevMotif = motif;
     }
 
     void cycleGatePrepareShoot(){
+        spindexer.setIntakePower(-0.1);
         Actions.runBlocking(
                 new ParallelAction(
                         shootGate.build(),
-                        autoActions.prepareShootAll(
-                                colorSenseTime,
-                                shootGateTime,
-                                motif,
+                        autoActions.manageShooterAuto(
+                               shootGateTime,
                                 xShoot,
                                 yShoot,
-                                hShoot
+                                hShoot,
+                                false
                         )
                 )
         );

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

@@ -18,6 +18,8 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
 import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.Pose2d;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.NormalizedRGBA;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
@@ -54,8 +56,8 @@ public class AutoActions {
     public static double firstSpindexShootPos = spinStartPos;
     private boolean shootForward = true;
     public int motif = 0;
-    double spinEndPos = ServoPositions.spinEndPos;
-
+    double spinEndPos = 0.95;
+    private boolean intaking = false;
     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;
@@ -427,8 +429,10 @@ public class AutoActions {
 
                 if ((System.currentTimeMillis() - stamp) > (time * 1000)) {
                     servos.setSpinPos(spindexer_intakePos1);
+                    intaking = false;
                     return false;
                 } else {
+                    intaking = true;
                     return true;
                 }
             }
@@ -523,7 +527,7 @@ public class AutoActions {
 
 
                 drive.updatePoseEstimate();
-                Pose2d currentPose = drive.localizer.getPose();
+                Pose2d currentPose = null; //drive.localizer.getPose();
 
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
@@ -540,10 +544,10 @@ public class AutoActions {
 
                 ticker++;
 
-                double robotX = currentPose.position.x;
-                double robotY = currentPose.position.y;
+                double robotX = 0.0;//currentPose.position.x;
+                double robotY = 0.0;//currentPose.position.y;
 
-                double robotHeading = currentPose.heading.toDouble();
+                double robotHeading = 0.0;//currentPose.heading.toDouble();
 
                 double goalX = -15;
                 double goalY = 0;
@@ -552,11 +556,11 @@ public class AutoActions {
                 double dy = robotY - goalY;  // delta y from robot to goal
 
 
-                Pose2d deltaPose;
+                Pose deltaPose;
                 if (posX != 0.501) {
-                    deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
+                    deltaPose = new Pose(posX, posY, Math.toRadians(posH));
                 } else {
-                    deltaPose = new Pose2d(dx, dy, robotHeading);
+                    deltaPose = new Pose(dx, dy, robotHeading);
                 }
                 Turret.limelightUsed = true;
 
@@ -644,9 +648,9 @@ public class AutoActions {
 
                 double dx = robotX - goalX;  // delta x from robot to goal
                 double dy = robotY - goalY;  // delta y from robot to goal
-                Pose2d deltaPose;
+                Pose deltaPose;
                 if (turr == 0.501) {
-                    deltaPose = new Pose2d(dx, dy, robotHeading);
+                    deltaPose = new Pose(dx, dy, robotHeading);
                     if (!detectingObelisk) {
                         turret.trackGoal(deltaPose);
                     }
@@ -672,6 +676,44 @@ public class AutoActions {
             }
         };
     }
+
+    public Action ShakeDrivetrain(
+            double time
+    ){
+        return new Action() {
+            int ticker = 0;
+            double stamp = 0;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                if (ticker == 0){
+                    stamp = System.currentTimeMillis();
+                }
+                ticker++;
+
+                double currentStamp = System.currentTimeMillis();
+                if (currentStamp - stamp < time*1000 && (intaking || ticker < 50)) {
+                    if (ticker % 10000 < 5000) {
+                        robot.frontLeft.setPower(0.5);
+                        robot.backLeft.setPower(0.5);
+                        robot.frontRight.setPower(0.5);
+                        robot.backRight.setPower(0.5);
+                    } else {
+                        robot.frontLeft.setPower(-0.5);
+                        robot.backLeft.setPower(-0.5);
+                        robot.frontRight.setPower(-0.5);
+                        robot.backRight.setPower(-0.5);
+                    }
+                    return true;
+                } else {
+                    robot.frontLeft.setPower(0);
+                    robot.backLeft.setPower(0);
+                    robot.frontRight.setPower(0);
+                    robot.backRight.setPower(0);
+                    return false;
+                }
+            }
+        };
+    }
 }
 
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java

@@ -72,6 +72,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.TranslationalVelConstraint;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -489,7 +490,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode {
 
                 double dx = robotX - goalX;  // delta x from robot to goal
                 double dy = robotY - goalY;  // delta y from robot to goal
-                Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
+                Pose deltaPose = new Pose(dx, dy, robotHeading);
 
                 double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java

@@ -77,6 +77,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.TranslationalVelConstraint;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -628,7 +629,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode {
 
                 double dx = robotX - goalX;  // delta x from robot to goal
                 double dy = robotY - goalY;  // delta y from robot to goal
-                Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
+                Pose deltaPose = new Pose(dx, dy, robotHeading);
 
                 double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 

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

@@ -7,23 +7,29 @@ public class Back_Poses {
     public static double rLeaveX = 90, rLeaveY = 50, rLeaveH = 50.1;
     public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50;
 
-    public static double rShootX = 100, rShootY = 55, rShootH = 90;
-    public static double bShootX = 100, bShootY = -55, bShootH = -90;
+    public static double rShootX = 100, rShootY = 60, rShootH = 125.2;
+    public static double bShootX = 100, bShootY = -60, bShootH = -125.2;
 
-    public static double rStackPickupAX = 73, rStackPickupAY = 51, rStackPickupAH = 140;
-    public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
+    public static double rStackPickupFarAX = 75, rStackPickupFarAY = 45, rStackPickupFarAH = 150;
+    public static double bStackPickupFarAX = 75, bStackPickupFarAY = -45, bStackPickupFarAH = -150;
 
-    public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1;
-    public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1;
+    public static double rStackPickupFarBX = 45, rStackPickupFarBY = 80, rStackPickupFarBH = 145.1;
+    public static double bStackPickupFarBX = 45, bStackPickupFarBY = -80, bStackPickupFarBH = -145.1;
 
-    public static double rPickupGateX = 50, rPickupGateY = 83, rPickupGateH = 140;
-    public static double bPickupGateX = 70, bPickupGateY = -90, bPickupGateH = -140;
-    public static double pickupGateXB = 84, pickupGateYB = 76, pickupGateHB = 140;
-    public static double pickupGateXC = 50, pickupGateYC = 83, pickupGateHC = 190;
+    public static double rStackPickupMiddleAX = 55, rStackPickupMiddleAY = 39, rStackPickupMiddleAH = 145;
+    public static double bStackPickupMiddleAX = 55, bStackPickupMiddleAY = -39, bStackPickupMiddleAH = -145;
 
+    public static double rStackPickupMiddleBX = 45, rStackPickupMiddleBY = 49, rStackPickupMiddleBH = 145.1;
+    public static double bStackPickupMiddleBX = 45, bStackPickupMiddleBY = -49, bStackPickupMiddleBH = -145.1;
 
+    public static double rPickupGateXA = 60, rPickupGateYA = 90, rPickupGateHA = 140;
+    public static double bPickupGateXA = 60, bPickupGateYA = -90, bPickupGateHA = -140;
+    public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 145;
+    public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -145;
+    public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190;
+    public static double bPickupGateXC = 50, bPickupGateYC = -83, bPickupGateHC = -190;
 
-    public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;
-    public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;
+    public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 54;
+    public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -54;
 
 }

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

@@ -7,14 +7,14 @@ import com.acmerobotics.roadrunner.Pose2d;
 public class Front_Poses {
 
 
-    public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
-    public static double bx1 = 20, by1 = -0.5, bh1 = -0.1;
+    public static double rx1 = 30, ry1 = 5, rh1 = 0.1;
+    public static double bx1 = 30, by1 = -5, bh1 = -0.1;
 
     public static double rx2a = 41, ry2a = 18, rh2a = 140;
     public static double bx2a = 41, by2a = -18, bh2a = -140;
 
     public static double rx2b = 21, ry2b = 34, rh2b = 140.1;
-    public static double bx2b = 23, by2b = -36, bh2b = -140.1;
+    public static double bx2b = 23, by2b = -34, bh2b = -140.1;
 
     public static double rx3a = 55, ry3a = 39, rh3a = 140;
     public static double bx3a = 55, by3a = -39, bh3a = -140;
@@ -33,8 +33,8 @@ public class Front_Poses {
 
     public static double rfx1 = 10, rfy1 = 0, rfh1 = 0;
 
-    public static double rShootX = 40, rShootY = 10, rShootH = 50;
-    public static double bShootX = 40, bShootY = -10, bShootH = -50;
+    public static double rShootX = 60, rShootY = 10, rShootH = 50;
+    public static double bShootX = 60, bShootY = -10, bShootH = -50;
 
     public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
     public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
@@ -58,5 +58,14 @@ public class Front_Poses {
     public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180;
     public static double pickupGateCX = 34, pickupGateCY = 58, pickupGateCH = 220;
 
+    public static double rOpenGateCloseX = 20, rOpenGateCloseY = 35, rOpenGateCloseH = 230;
+    public static double bOpenGateCloseX = 20, bOpenGateCloseY = -35, bOpenGateCloseH = -230;
+
+    public static double rOpenGateMiddleX = 36, rOpenGateMiddleY = 59, rOpenGateMiddleH = 50;
+    public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -50;
+
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
+
+    //For PedroPathing TODO: figure out how to change start poses in auto
+    public static double teleStartPoseX = 72, teleStartPoseY = 72, teleStartPoseH = 0;
 }

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

@@ -9,17 +9,17 @@ public class ServoPositions {
 
     public static double spindexer_intakePos2 = 0.37; //0.33;//0.5;
 
-    public static double spindexer_intakePos3 = 0.56; //0.53;//0.66;
+    public static double spindexer_intakePos3 = 0.55; //0.53;//0.66;
 
-    public static double spindexer_outtakeBall3 = 0.84; //0.65; //0.24;
+    public static double spindexer_outtakeBall3 = 0.83; //0.65; //0.24;
     public static double spindexer_outtakeBall3b = 0.27; //0.65; //0.24;
 
-    public static double spindexer_outtakeBall2 = 0.66; //0.46; //0.6;
-    public static double spindexer_outtakeBall1 = 0.47; //0.27; //0.4;
-    public static double spinStartPos = 0.10;
-    public static double spinEndPos = 0.95;
+    public static double spindexer_outtakeBall2 = 0.65; //0.46; //0.6;
+    public static double spindexer_outtakeBall1 = 0.46; //0.27; //0.4;
+    public static double spinStartPos = 0;
+    public static double spinEndPos = 0.6;
 
-    public static double shootAllSpindexerSpeedIncrease = 0.014;
+    public static double shootAllSpindexerSpeedIncrease = 0.01;
 
     public static double transferServo_out = 0.15;
 
@@ -27,7 +27,7 @@ public class ServoPositions {
 
     public static double hoodAuto = 0.27;
 
-    public static double hoodOffset = -0.04; // offset from 0.93
+    public static double hoodOffset = -0.05; // offset from 0.93 (or position at 0,0 in targeting class)
 
     public static double turret_redClose = 0;
     public static double turret_blueClose = 0;

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

@@ -1,19 +1,60 @@
 package org.firstinspires.ftc.teamcode.libs.pedroPathing;
 
+import com.acmerobotics.dashboard.config.Config;
+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 com.pedropathing.ftc.drivetrains.MecanumConstants;
+import com.pedropathing.ftc.localization.constants.PinpointConstants;
 import com.pedropathing.paths.PathConstraints;
+import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+@Config
 public class Constants {
-    public static FollowerConstants followerConstants = new FollowerConstants();
+    public static FollowerConstants followerConstants = new FollowerConstants()
+            .mass(15.5)
+            .forwardZeroPowerAcceleration(-29.512)
+            .lateralZeroPowerAcceleration(-72.872)
+            .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012))
+            .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.025, 0.02))
+            .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03))
+            .centripetalScaling(0.0005);
 
-    public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
+    public static MecanumConstants driveConstants = new MecanumConstants()
+            .maxPower(1)
+            .rightFrontMotorName("fr")
+            .rightRearMotorName("br")
+            .leftRearMotorName("bl")
+            .leftFrontMotorName("fl")
+            .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE)
+            .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE)
+            .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD)
+            .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD)
+            .xVelocity(64.675)
+            .yVelocity(49.583);
+
+    public static double breakingStrength = 1;
+    public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1);
+
+    public static PinpointConstants localizerConstants = new PinpointConstants()
+            .forwardPodY(-7.5)
+            .strafePodX(-3.75)
+            .distanceUnit(DistanceUnit.INCH)
+            .hardwareMapName("pinpoint")
+            .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)
+            .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED)
+            .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD);
 
     public static Follower createFollower(HardwareMap hardwareMap) {
         return new FollowerBuilder(followerConstants, hardwareMap)
                 .pathConstraints(pathConstraints)
+                .mecanumDrivetrain(driveConstants)
+                .pinpointLocalizer(localizerConstants)
                 .build();
     }
-}
+}

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

@@ -2,6 +2,10 @@ package org.firstinspires.ftc.teamcode.teleop;
 
 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.Front_Poses.teleStartPoseH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease;
 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;
@@ -11,13 +15,17 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.controller.PIDFController;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.constants.Color;
+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.libs.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utils.Drivetrain;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Light;
@@ -33,13 +41,15 @@ import java.util.List;
 @Config
 @TeleOp
 public class TeleopV3 extends LinearOpMode {
+    private double metersToInches = 39.3700787402;
     public static double manualVel = 3000;
     public static double hoodDefaultPos = 0.5;
-
+    private double predictedResetX, predictedResetY, predictedResetH;
+    public static double redPredictedResetX = 9, redPredictedResetY = 10.25, redPredictedResetH = 0;
+    public static double bluePredictedResetX = 135.0, bluePredictedResetY = 9, bluePredictedResetH = 180;
     public static double spinPow = 0.09;
     public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
-    public static double spinSpeedIncrease = 0.03;
-    public static int resetSpinTicks = 4;
+    public static int resetSpinTicks = 0;
     public static double hoodSpeedOffset = 0.01;
     public static double turretSpeedOffset = 0.01;
     public double vel = 3000;
@@ -53,12 +63,13 @@ public class TeleopV3 extends LinearOpMode {
     Light light;
     Servos servo;
     Flywheel flywheel;
-    MecanumDrive drive;
+//    MecanumDrive drive;
     Spindexer spindexer;
     Targeting targeting;
     Targeting.Settings targetingSettings;
     Drivetrain drivetrain;
     MeasuringLoopTimes loopTimes;
+    Follower follower;
     double autoHoodOffset = 0.0;
     int shooterTicker = 0;
     boolean intake = false;
@@ -76,12 +87,11 @@ public class TeleopV3 extends LinearOpMode {
 
     int intakeTicker = 0;
     private boolean shootAll = false;
-    boolean relocalize = false;
+    public static boolean relocalize = false;
 
     @Override
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);
-        robot.light.setPosition(0);
         List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
 
         for (LynxModule hub : allHubs) {
@@ -91,12 +101,15 @@ public class TeleopV3 extends LinearOpMode {
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
         servo = new Servos(hardwareMap);
         flywheel = new Flywheel(hardwareMap);
-        drive = new MecanumDrive(hardwareMap, teleStart);
+//        drive = new MecanumDrive(hardwareMap, teleStart);
+        follower = Constants.createFollower(hardwareMap);
+        Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
+        follower.setStartingPose(start);
         spindexer = new Spindexer(hardwareMap);
         targeting = new Targeting();
         targetingSettings = new Targeting.Settings(0.0, 0.0);
 
-        drivetrain = new Drivetrain(robot, drive);
+        drivetrain = new Drivetrain(robot, follower);
 
         loopTimes = new MeasuringLoopTimes();
         loopTimes.init();
@@ -112,23 +125,36 @@ public class TeleopV3 extends LinearOpMode {
 
         light.setState(StateEnums.LightState.MANUAL);
         limelightUsed = true;
+        Spindexer.teleop = true;
 
         while (opModeInInit()) {
+            //ONLY FOR TESTING: COMMENT OUT FOR COMPETITIONS
+            if (gamepad1.crossWasPressed()){
+                redAlliance = !redAlliance;
+            }
+
             robot.limelight.start();
             if (redAlliance) {
                 turret.pipelineSwitch(4);
                 light.setManualLightColor(Color.LightRed);
+                predictedResetX = redPredictedResetX;
+                predictedResetY = redPredictedResetY;
+                predictedResetH = Math.toRadians(redPredictedResetH);
             } else {
                 turret.pipelineSwitch(2);
                 light.setManualLightColor(Color.LightBlue);
-
+                predictedResetX = bluePredictedResetX;
+                predictedResetY = bluePredictedResetY;
+                predictedResetH = Math.toRadians(bluePredictedResetH);
             }
-            robot.light.setPosition(1);
+            limelightUsed = true;
+
+            TELE.addData("Red Alliance?", redAlliance);
+            TELE.update();
 
             light.update();
         }
 
-        limelightUsed = true;
 
         waitForStart();
         if (isStopRequested()) return;
@@ -139,10 +165,71 @@ public class TeleopV3 extends LinearOpMode {
         while (opModeIsActive()) {
 
             //TELE.addData("Is limelight on?", robot.limelight.getStatus());
+            follower.update();
+            Pose currentPose = follower.getPose();
+
+            if (enableSpindexerManager) {
+                //if (!shootAll) {
+                spindexer.processIntake();
+                //}
+
+                // RIGHT_BUMPER
+                if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
+                    spindexer.setIntakePower(1);
+                } else if (gamepad1.cross) {
+                    spindexer.setIntakePower(-1);
+                } else {
+                    spindexer.setIntakePower(0);
+                }
+
+                // LEFT_BUMPER
+                if (!shootAll && gamepad1.leftBumperWasReleased()) {
+                    shootStamp = getRuntime();
+                    shootAll = true;
+
+                    shooterTicker = 0;
+
+                }
+                intakeTicker++;
+                if (shootAll) {
+                    intakeTicker = 0;
+                    intake = false;
+                    reject = false;
+
+                    if (shooterTicker == 0) {
+                        spindexer.prepareShootAllContinous();
+                        //TELE.addLine("preparing to shoot");
+//                    } else if (shooterTicker == 2) {
+//                        //servo.setTransferPos(transferServo_in);
+//                        spindexer.shootAll();
+//                        TELE.addLine("starting to shoot");
+                    } else if (spindexer.shootAllComplete()) {
+                        //spindexPos = spindexer_intakePos1;
+                        shootAll = false;
+                        spindexer.resetSpindexer();
+                        //spindexer.processIntake();
+                        //TELE.addLine("stop shooting");
+                    }
+                    shooterTicker++;
+                    //spindexer.processIntake();
+                }
+
+                if (gamepad1.left_stick_button) {
+                    servo.setTransferPos(transferServo_out);
+                    //spindexPos = spindexer_intakePos1;
+                    shootAll = false;
+                    spindexer.resetSpindexer();
+                }
+            }
 
             //DRIVETRAIN:
 
-            drivetrain.drive(-gamepad1.right_stick_y, gamepad1.right_stick_x, gamepad1.left_stick_x, gamepad1.left_trigger);
+            drivetrain.drive(
+                    -gamepad1.right_stick_y,
+                    gamepad1.right_stick_x,
+                    gamepad1.left_stick_x,
+                    gamepad1.left_trigger
+            );
 
             if (gamepad1.right_bumper) {
 
@@ -151,46 +238,52 @@ public class TeleopV3 extends LinearOpMode {
 
                 light.setState(StateEnums.LightState.BALL_COUNT);
 
-            } else if (gamepad2.triangle) {
-                light.setState(StateEnums.LightState.BALL_COLOR);
+                //} else if (gamepad2.triangle){
+                //light.setState(StateEnums.LightState.BALL_COLOR);
 
+                //}
             } else {
-                light.setState(StateEnums.LightState.GOAL_LOCK);
+                light.setState(StateEnums.LightState.BALL_COUNT);
             }
 
             //TURRET TRACKING
 
-            double robX = drive.localizer.getPose().position.x;
-            double robY = drive.localizer.getPose().position.y;
-            double robH = drive.localizer.getPose().heading.toDouble();
+            double robX = currentPose.getX();
+            double robY = currentPose.getY();
+            double robH = currentPose.getHeading();
 
             double robotX = robX + xOffset;
             double robotY = robY + yOffset;
             double robotHeading = robH + hOffset;
 
-            double goalX = -15;
-            double goalY = 0;
-
-            double dx = robotX - goalX;  // delta x from robot to goal
-            double dy = robotY - goalY;  // delta y from robot to goal
-            Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
+//            double goalX = -15;
+//            double goalY = 0;
+//
+//            double dx = robotX - goalX;  // delta x from robot to goal
+//            double dy = robotY - goalY;  // delta y from robot to goal
+//            Pose deltaPose = new Pose(dx, dy, robotHeading);
+            Pose deltaPose = new Pose(robotX, robotY, robotHeading);
 
 //            double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 
-            targetingSettings = targeting.calculateSettings(robotX, robotY, robotHeading, 0.0, turretInterpolate);
+            targetingSettings = targeting.calculateSettings
+                    (robotX, robotY, robotHeading, 0.0, turretInterpolate);
 
             //RELOCALIZATION
 
-            if (gamepad2.squareWasPressed()) {
+            if (gamepad2.triangleWasPressed()){
                 relocalize = !relocalize;
                 gamepad2.rumble(500);
             }
 
-            if (relocalize) {
+            if (relocalize){
                 turret.relocalize();
-                xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
-                yOffset = (turret.getLimelightX() * 39.3701) - robY;
-                hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
+                xOffset = ((turret.getLimelightY()*metersToInches)+72) - robX;
+                yOffset = (72-(turret.getLimelightX()*metersToInches)) - robY;
+                hOffset = (Math.toRadians(turret.getLimelightH() + 90));
+                while (hOffset > 180) {hOffset-=360;}
+                while (hOffset < -180) {hOffset+=360;}
+                hOffset = hOffset - robH;
             } else {
                 turret.trackGoal(deltaPose);
             }
@@ -225,7 +318,6 @@ public class TeleopV3 extends LinearOpMode {
 
             //HOOD:
 
-
             if (targetingHood) {
                 servo.setHoodPos(targetingSettings.hoodAngle + autoHoodOffset);
             } else {
@@ -258,66 +350,19 @@ public class TeleopV3 extends LinearOpMode {
             }
 
             if (gamepad2.crossWasPressed()) {
-                drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+//                drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+                follower.setPose(new Pose(predictedResetX, predictedResetY, predictedResetH));
+                gamepad2.rumble(200);
+                sleep(500);
             }
 
-
-            if (enableSpindexerManager) {
-                //if (!shootAll) {
-                spindexer.processIntake();
-                //}
-
-                // RIGHT_BUMPER
-                if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
-                    spindexer.setIntakePower(1);
-                } else if (gamepad1.cross) {
-                    spindexer.setIntakePower(-1);
-                } else {
-                    spindexer.setIntakePower(0);
-                }
-
-                // LEFT_BUMPER
-                if (!shootAll && gamepad1.leftBumperWasReleased()) {
-                    shootStamp = getRuntime();
-                    shootAll = true;
-
-                    shooterTicker = 0;
-
-                }
-                intakeTicker++;
-                if (shootAll) {
-                    intakeTicker = 0;
-                    intake = false;
-                    reject = false;
-
-                    if (shooterTicker == 0) {
-                        spindexer.prepareShootAllContinous();
-                        //TELE.addLine("preparing to shoot");
-//                   else if (shooterTicker == 2) {
-//                        //servo.setTransferPos(transferServo_in);
-//                        spindexer.shootAll();
-//                        TELE.addLine("starting to shoot");
-                    } else if (spindexer.shootAllComplete()) {
-                        //spindexPos = spindexer_intakePos1;
-                        shootAll = false;
-                        spindexer.resetSpindexer();
-                        //spindexer.processIntake();
-                        //TELE.addLine("stop shooting");
-                    }
-                    shooterTicker++;
-                    //spindexer.processIntake();
-                }
-
-                if (gamepad1.left_stick_button) {
-                    servo.setTransferPos(transferServo_out);
-                    //spindexPos = spindexer_intakePos1;
-                    shootAll = false;
-                    spindexer.resetSpindexer();
-                }
+            if (gamepad2.squareWasPressed()){
+                shootAllSpindexerSpeedIncrease = shootAllSpindexerSpeedIncrease-0.01;
+            } else if (gamepad2.circleWasPressed()){
+                shootAllSpindexerSpeedIncrease = shootAllSpindexerSpeedIncrease+0.01;
             }
 
             //EXTRA STUFFINESS:
-            drive.updatePoseEstimate();
 
             for (LynxModule hub : allHubs) {
                 hub.clearBulkCache();
@@ -363,9 +408,9 @@ public class TeleopV3 extends LinearOpMode {
             TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
             TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
             TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
-            TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
-            TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
-            TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
+//            TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
+//            TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
+//            TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
 
             TELE.update();
 

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

@@ -8,13 +8,13 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
 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.teleop.TeleopV3.spinSpeedIncrease;
 import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -107,7 +107,7 @@ public class ShooterTest extends LinearOpMode {
 
             double dx = robX - goalX;  // delta x from robot to goal
             double dy = robY - goalY;  // delta y from robot to goal
-            Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
+            Pose deltaPose = new Pose(dx, dy, robotHeading);
 
             double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 

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

@@ -65,46 +65,51 @@ public class SortingTest extends LinearOpMode {
         waitForStart();
         if (isStopRequested()) return;
 
-        while (opModeIsActive()){
-            spindexer.setIntakePower(1);
-            robot.transfer.setPower(1);
-
-            if (gamepad1.crossWasPressed()){
-                motif = 21;
-            } else if (gamepad1.squareWasPressed()){
-                motif = 22;
-            } else if (gamepad1.triangleWasPressed()){
-                motif = 23;
-            }
-            flywheel.manageFlywheel(2500);
-
-            if (gamepad1.leftBumperWasPressed()){
-                intaking = false;
-                Actions.runBlocking(
-                        autoActions.prepareShootAll(
-                                3,
-                                5,
-                                motif,
-                                0.501,
-                                0.501,
-                                0.501
-                        )
-                );
-            } else if (gamepad1.rightBumperWasPressed()){
-                intaking = false;
-                Actions.runBlocking(
-                        autoActions.shootAllAuto(
-                                3.5,
-                                0.014,
-                                0.501,
-                                0.501,
-                                0.501
-                        )
-                );
-                intaking = true;
-            } else if (intaking){
-                spindexer.processIntake();
-            }
+        if (opModeIsActive()){
+            Actions.runBlocking(
+                    autoActions.ShakeDrivetrain(
+                            100
+                    )
+            );
+//            spindexer.setIntakePower(1);
+//            robot.transfer.setPower(1);
+//
+//            if (gamepad1.crossWasPressed()){
+//                motif = 21;
+//            } else if (gamepad1.squareWasPressed()){
+//                motif = 22;
+//            } else if (gamepad1.triangleWasPressed()){
+//                motif = 23;
+//            }
+//            flywheel.manageFlywheel(2500);
+//
+//            if (gamepad1.leftBumperWasPressed()){
+//                intaking = false;
+//                Actions.runBlocking(
+//                        autoActions.prepareShootAll(
+//                                3,
+//                                5,
+//                                motif,
+//                                0.501,
+//                                0.501,
+//                                0.501
+//                        )
+//                );
+//            } else if (gamepad1.rightBumperWasPressed()){
+//                intaking = false;
+//                Actions.runBlocking(
+//                        autoActions.shootAllAuto(
+//                                3.5,
+//                                0.014,
+//                                0.501,
+//                                0.501,
+//                                0.501
+//                        )
+//                );
+//                intaking = true;
+//            } else if (intaking){
+//                spindexer.processIntake();
+//            }
         }
     }
 }

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

@@ -4,11 +4,14 @@ import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.libs.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Turret;
 
@@ -25,26 +28,31 @@ public class TurretTest extends LinearOpMode {
         );
 
         Turret turret = new Turret(robot, TELE, robot.limelight);
+
+        Follower follower;
+        follower = Constants.createFollower(hardwareMap);
+        Pose start = new Pose(72, 72, 0);
+        follower.setStartingPose(start);
+        follower.update();
         waitForStart();
 
-        MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0));
+        Turret.limelightUsed = false;
 
         while(opModeIsActive()){
+            follower.update();
+            turret.trackGoal(follower.getPose());
 
-            drive.updatePoseEstimate();
-            turret.trackGoal(drive.localizer.getPose());
+//            TELE.addData("tpos", turret.getTurrPos());
+//            TELE.addData("Limelight tx", turret.getBearing());
+//            TELE.addData("Limelight ty", turret.getTy());
+//            TELE.addData("Limelight X", turret.getLimelightX());
+//            TELE.addData("Limelight Y", turret.getLimelightY());
 
-            TELE.addData("tpos", turret.getTurrPos());
-            TELE.addData("Limelight tx", turret.getBearing());
-            TELE.addData("Limelight ty", turret.getTy());
-            TELE.addData("Limelight X", turret.getLimelightX());
-            TELE.addData("Limelight Y", turret.getLimelightY());
+//            if(zeroTurr){
+//                turret.zeroTurretEncoder();
+//            }
 
-            if(zeroTurr){
-                turret.zeroTurretEncoder();
-            }
-
-            TELE.update();
+//            TELE.update();
         }
 
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java

@@ -1,54 +1,76 @@
 package org.firstinspires.ftc.teamcode.utils;
 
-import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.ProfileAccelConstraint;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.TranslationalVelConstraint;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.acmerobotics.roadrunner.ftc.Actions;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.PathChain;
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 
 public class Drivetrain {
 
-     Robot robot;
+    Robot robot;
     boolean autoDrive = false;
 
-    Pose2d brakePos = new Pose2d(0, 0, 0);
+    Pose brakePos = new Pose(0, 0, 0);
 
-    MecanumDrive drive;
+//    MecanumDrive drive;
+    Follower follower;
 
     private final TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200);
     private final ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
 
 
-    public Drivetrain (Robot rob, MecanumDrive mecanumDrive){
+    public Drivetrain (Robot rob, Follower follower){
 
         this.robot = rob;
-        this.drive = mecanumDrive;
+        this.follower = follower;
 
     }
 
+    private double prevY = 0;
+    private double prevX = 0;
+    private double prevRX = 0;
+    private double prevBrake = 0;
     public void drive(double y, double x, double rx, double brake){
+        int countConstant = 0;
+        boolean brakeChange = false;
+        if (Math.abs(prevY - y) > 0.05){
+            prevY = y;
+            countConstant++;
+        }
+        if (Math.abs(prevX - x) > 0.05){
+            prevX = x;
+            countConstant++;
+        }
+        if (Math.abs(prevRX - rx) > 0.05){
+            prevRX = rx;
+            countConstant++;
+        }
+        if (Math.abs(prevBrake - brake) > 0.05){
+            prevBrake = brake;
+            brakeChange = true;
+        }
 
-        if (!autoDrive) {
+        if (!autoDrive && countConstant > 0) {
 
             x = x* 1.1; // Counteract imperfect strafing
 
 
-            double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
-            double frontLeftPower = (y + x + rx) / denominator;
-            double backLeftPower = (y - x + rx) / denominator;
-            double frontRightPower = (y - x - rx) / denominator;
-            double backRightPower = (y + x - rx) / denominator;
+            double denominator = Math.max(Math.abs(prevY) + Math.abs(prevX) + Math.abs(prevRX), 1);
+            double frontLeftPower = (prevY + prevX + prevRX) / denominator;
+            double backLeftPower = (prevY - prevX + prevRX) / denominator;
+            double frontRightPower = (prevY - prevX - prevRX) / denominator;
+            double backRightPower = (prevY + prevX - prevRX) / denominator;
 
             robot.frontLeft.setPower(frontLeftPower);
             robot.backLeft.setPower(backLeftPower);
             robot.frontRight.setPower(frontRightPower);
             robot.backRight.setPower(backRightPower);
         }
+        Pose currentPos = follower.getPose();
+        brakePos = currentPos;
 
         if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) {
             robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -56,23 +78,17 @@ public class Drivetrain {
             robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
             robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
-            drive.updatePoseEstimate();
-
-            brakePos = drive.localizer.getPose();
             autoDrive = true;
 
         } else if (brake > 0.4) {
-            drive.updatePoseEstimate();
 
-            Pose2d currentPos = drive.localizer.getPose();
+            PathChain traj2 = follower.pathBuilder()
+                    .addPath(new BezierLine(currentPos, brakePos))
+                    .setLinearHeadingInterpolation(currentPos.getHeading(), brakePos.getHeading())
+                    .build();
 
-            TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos)
-                    .strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT);
-
-            if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) {
-                Actions.runBlocking(
-                        traj2.build()
-                );
+            if (Math.abs(currentPos.getX() - brakePos.getX()) > 1 || Math.abs(currentPos.getY() - brakePos.getY()) > 1) {
+                follower.followPath(traj2);
             }
         } else {
             autoDrive = false;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java

@@ -64,17 +64,17 @@ public final class Light {
                 break;
 
             case BALL_COLOR:
-
-                if ((System.currentTimeMillis() % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) {
+                double currentTime = System.currentTimeMillis();
+                if ((currentTime % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) {
                     lightColor = spindexer.getRearCenterLight();
-                } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime / 3)) {
+                } else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime / 3)) {
                     lightColor = 0;
-                } else if ((System.currentTimeMillis() % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) {
+                } else if ((currentTime % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) {
                     lightColor = spindexer.getDriverLight();
 
-                } else if ((System.currentTimeMillis() % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) {
+                } else if ((currentTime % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) {
                     lightColor = 0;
-                } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime - restingTime)) {
+                } else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime - restingTime)) {
                     lightColor = spindexer.getPassengerLight();
 
                 } else {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java

@@ -79,6 +79,6 @@ public class Servos {
     }
 
     public boolean spinEqual(double pos) {
-        return Math.abs(pos - this.getSpinPos()) < 0.03;
+        return Math.abs(pos - this.getSpinPos()) < 0.05;
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java

@@ -48,7 +48,7 @@ public class Spindexer {
     public double distanceFrontDriver = 0.0;
     public double distanceFrontPassenger = 0.0;
 
-    public double spindexerWiggle = 0.01;
+    public double spindexerWiggle = 0.03;
 
     public double spindexerOuttakeWiggle = 0.01;
     private double prevPos = 0.0;
@@ -173,10 +173,25 @@ public class Spindexer {
     // Detects if a ball is found and what color.
     // Returns true is there was a new ball found in Position 1
     // FIXIT: Reduce number of times that we read the color sensors for loop times.
+    public static boolean teleop = false;
     public boolean detectBalls(boolean detectRearColor, boolean detectFrontColor) {
 
         boolean newPos1Detection = false;
         int spindexerBallPos = 0;
+        double rearDistance;
+        double frontDriverDistance;
+        double frontPassengerDistance;
+        if (teleop){
+            rearDistance = 48;
+            frontDriverDistance = 50;
+            frontPassengerDistance = 29;
+            detectFrontColor = false;
+            detectRearColor = false;
+        } else {
+            rearDistance = 48;
+            frontDriverDistance = 56;
+            frontPassengerDistance = 29;
+        }
 
         // Read Distances
         double dRearCenter = robot.color1.getDistance(DistanceUnit.MM);
@@ -187,12 +202,12 @@ public class Spindexer {
         distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
 
         // Position 1
-        if (distanceRearCenter < 48) {
+        if (distanceRearCenter < rearDistance) {
 
             // Mark Ball Found
             newPos1Detection = true;
 
-            if (detectRearColor) {
+            if (detectRearColor && !teleop) {
                 // Detect which color
                 NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors();
 
@@ -209,10 +224,10 @@ public class Spindexer {
         // Position 2
         // Find which ball position this is in the spindexer
         spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()];
-        if (distanceFrontDriver < 50) {
+        if (distanceFrontDriver < frontDriverDistance) {
             // reset FoundEmpty because looking for 3 in a row before reset
             ballPositions[spindexerBallPos].foundEmpty = 0;
-            if (detectFrontColor) {
+            if (detectFrontColor && !teleop) {
                 NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors();
 
                 double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue);
@@ -235,11 +250,11 @@ public class Spindexer {
 
         // Position 3
         spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()];
-        if (distanceFrontPassenger < 29) {
+        if (distanceFrontPassenger < frontPassengerDistance) {
 
             // reset FoundEmpty because looking for 3 in a row before reset
             ballPositions[spindexerBallPos].foundEmpty = 0;
-            if (detectFrontColor) {
+            if (detectFrontColor && !teleop) {
                 NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors();
 
                 double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
@@ -433,13 +448,13 @@ public class Spindexer {
             case MOVING:
                 // Stopping when we get to the new position
                 if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
-                    if (intakeTicker > 1){
+                    //if (intakeTicker > 1){
                         currentIntakeState = Spindexer.IntakeState.INTAKE;
                         stopSpindexer();
                         intakeTicker = 0;
-                    } else {
-                        intakeTicker++;
-                    }
+                    //} else {
+                    //    intakeTicker++;
+                    //}
                     //detectBalls(false, false);
                 } else {
                     // Keep moving the spindexer
@@ -535,13 +550,11 @@ public class Spindexer {
                 break;
 
             case SHOOT_PREP_CONTINOUS:
-                if (shootTicks > waitFirstBallTicks){
-                    currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
-                    shootTicks++;
-                } else if (servos.spinEqual(spinStartPos)){
-                    shootTicks++;
+                if (servos.spinEqual(spinStartPos)){
                     servos.setTransferPos(transferServo_in);
+                    currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
                 } else {
+                    servos.setTransferPos(transferServo_out);
                     servos.setSpinPos(spinStartPos);
                 }
                 break;
@@ -557,7 +570,7 @@ public class Spindexer {
                     shootTicks = 0;
                     currentIntakeState = IntakeState.FINDNEXT;
                 } else {
-                    double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
+                    double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease;
                     if (spinPos > spinEndPos + 0.03){
                         spinPos = spinEndPos + 0.03;
                     }
@@ -671,10 +684,8 @@ public class Spindexer {
         return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor;
     }
     private double prevPow = 0.501;
-    private boolean firstIntakePow = true;
     public void setIntakePower(double pow){
-        if (firstIntakePow || prevPow != pow){
-            firstIntakePow = false;
+        if (prevPow != 0.501 && prevPow != pow){
             robot.intake.setPower(pow);
         }
         prevPow = pow;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java

@@ -1,9 +1,12 @@
 package org.firstinspires.ftc.teamcode.utils;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
 
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import java.lang.Math;
 
+import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 
 public class Targeting {
@@ -71,7 +74,7 @@ public class Targeting {
     public final int TILE_UPPER_QUARTILE = 18;
     public final int TILE_LOWER_QUARTILE = 6;
     public double robotInchesX, robotInchesY = 0.0;
-    public int robotGridX, robotGridY = 0;
+    public int robotGridX = 0, robotGridY = 0;
     MultipleTelemetry TELE;
     double cancelOffsetX = 0.0;  // was -40.0
     double cancelOffsetY = 0.0;  // was 7.0
@@ -82,40 +85,64 @@ public class Targeting {
     public Targeting() {
     }
 
-    double cos54 = Math.cos(Math.toRadians(-54));
-    double sin54 = Math.sin(Math.toRadians(-54));
-
+    //TODO: change code so it uses pedropathing paths
     public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) {
         Settings recommendedSettings = new Settings(0.0, 0.0);
-        if (!redAlliance){
-            sin54 = Math.sin(Math.toRadians(54));
-        } else {
-            sin54 = Math.sin(Math.toRadians(-54));
-        }
-        // TODO: test these values determined from the fmap
-        double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
-        double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
-
-        // Convert robot coordinates to inches
-        robotInchesX = rotatedX * unitConversionFactor;
-        robotInchesY = rotatedY * unitConversionFactor;
-
-        // Find approximate location in the grid
-        int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
-        int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
-
-        int remX = Math.floorMod((int) robotInchesX, tileSize);
-        int remY = Math.floorMod((int) robotInchesY, tileSize);
-
-        //clamp
-
-        //if (redAlliance) {
-            robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
-            robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
-        //} else {
+        int gridX;
+        int gridY;
+        int remX = 0;
+        int remY = 0;
+        // Old code
+//        if (!redAlliance){
+//            sin54 = Math.sin(Math.toRadians(54));
+//            double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
+//            double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
+//
+//            // Convert robot coordinates to inches
+//            robotInchesX = rotatedX * unitConversionFactor + 20;
+//            robotInchesY = rotatedY * unitConversionFactor + 20;
+//
+//            // Find approximate location in the grid
+//            gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize));
+//            gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
+//        } else {
+//            sin54 = Math.sin(Math.toRadians(-54));
+//            double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
+//            double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
+//
+//            // Convert robot coordinates to inches
+//            robotInchesX = rotatedX * unitConversionFactor;
+//            robotInchesY = rotatedY * unitConversionFactor;
+//
+//            // Find approximate location in the grid
+//            gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
+//            gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
+//        }
+//
+//
+        remX = Math.floorMod((int) robotX, tileSize);
+        remY = Math.floorMod((int) robotY, tileSize);
+//
+//        //clamp
+//
+//        if (redAlliance) {
 //            robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
 //            robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
-        //}
+//        } else {
+//            robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
+//            robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
+//        }
+
+        // New code
+        if (redAlliance){
+            gridY = Math.round((float) (((144-robotX)-12) / 24));
+        } else {
+            gridY = Math.round((float) ((robotX-12) / 24));
+        }
+        gridX = Math.round((float) (((144-robotY)-12) / 24));
+
+        robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
+        robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
 
         // Determine if we need to interpolate based on tile position.
         // if near upper or lower quarter or tile interpolate with next tile.
@@ -192,7 +219,7 @@ public class Targeting {
         if (true) { //!interpolate) {
             if ((robotGridY < 6) && (robotGridX < 6)) {
                 recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
-                recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
+                recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle + hoodOffset;
             }
             return recommendedSettings;
         } else {

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

@@ -6,6 +6,7 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.controller.PIDController;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
@@ -13,6 +14,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
 import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.teleop.TeleopV3;
 
 import java.util.ArrayList;
 import java.util.List;
@@ -23,10 +25,10 @@ public class Turret {
 
     public static double turretTolerance = 0.02;
     public static double turrPosScalar = 0.00011264432;
-    public static double turret180Range = 0.54;
+    public static double turret180Range = 0.55;
     public static double turrDefault = 0.35;
     public static double turrMin = 0;
-    public static double turrMax = 1;
+    public static double turrMax = 0.69;
     public static boolean limelightUsed = true;
     public static double limelightPosOffset = 5;
     public static double manualOffset = 0.0;
@@ -79,7 +81,7 @@ public class Turret {
     }
 
     public double getTurrPos() {
-        return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
+        return robot.turr1.getPosition();
 
     }
     private double prevTurrPos = 0;
@@ -107,38 +109,24 @@ public class Turret {
     private void limelightRead() { // only for tracking purposes, not general reads
         Double xPos = null;
         Double yPos = null;
-        Double zPos = null;
+        double zPos;
         Double hPos = null;
         result = webcam.getLatestResult();
         if (result != null) {
             if (result.isValid()) {
                 tx = result.getTx();
                 ty = result.getTy();
-                // MegaTag1 code for receiving position
-                Pose3D botpose = result.getBotpose();
-                if (botpose != null) {
-                    limelightPosX = botpose.getPosition().x;
-                    limelightPosY = botpose.getPosition().y;
-                }
-                List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
-                for (LLResultTypes.FiducialResult fiducial : fiducials) {
-                    limelightTagPose = fiducial.getRobotPoseTargetSpace();
-                    if (limelightTagPose != null){
-                        xPos = limelightTagPose.getPosition().x;
-                        yPos = limelightTagPose.getPosition().y;
-                        zPos = limelightTagPose.getPosition().z;
-                        hPos = limelightTagPose.getOrientation().getYaw();
+                if (TeleopV3.relocalize){
+                    zPos = result.getBotpose().getPosition().z;
+                    if (zPos < 0.15){
+                        xPos = result.getBotpose().getPosition().x;
+                        yPos = result.getBotpose().getPosition().y;
+                        hPos = result.getBotpose().getOrientation().getYaw();
+                        limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
+                        limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
+                        limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH);
                     }
                 }
-
-            }
-        }
-        if (xPos != null){
-            if (zPos<0) {
-                limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
-                limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
-                limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ);
-                limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH);
             }
         }
     }
@@ -153,7 +141,6 @@ public class Turret {
         return ty;
     }
 
-    Pose3D limelightTagPose;
     double limelightTagX = 0.0;
     double limelightTagY = 0.0;
     double limelightTagZ = 0.0;
@@ -246,17 +233,31 @@ public class Turret {
         return bearingOffset;
     }
 
-    public void trackGoal(Pose2d deltaPos) {
+    double targetTurretPos;
+    public void trackGoal(Pose deltaPos) {
 
         /* ---------------- FIELD → TURRET GEOMETRY ---------------- */
+        double posX;
+        if (Color.redAlliance){
+            posX = 134 - deltaPos.getX();
+        } else {
+            posX = deltaPos.getX() - 10;
+        }
+        double posY = 140 - deltaPos.getY();
+        double posH = Math.toDegrees(deltaPos.getHeading());
+        while (posH > 180) posH -= 360;
+        while (posH < -180) posH += 360;
 
         // Angle from robot to goal in robot frame
-        double desiredTurretAngleDeg = Math.toDegrees(
-                Math.atan2(deltaPos.position.y, deltaPos.position.x)
-        );
+        double desiredTurretAngleDeg = Math.toDegrees(Math.atan2(posY, posX)) - 45;
 
         // Robot heading (field → robot)
-        double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble());
+        double robotHeadingDeg;
+        if (Color.redAlliance){
+            robotHeadingDeg = posH + 135;
+        } else {
+            robotHeadingDeg = posH + 45;
+        }
 
         // Turret angle needed relative to robot
         double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
@@ -277,7 +278,7 @@ public class Turret {
 
         limelightRead();
         // Active correction if we see the target
-        if (result.isValid() && !lockOffset && limelightUsed) {
+        if (result.isValid() && !lockOffset && limelightUsed && targetTurretPos > turrMin && targetTurretPos < turrMax) {
             currentTrackOffset += bearingAlign(result);
             currentTrackCount++;
 
@@ -333,7 +334,7 @@ public class Turret {
 
         /* ---------------- ANGLE → SERVO POSITION ---------------- */
 
-        double targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
+        targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
 
         // Clamp to physical servo limits
         targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax));
@@ -358,13 +359,14 @@ public class Turret {
 
 //        TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg);
 //        TELE.addData("Target Pos", "%.3f", targetTurretPos);
-//        TELE.addData("Current Pos", "%.3f", currentPos);
+//        TELE.addData("Current Localization Pos", deltaPos);
 //        TELE.addData("Commanded Pos", "%.3f", turretPos);
 //        TELE.addData("LL Valid", result.isValid());
 //        TELE.addData("LL getTx", result.getTx());
 //        TELE.addData("LL Offset", offset);
 //        TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET");
 //        TELE.addData("Learned Offset", "%.2f", offset);
+//        TELE.update();
     }
 
 }

build.dependencies.gradle

@@ -21,9 +21,9 @@ dependencies {
     implementation 'androidx.appcompat:appcompat:1.2.0'
 
 
-    implementation 'com.pedropathing:ftc:2.0.6' //PedroCore
-    implementation 'com.pedropathing:telemetry:1.0.0' //PedroTele
-    implementation 'com.bylazar:fullpanels:1.0.2' //Panels
+    implementation 'com.pedropathing:ftc:2.1.1'
+    implementation 'com.pedropathing:telemetry:1.0.0'
+    implementation 'com.bylazar:fullpanels:1.0.12'
     implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB
 
     implementation 'com.rowanmcalpin.nextftc:core:0.6.2' //NEXT FTC