SWERVE FIXES n SUCH

src/main/deploy/ShooterData.csv

@@ -1,14 +1,16 @@
 Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds)
 0             ,-29.5         ,8000
 78.5          ,-29.5         ,8000
-99            ,-39.62        ,8500
+88            ,-34.2         ,8600
+99            ,-39.62        ,8600
 111           ,-42           ,9000
 127.25        ,-49.12        ,9500
 141           ,-59.4         ,9900
 150           ,-66.22        ,10000
 164.5         ,-75.52        ,10000
-186           ,-76.24        ,10000
+189.9         ,-81.39        ,11000
 207           ,-104.07       ,11000
 227           ,-105.32       ,11500
+239           ,-105.5        ,12380
 255.5         ,-105.8        ,13500
 999           ,-105.8        ,13500

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

@@ -156,7 +156,7 @@ public final class Constants {
   }
 
   public static final class ExtenderConstants {
-    public static final double EXTENDER_FORWARD_LIMIT = 235.0;//250.0;
+    public static final double EXTENDER_FORWARD_LIMIT = 200.3;//250.0;
     public static final double EXTENDER_REVERSE_LIMIT = 0.0;
   }
 
@@ -164,7 +164,7 @@ public final class Constants {
     public static final int STORAGE_CAN_ID = 18;
     public static final int BEAM_SENSOR_SHOOTER = 28; //TODO
     public static final int BEAM_SENSOR_INTAKE = 29; //TODO
-    public static final double STORAGE_SPEED = 0.9;
+    public static final double STORAGE_SPEED = 1.0;//0.9;
   }
   public static final class LEDConstants {
     public static final int LED_SPARK_ID = 0;

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

@@ -75,7 +75,7 @@ public class Robot extends TimedRobot {
     SmartDashboard.putData(CommandScheduler.getInstance());
 
     // desmosServer.start();
-    m_robotContainer.m_robotVisionOdometry.setLEDs(true);
+    m_robotContainer.m_robotVisionOdometry.setLEDs(false);
     // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0});
   }
 
@@ -146,11 +146,13 @@ public class Robot extends TimedRobot {
         LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
     }
 
-    m_robotContainer.m_robotVisionOdometry.setLEDs(true);
+    m_robotContainer.m_robotVisionOdometry.setLEDs(false);
   }
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+    // m_robotContainer.m_robotVisionOdometry.setLEDs(false);
+  }
 
   /**
    * This autonomous runs the autonomous command selected by your

src/main/java/frc4388/robot/RobotMap.java

@@ -300,6 +300,7 @@ void configureExtenderMotors() {
     
     void configureStorageMotors() {
         storageMotor.restoreFactoryDefaults();
+        storageMotor.setIdleMode(IdleMode.kCoast);
     }
     
 }

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

@@ -94,7 +94,7 @@ public class TrackTarget extends CommandBase {
       //// points = filterPoints(points);
       Point average = VisionOdometry.averagePoint(points);
       
-      double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
+      double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
       output *= 2.1;
       
       m_turret.runTurretWithInput(output);
@@ -133,8 +133,6 @@ public class TrackTarget extends CommandBase {
       e.printStackTrace();
     }
     
-    // run storage
-    
   }
 
   public ArrayList<Point> filterPoints(ArrayList<Point> points) {
@@ -197,14 +195,14 @@ public class TrackTarget extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (this.isAuto) {
+    // if (this.isAuto) {
-      if (targetLocked && !timerStarted) {
+    //   if (targetLocked && !timerStarted) {
-        timerStarted = true;
+    //     timerStarted = true;
-        startTime = System.currentTimeMillis();
+    //     startTime = System.currentTimeMillis();
-      }
+    //   }
-      return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
+    //   return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
-    } else {
+    // } else {
       return false;
-    }
+    // }
   }
 }

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

@@ -106,9 +106,9 @@ public class SwerveDrive extends SubsystemBase {
     double ySpeedMetersPerSecond = speed.getY();
     chassisSpeeds = fieldRelative
         ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-            -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
+            -rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
         : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-            -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
+            -rot * SwerveDriveConstants.ROTATION_SPEED * 2);
     SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
     setModuleStates(states);
   }
@@ -128,8 +128,8 @@ public class SwerveDrive extends SubsystemBase {
     double ySpeedMetersPerSecond = speed.getY();
     chassisSpeeds = fieldRelative
         ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-            rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
+            rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
-        : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
+        : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 2);
     SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
         chassisSpeeds);
     // if (ignoreAngles) {
@@ -173,12 +173,12 @@ public class SwerveDrive extends SubsystemBase {
   public void periodic() {
 
     updateOdometry();
-    updateSmartDash();
+    // updateSmartDash();
 
-    SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
+    // SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
-    SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
+    // SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
-    SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
+    // SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
-    SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
+    // SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
 
     m_field.setRobotPose(m_odometry.getPoseMeters());
     super.periodic();

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

@@ -114,41 +114,41 @@ public class Turret extends SubsystemBase {
     // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
     // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
     
-    SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition());
+    // SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition());
-    SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT);
+    // SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT);
-    SmartDashboard.putBoolean("Turret Left Limit", m_boomBoomLeftLimit.isPressed());
+    // SmartDashboard.putBoolean("Turret Left Limit", m_boomBoomLeftLimit.isPressed());
-    SmartDashboard.putBoolean("Turret Right Limit", m_boomBoomRightLimit.isPressed());
+    // SmartDashboard.putBoolean("Turret Right Limit", m_boomBoomRightLimit.isPressed());
 
     // limit switch annoying time thing but actually worked first try wtf
-    leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed).
+    // leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed).
 
-    hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state.
+    // hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state.
 
-    if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time.
+    // if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time.
-      leftCurrentTime = System.currentTimeMillis();
+    //   leftCurrentTime = System.currentTimeMillis();
-      leftElapsedTime = 0;
+    //   leftElapsedTime = 0;
-    }
+    // }
 
-    if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false;
+    // if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false;
 
-    if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){
+    // if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){
-        recentlyPressed = true;
+    //     recentlyPressed = true;
-        m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/);
+    //     m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/);
-    }
+    // }
-     SmartDashboard.putBoolean("Recently Pressed", recentlyPressed);
+    // //  SmartDashboard.putBoolean("Recently Pressed", recentlyPressed);
 
-    if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time.
+    // if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time.
-      leftElapsedTime = System.currentTimeMillis() - leftCurrentTime;
+    //   leftElapsedTime = System.currentTimeMillis() - leftCurrentTime;
-    }
+    // }
     
-    if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position.
+    // if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position.
-      m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);
+    //   m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);
-    }
+    // }
 
-    leftPrevState = leftState; // * Update the state of the left limit switch.
+    // leftPrevState = leftState; // * Update the state of the left limit switch.
   
-    // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
+    // // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
-    runVelocityRamping();
+    // runVelocityRamping();
   }
 
   public void runVelocityRamping() {

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

@@ -44,7 +44,9 @@ public class VisionOdometry extends SubsystemBase {
    * @param shooter The turret subsystem
    */
   public VisionOdometry(SwerveDrive drive, Turret shooter) {
-    m_camera = new PhotonCamera(VisionConstants.NAME);
+    // do{
+      m_camera = new PhotonCamera(VisionConstants.NAME);
+    // }while(m_camera.getLatestResult().getLatencyMillis() == 0.d);
     m_drive = drive;
     m_shooter = shooter;