readjusted shooter test

@KeshavAnandCode please merge

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

@@ -22,28 +22,28 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
-import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
-import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utilsv2.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 org.firstinspires.ftc.teamcode.utilsv2.Turret;
 
 import java.util.List;
 
 @Config
-@Autonomous (preselectTeleOp = "TeleopV3")
+@Autonomous (preselectTeleOp = "TeleopV4")
 public class Auto12BallPedroPathing extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;
-    Flywheel flywheel;
-    Targeting targeting;
-    Targeting.Settings targetingSettings;
+//    Flywheel flywheel;
+//    Targeting targeting;
+//    Targeting.Settings targetingSettings;
     Follower follower;
-    Turret turret;
-    Spindexer spindexer;
-    Servos servos;
+//    Turret turret;
+//    Spindexer spindexer;
+//    Servos servos;
     MeasuringLoopTimes loopTimes;
 
     // Wait Times
@@ -222,10 +222,10 @@ public class Auto12BallPedroPathing extends LinearOpMode {
                 driveShoot(PathState.WAIT_SHOOT3, currentTime);
                 break;
             case WAIT_SHOOT3:
-                if (spindexer.shootAllComplete()){
-                    spindexer.resetSpindexer();
-                    TELE.addLine("Done Auto");
-                }
+//                if (spindexer.shootAllComplete()){
+//                    spindexer.resetSpindexer();
+//                    TELE.addLine("Done Auto");
+//                }
                 break;
             default:
                 break;
@@ -237,21 +237,21 @@ public class Auto12BallPedroPathing extends LinearOpMode {
     private void intakePowerDown(double stamp, double currentTime) {
         double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust
         if (pow < -1) {pow = 0;}
-        spindexer.setIntakePower(pow);
+//        spindexer.setIntakePower(pow);
     }
     private void driveShoot(PathState nextState, double currentTime){
         if (!follower.isBusy()){
             pathState = nextState;
             timeStamp = currentTime;
-            spindexer.prepareShootAllContinous();
+//            spindexer.prepareShootAllContinous();
         }
     }
     private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) {
-        if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime) {
-            spindexer.resetSpindexer();
+        if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() ||
+//            spindexer.resetSpindexer();
             pathState = nextState;
             follower.followPath(nextPath, true);
-            spindexer.setIntakePower(1);
+//            spindexer.setIntakePower(1);
         }
     }
     private void drivePickup(PathState nextState, PathChain nextPath) {
@@ -286,24 +286,25 @@ public class Auto12BallPedroPathing extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        robot = new Robot(hardwareMap);
+        Robot.resetInstance();
+        robot = Robot.getInstance(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);
+//        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);
+//        turret = new Turret(robot, TELE, robot.limelight);
+//        spindexer = new Spindexer(hardwareMap);
+//        servos = new Servos(hardwareMap);
         loopTimes = new MeasuringLoopTimes();
         loopTimes.init();
 
-        robot.light.setPosition(Color.LightRed);
+//        robot.light.setPosition(Color.LightRed);
 
         boolean initializeRobot = false;
         while (opModeInInit()){
@@ -311,11 +312,11 @@ public class Auto12BallPedroPathing extends LinearOpMode {
 
             if (gamepad1.crossWasPressed() && !initializeRobot){
                 Color.redAlliance = !Color.redAlliance;
-                if (Color.redAlliance){
-                    robot.light.setPosition(Color.LightRed);
-                } else {
-                    robot.light.setPosition(Color.LightBlue);
-                }
+//                if (Color.redAlliance){
+//                    robot.light.setPosition(Color.LightRed);
+//                } else {
+//                    robot.light.setPosition(Color.LightBlue);
+//                }
 
                 double[] xPoses = {startPoseX, shoot0X,
                         drivePickup1X, pickup1X, shoot1X,
@@ -338,8 +339,8 @@ public class Auto12BallPedroPathing extends LinearOpMode {
                 buildPaths();
                 sleep(2000);
 
-                turret.setTurret(turrDefault);
-                servos.setSpinPos(spinStartPos);
+//                turret.setTurret(turrDefault);
+//                servos.setSpinPos(spinStartPos);
             }
 
             TELE.addData("Red Alliance?", Color.redAlliance);
@@ -352,27 +353,27 @@ public class Auto12BallPedroPathing extends LinearOpMode {
 
         if (isStopRequested()) return;
 
-        robot.transfer.setPower(1);
+//        robot.transfer.setPower(1);
         limelightUsed = false;
 
         while (opModeIsActive()){
             follower.update();
             pathStateMachine();
             Pose currentPose = follower.getPose();
-            teleStartPoseX = currentPose.getX();
-            teleStartPoseY = currentPose.getY();
-            teleStartPoseH = Math.toDegrees(currentPose.getHeading());
-
-            turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
-            targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 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();}
+//            teleStartPoseX = currentPose.getX();
+//            teleStartPoseY = currentPose.getY();
+//            teleStartPoseH = Math.toDegrees(currentPose.getHeading());
+//
+//            turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
+//            targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 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();}
 
             for (LynxModule hub : allHubs) {
                 hub.clearBulkCache();

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

@@ -0,0 +1,341 @@
+//package org.firstinspires.ftc.teamcode.autonomous;
+//
+//import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
+//import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
+//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.pedroPathing.Constants;
+//import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
+//import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
+//import org.firstinspires.ftc.teamcode.utilsv2.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.utilsv2.Turret;
+//
+//import java.util.List;
+//
+//@Config
+//@Autonomous (preselectTeleOp = "TeleopV4")
+//public class Auto12Ball_Back_Sorted extends LinearOpMode {
+//    Robot robot;
+//    MultipleTelemetry TELE;
+//    //    Flywheel flywheel;
+////    Targeting targeting;
+////    Targeting.Settings targetingSettings;
+//    Follower follower;
+//    //    Turret turret;
+////    Spindexer spindexer;
+////    Servos servos;
+//    MeasuringLoopTimes loopTimes;
+//
+//    // Wait Times
+//    public static double shootTime = 2;
+//
+//    // Extra Variables
+//    public static double intakePower = 0.3;
+//    double shootX, shootY, shootH;
+//
+//    // Initialize path state machine
+//    private enum PathState {
+//        PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
+//        PICKUP1, OPENGATE, 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 = 84, startPoseY = 7, startPoseH = 90;
+//    public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
+//    public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
+//    public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0;
+//    public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165;
+//    public static double openGateX = 129, openGateY = 74, openGateH = 0;
+//    public static double shoot1ControlX = 112, shoot1ControlY = 75;
+//    public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12;
+//    public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0;
+//    public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0;
+//    public static double shoot2ControlX = 102, shoot2ControlY = 63;
+//    public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50;
+//    public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0;
+//    public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0;
+//    public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446;
+//    public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80;
+//    Pose startPose, pushBot, shoot0,
+//            pickup1, openGateControl, openGate, shoot1Control, shoot1,
+//            drivePickup2, pickup2, shoot2Control, shoot2,
+//            drivePickup3, pickup3, shoot3Control, shoot3;
+//    private void initializePoses(){
+//        startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH));
+//        pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH));
+//        shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
+//        pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H));
+//        openGateControl = new Pose(openGateControlX, openGateControlY);
+//        openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH));
+//        shoot1Control = new Pose(shoot1ControlX, shoot1ControlY);
+//        shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H));
+//        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));
+//        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));
+//    }
+//
+//    //Building Paths
+//    PathChain startPose_pushBot, pushBot_shoot0,
+//            shoot0_pickup1, pickup1_openGate, openGate_shoot1,
+//            shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2,
+//            shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3;
+//    private void buildPaths(){
+//        startPose_pushBot = follower.pathBuilder()
+//                .addPath(new BezierLine(startPose, pushBot))
+//                .setLinearHeadingInterpolation(startPose.getHeading(), pushBot.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;
+//                    shootX = shoot0X;
+//                    shootY = shoot0Y;
+//                    shootH = shoot0H;
+//                }
+//                driveShoot(PathState.WAIT_SHOOT0, currentTime);
+//                break;
+//            case WAIT_SHOOT0:
+//                waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime);
+//                break;
+//            case DRIVE_PICKUP1:
+//                drivePickup(PathState.PICKUP1, drivePickup1_pickup1);
+//                break;
+//            case PICKUP1:
+//                pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1);
+//                shootX = shoot1X;
+//                shootY = shoot1Y;
+//                shootH = shoot1H;
+//                break;
+//            case DRIVE_SHOOT1:
+//                intakePowerDown(timeStamp, currentTime);
+//                driveShoot(PathState.WAIT_SHOOT1, currentTime);
+//                break;
+//            case WAIT_SHOOT1:
+//                waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime);
+//                break;
+//            case DRIVE_PICKUP2:
+//                drivePickup(PathState.PICKUP2, drivePickup2_pickup2);
+//                break;
+//            case PICKUP2:
+//                pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2);
+//                shootX = shoot2X;
+//                shootY = shoot2Y;
+//                shootH = shoot2H;
+//                break;
+//            case DRIVE_SHOOT2:
+//                intakePowerDown(timeStamp, currentTime);
+//                driveShoot(PathState.WAIT_SHOOT2, currentTime);
+//                break;
+//            case WAIT_SHOOT2:
+//                waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime);
+//                break;
+//            case DRIVE_PICKUP3:
+//                drivePickup(PathState.PICKUP3, drivePickup3_pickup3);
+//                break;
+//            case PICKUP3:
+//                pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3);
+//                shootX = shoot3X;
+//                shootY = shoot3Y;
+//                shootH = shoot3H;
+//                break;
+//            case DRIVE_SHOOT3:
+//                intakePowerDown(timeStamp, currentTime);
+//                driveShoot(PathState.WAIT_SHOOT3, currentTime);
+//                break;
+//            case WAIT_SHOOT3:
+////                if (spindexer.shootAllComplete()){
+////                    spindexer.resetSpindexer();
+////                    TELE.addLine("Done Auto");
+////                }
+//                break;
+//            default:
+//                break;
+//        }
+//        TELE.update(); // use for debugging
+//    }
+//
+//    // Voids for State Machine
+//    private void intakePowerDown(double stamp, double currentTime) {
+//        double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust
+//        if (pow < -1) {pow = 0;}
+////        spindexer.setIntakePower(pow);
+//    }
+//    private void driveShoot(PathState nextState, double currentTime){
+//        if (!follower.isBusy()){
+//            pathState = nextState;
+//            timeStamp = currentTime;
+////            spindexer.prepareShootAllContinous();
+//        }
+//    }
+//    private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) {
+//        if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() ||
+////            spindexer.resetSpindexer();
+//            pathState = nextState;
+//            follower.followPath(nextPath, true);
+////            spindexer.setIntakePower(1);
+//        }
+//    }
+//    private void drivePickup(PathState nextState, PathChain nextPath) {
+//        if (!follower.isBusy()) {
+//            pathState = nextState;
+//            follower.followPath(nextPath, intakePower, false);
+//        }
+//    }
+//    private void pickup(PathState nextState, PathChain nextPath) {
+//        if (!follower.isBusy()) {
+//            pathState = nextState;
+//            follower.followPath(nextPath, true);
+//        }
+//    }
+//
+//    // Used for changing alliance
+//    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.resetInstance();
+//        robot = Robot.getInstance(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);
+//        loopTimes = new MeasuringLoopTimes();
+//        loopTimes.init();
+//
+////        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);
+////                }
+//
+////                double[] xPoses = {startPoseX, shoot0X,
+////                        drivePickup1X, pickup1X, shoot1X,
+////                        drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
+////                        drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
+////
+////                double[] headings = {startPoseH, shoot0H,
+////                        drivePickup1H, pickup1H, shoot1H,
+////                        drivePickup2H, pickup2H, shoot2H,
+////                        drivePickup3H, pickup3H, shoot3H};
+//
+////                for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
+////                for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
+//            }
+//
+//            if (gamepad1.triangleWasPressed()){
+//                initializeRobot = true;
+//                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;
+//
+//        while (opModeIsActive()){
+//            follower.update();
+//            pathStateMachine();
+//            Pose currentPose = follower.getPose();
+////            teleStartPoseX = currentPose.getX();
+////            teleStartPoseY = currentPose.getY();
+////            teleStartPoseH = Math.toDegrees(currentPose.getHeading());
+////
+////            turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
+////            targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 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();}
+//
+//            for (LynxModule hub : allHubs) {
+//                hub.clearBulkCache();
+//            }
+//            loopTimes.loop();
+//
+//            TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
+//            TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
+//            TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
+//            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/pedroPathing/Constants.java

@@ -18,9 +18,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 public class Constants {
     public static FollowerConstants followerConstants = new FollowerConstants()
             .mass(14.37888)
-            .forwardZeroPowerAcceleration(-29.512)
-            .lateralZeroPowerAcceleration(-72.872)
-            .centripetalScaling(0);
+            .forwardZeroPowerAcceleration(-30.322)
+            .lateralZeroPowerAcceleration(-60.876)
+            .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.015, 0.02))
+            .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02))
+            .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.02, 0, 0.00001, 0.6, 0.015))
+            .centripetalScaling(0.0005);
 
     public static MecanumConstants driveConstants = new MecanumConstants()
             .maxPower(1)
@@ -32,15 +35,15 @@ public class Constants {
             .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE)
             .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD)
             .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD)
-            .xVelocity(64.675)
-            .yVelocity(49.583);
+            .xVelocity(84.376)
+            .yVelocity(64.052);
 
     public static double breakingStrength = 1;
     public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1);
 
     public static PinpointConstants localizerConstants = new PinpointConstants()
-            .forwardPodY(-3.676)
-            .strafePodX(3.780)
+            .forwardPodY(3.7795)
+            .strafePodX(-3.676)
             .distanceUnit(DistanceUnit.INCH)
             .hardwareMapName("pinpoint")
             .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)

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

@@ -416,7 +416,7 @@ class ForwardVelocityTuner extends OpMode {
 
 
         if (!end) {
-            if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) {
+            if (Math.abs(follower.getPose().getX()) > (DISTANCE)) {
                 end = true;
                 stopRobot();
             } else {
@@ -524,7 +524,7 @@ class LateralVelocityTuner extends OpMode {
         drawCurrentAndHistory();
 
         if (!end) {
-            if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) {
+            if (Math.abs(follower.getPose().getY()) > (DISTANCE)) {
                 end = true;
                 stopRobot();
             } else {

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

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.tests;
 
+import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -8,7 +9,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.NormalizedRGBA;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
 
 @Config
 @TeleOp
@@ -50,8 +51,9 @@ public class Hardware_Tester extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        robot = new Robot(hardwareMap);
-        TELE = new MultipleTelemetry();
+        Robot.resetInstance();
+        robot = Robot.getInstance(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
 
         robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

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

@@ -8,6 +8,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
 import org.firstinspires.ftc.teamcode.utilsv2.Robot;
 import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
 
@@ -17,6 +18,10 @@ public class NewShooterTest extends LinearOpMode {
 
     Robot robot;
 
+    MultipleTelemetry TELE;
+
+    Flywheel rpmFlywheel;
+
     public static boolean intake = true;
     public static boolean shoot = false;
     public static double intakePower = 1.0;
@@ -26,6 +31,8 @@ public class NewShooterTest extends LinearOpMode {
     public static double hoodPos = 0.51;
     public static double flywheel = 0;
 
+    public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75;
+
     private enum ShootState {
         IDLE,
         WAIT_GATE,
@@ -38,10 +45,10 @@ public class NewShooterTest extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
 
-        Robot.resetInstance();
-
         robot = Robot.getInstance(hardwareMap);
 
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
         Shooter shooter = new Shooter(
                 robot,
                 new MultipleTelemetry(
@@ -52,6 +59,8 @@ public class NewShooterTest extends LinearOpMode {
                 true
         );
 
+        rpmFlywheel = new Flywheel(robot);
+
         shooter.setState(Shooter.ShooterState.MANUAL);
 
         waitForStart();
@@ -63,6 +72,9 @@ public class NewShooterTest extends LinearOpMode {
             robot.setHoodPos(hoodPos);
             shooter.setTurretPosition(turretPos);
             shooter.setFlywheelVelocity(flywheel);
+            double voltage = robot.voltage.getVoltage();
+            rpmFlywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage);
+
             robot.setSpinPos(ServoPositions.spindexer_A2);
 
             if (intake && !shoot) {
@@ -76,9 +88,7 @@ public class NewShooterTest extends LinearOpMode {
                 robot.setIntakePower(intakePower);
                 robot.setTransferServoPos(
                         ServoPositions.transferServo_out);
-            }
-
-            else if (shoot) {
+            } else if (shoot) {
                 robot.setIntakePower(intakePower);
 
 
@@ -120,6 +130,9 @@ public class NewShooterTest extends LinearOpMode {
                 }
             }
 
+            TELE.addData("Flywheel Velocity", (robot.shooter1.getVelocity() * 60) / 28);
+            TELE.update();
+
             shooter.update();
         }
     }

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

@@ -1,12 +1,10 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
+
 import com.qualcomm.robotcore.hardware.HardwareMap;
-
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 
-import org.firstinspires.ftc.teamcode.utilsv2.Robot;
-
 import java.util.LinkedList;
 
 public class Flywheel {
@@ -27,10 +25,8 @@ public class Flywheel {
     private final LinkedList<Double> velocityHistory = new LinkedList<>();
 
     public Flywheel(Robot rob) {
-
         robot = rob;
-        shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
-        shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
+        shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / 12);
     }
 
     public double getVelo() {
@@ -65,19 +61,14 @@ public class Flywheel {
         shooterPIDF1.d = d;
         shooterPIDF1.f = f;
 
-        shooterPIDF2.p = p;
-        shooterPIDF2.i = i;
-        shooterPIDF2.d = d;
-        shooterPIDF2.f = f;
+        robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
 
-        if (Math.abs(prevF - f) > voltagePIDFDifference) {
-
-            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
-
-            robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
-
-            prevF = f;
-        }
+//        if (Math.abs(prevF - f) > voltagePIDFDifference) {
+//
+//            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
+//
+//            prevF = f;
+//        }
     }
 
     // Convert from RPM to Ticks per Second
@@ -115,8 +106,8 @@ public class Flywheel {
         }
 
         robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
-
-        robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
+        double power = robot.shooter1.getPower();
+        robot.shooter2.setPower(power);
 
         velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
 

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

@@ -106,13 +106,11 @@ public class Robot {
         shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
 
         shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
-        shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
+        shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12);
         shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
         shooter1.setVelocity(0);
-        shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
-        shooter2.setVelocity(0);
+        shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
         hood = hardwareMap.get(Servo.class, "hood");