track target distance regression WORKS

src/main/java/frc4388/robot/Constants.java

@@ -190,7 +190,7 @@ public final class Constants {
     public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21;
     public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22;
     public static final double TURRET_SPEED_MULTIPLIER = 0.6;
-    public static final double DEGREES_PER_ROT = 180.0/105.45445251464844;
+    public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844;
     public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0;
     public static final int TURRET_MOTOR_ROTS_PER_ROT = 0;
     public static final double ENCODER_TICKS_PER_REV = 2048;

src/main/java/frc4388/robot/Robot.java

@@ -121,7 +121,7 @@ public class Robot extends TimedRobot {
     });
 
     desmosServer.start();
-    DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0});
+    // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0});
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -59,6 +59,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AimToCenter;
@@ -291,11 +292,11 @@ public class RobotContainer {
     // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
     //     .whenPressed(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0)));
 
-    new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
+    // new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
-        .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
+    //     .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
-        .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
+    //     .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
 
-    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+    new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
         .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
         .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
 
@@ -317,10 +318,14 @@ public class RobotContainer {
       // A > Shoot with Odo
     /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
         .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/
+      
+      new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
+          .whenPressed(new RunCommand(() -> m_robotTurret.runshooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret));
 
        //B > Shoot with Lime
-    // new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-    //     .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
+        .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))
+        .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false)));
 
   
     /* Button Box Buttons */

src/main/java/frc4388/robot/commands/TrackTarget.java

@@ -112,12 +112,14 @@ public class TrackTarget extends CommandBase {
   }
 
   public final double distanceRegression(double distance) {
-    return m * distance + b;
+    return (1.09517561985 * distance + 20.1846165624);
   }
 
   public void updateRegressionDesmos() {
     m = DesmosServer.readDouble("m");
     b = DesmosServer.readDouble("b");
+
+    DesmosServer.putArray("MB", m, b);
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc4388/robot/subsystems/Turret.java

@@ -72,7 +72,7 @@ public class Turret extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition());
-    SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.DEGREES_PER_ROT);
+    SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT);
   }
 
   /**
@@ -94,7 +94,7 @@ public class Turret extends SubsystemBase {
   }
 
   public void runshooterRotatePID(double targetAngle) {
-    targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT;
+    targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT;
     m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition);
   }