skibidi

src/main/java/frc/robot/commands/DriveCommands.java

@@ -26,6 +26,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.drive.Drive;
@@ -44,7 +45,7 @@ public class DriveCommands {
   private static final double ANGLE_MAX_VELOCITY = 8.0;
   private static final double ANGLE_MAX_ACCELERATION = 20.0;
   private static final double FF_START_DELAY = 2.0; // Secs
-  private static final double FF_RAMP_RATE = 0.1; // Volts/Sec
+  private static final double FF_RAMP_RATE = 0.75; // Volts/Sec
   private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec
   private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2
 
@@ -90,6 +91,7 @@ public static Command joystickDrive(
                   linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                   linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
                   omega * drive.getMaxAngularSpeedRadPerSec());
+
           boolean isFlipped =
               DriverStation.getAlliance().isPresent()
                   && DriverStation.getAlliance().get() == Alliance.Red;
@@ -214,9 +216,9 @@ public static Command feedforwardCharacterization(Drive drive) {
                   double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX);
 
                   NumberFormat formatter = new DecimalFormat("#0.00000");
-                  System.out.println("********** Drive FF Characterization Results **********");
-                  System.out.println("\tkS: " + formatter.format(kS));
-                  System.out.println("\tkV: " + formatter.format(kV));
+
+                  SmartDashboard.putString("kS Output", formatter.format(kS));
+                  SmartDashboard.putString("kV Output", formatter.format(kV));
                 }));
   }
 

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -225,6 +225,15 @@ private SwerveModuleState[] getModuleStates() {
     return states;
   }
 
+  @AutoLogOutput(key = "SwerveStates/MeasuredAbsolute")
+  private SwerveModuleState[] getAbsoluteModuleStates() {
+    SwerveModuleState[] states = new SwerveModuleState[4];
+    for (int i = 0; i < 4; i++) {
+      states[i] = modules[i].getAbsoluteState();
+    }
+    return states;
+  }
+
   /** Returns the module positions (turn angles and drive positions) for all of the modules. */
   private SwerveModulePosition[] getModulePositions() {
     SwerveModulePosition[] states = new SwerveModulePosition[4];

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -22,8 +22,8 @@
 public class DriveConstants {
   public static final double maxSpeedMetersPerSec = 4.8;
   public static final double odometryFrequency = 100.0; // Hz
-  public static final double trackWidth = Units.inchesToMeters(26.5);
-  public static final double wheelBase = Units.inchesToMeters(26.5);
+  public static final double trackWidth = Units.inchesToMeters(22.75);
+  public static final double wheelBase = Units.inchesToMeters(22.75);
   public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0);
   public static final Translation2d[] moduleTranslations =
       new Translation2d[] {
@@ -33,11 +33,17 @@ public class DriveConstants {
         new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0)
       };
 
-  // Zeroed rotation values for each module, see setup instructions
-  public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.1574540138244629);
-  public static final Rotation2d frontRightZeroRotation = new Rotation2d(-0.16320089995861053);
-  public static final Rotation2d backLeftZeroRotation = new Rotation2d(-2.942538261413574);
-  public static final Rotation2d backRightZeroRotation = new Rotation2d(2.5917508602142334);
+  //   // Zeroed rotation values for each module, see setup instructions
+  //   public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.07709961857367231);
+  //   public static final Rotation2d frontRightZeroRotation =
+  //       new Rotation2d(3.069966630478959);
+  //   public static final Rotation2d backLeftZeroRotation = new Rotation2d(1.633997476530679);
+  //   public static final Rotation2d backRightZeroRotation = new Rotation2d(1.273714169847585);
+
+  public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.16028737150729522);
+  public static final Rotation2d frontRightZeroRotation = new Rotation2d(-0.1422097592800296);
+  public static final Rotation2d backLeftZeroRotation = new Rotation2d(-3.009554996093968);
+  public static final Rotation2d backRightZeroRotation = new Rotation2d(2.559973505647124);
 
   // Device CAN ID
   public static final int frontLeftTurnCanId = 2;
@@ -72,15 +78,17 @@ public class DriveConstants {
       (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec
 
   // Drive PID configuration
-  public static final double driveKp = 0.0;
+  public static final double driveKp = 0.005;
   public static final double driveKd = 0.0;
-  public static final double driveKs = 0.0;
-  public static final double driveKv = 0.0;
+  public static final double driveKs = 0.19700;
+  public static final double driveKv = 0.12941;
   public static final double driveSimP = 0.05;
   public static final double driveSimD = 0.0;
   public static final double driveSimKs = 0.0;
   public static final double driveSimKv = 0.0789;
 
+  //   0.11891 0.13199
+
   // Turn motor configuration
   public static final boolean turnInverted = false;
   public static final int turnMotorCurrentLimit = 20;
@@ -95,8 +103,9 @@ public class DriveConstants {
       (2 * Math.PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec
 
   // Turn PID configuration
-  public static final double turnKp = 4000;
-  public static final double turnKd = 50;
+  public static final double turnKp = 2.0;
+  public static final double turnKd = 0.05;
+
   public static final double turnSimP = 8.0;
   public static final double turnSimD = 0.0;
   public static final double turnPIDMinInput = 0; // Radians

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -109,6 +109,10 @@ public SwerveModuleState getState() {
     return new SwerveModuleState(getVelocityMetersPerSec(), getAngle());
   }
 
+  public SwerveModuleState getAbsoluteState() {
+    return new SwerveModuleState(getVelocityMetersPerSec(), inputs.turnAbsolutePosition);
+  }
+
   /** Returns the module positions received this cycle. */
   public SwerveModulePosition[] getOdometryPositions() {
     return odometryPositions;

src/main/java/frc/robot/subsystems/drive/ModuleIO.java

@@ -26,6 +26,7 @@ public static class ModuleIOInputs {
     public double driveCurrentAmps = 0.0;
 
     public boolean turnConnected = false;
+    public Rotation2d turnAbsolutePosition = new Rotation2d();
     public Rotation2d turnPosition = new Rotation2d();
     public double turnVelocityRadPerSec = 0.0;
     public double turnAppliedVolts = 0.0;

src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java

@@ -59,6 +59,8 @@ public class ModuleIOSpark implements ModuleIO {
   private final Queue<Double> drivePositionQueue;
   private final Queue<Double> turnPositionQueue;
 
+  private boolean hasResetTurnPosition = false;
+
   // Connection debouncers
   private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
   private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
@@ -145,12 +147,11 @@ public ModuleIOSpark(int module) {
 
     // Configure turn motor
     var turnConfig = new SparkMaxConfig();
-
     turnConfig
-        .inverted(turnInverted)
         .idleMode(IdleMode.kCoast)
-        .smartCurrentLimit(turnMotorCurrentLimit)
-        .voltageCompensation(12.0);
+        .smartCurrentLimit(20)
+        .voltageCompensation(12.0)
+        .inverted(true);
     turnConfig
         .encoder
         .positionConversionFactor(turnEncoderPositionFactor)
@@ -161,9 +162,8 @@ public ModuleIOSpark(int module) {
         .closedLoop
         .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
         .positionWrappingEnabled(true)
-        .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput)
+        .positionWrappingInputRange(-Math.PI, Math.PI) // TODO
         .pidf(turnKp, 0.0, turnKd, 0.0);
-
     turnConfig
         .signals
         .primaryEncoderPositionAlwaysOn(true)
@@ -181,9 +181,6 @@ public ModuleIOSpark(int module) {
             turnSpark.configure(
                 turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
 
-    turnEncoder.setPosition(
-        Rotation2d.fromRadians(turnAbsoluteEncoder.get()).minus(zeroRotation).getRadians());
-
     // Create odometry queues
     timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
     drivePositionQueue =
@@ -218,6 +215,7 @@ public void updateInputs(ModuleIOInputs inputs) {
         (values) -> inputs.turnAppliedVolts = values[0] * values[1]);
     ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value);
     inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault);
+    inputs.turnAbsolutePosition = getOffsetAbsoluteAngle();
 
     // Update odometry inputs
     inputs.odometryTimestamps =
@@ -256,8 +254,17 @@ public void setDriveVelocity(double velocityRadPerSec) {
 
   @Override
   public void setTurnPosition(Rotation2d rotation) {
-    double setpoint =
-        MathUtil.inputModulus(rotation.plus(zeroRotation).getRadians(), -Math.PI, Math.PI);
-    turnController.setReference(setpoint, ControlType.kPosition);
+    if (!hasResetTurnPosition) {
+      turnEncoder.setPosition(getOffsetAbsoluteAngle().getRadians());
+      hasResetTurnPosition = true;
+    }
+
+    // Ensure Setpoint is within range
+    turnController.setReference(
+        MathUtil.angleModulus(rotation.getRadians()), ControlType.kPosition);
+  }
+
+  private Rotation2d getOffsetAbsoluteAngle() {
+    return Rotation2d.fromRadians(turnAbsoluteEncoder.get()).minus(zeroRotation);
   }
 }