scale velocity by position

Talon540Programming · Mar 16, 2024 · 027a819 · 027a819
1 parent ca84f66
commit 027a819
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 39 deletions.
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java
@@ -95,7 +95,7 @@ public void periodic() {
       OdometryQueueManager.getInstance().updateTimestampsInput(m_odometryTimestampInputs);
       m_gyroIO.updateInputs(m_gyroInputs);
       for (var module : m_modules) {
-        module.updateInputs();
+        module.periodic();
       }
     } finally {
       PoseEstimator.odometryLock.unlock();

diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -54,12 +54,15 @@ public class Module {
   // Calculates the drive motor's feedforward value in volts/radians per second
   private SimpleMotorFeedforward m_driveFeedforward;
 
+  private Rotation2d angleSetpoint;
+  private Double velocitySetpointRadsPerSecond;
+
   public Module(int index, ModuleIO io) {
     moduleIndex = index;
     m_io = io;
   }
 
-  public void updateInputs() {
+  public void periodic() {
     m_io.updateInputs(m_inputs);
 
     LoggedTunableNumber.ifChanged(
@@ -79,14 +82,31 @@ public void updateInputs() {
         turnKp,
         turnKi,
         turnKd);
+
+    if (angleSetpoint == null) {
+      return;
+    }
+
+    m_io.runTurnAngleSetpoint(angleSetpoint);
+
+    if (velocitySetpointRadsPerSecond == null) {
+      return;
+    }
+
+    // Increase module velocity as delta angle approaches zero
+    double scaledVelocitySetpointRadsPerSec =
+        velocitySetpointRadsPerSecond * angleSetpoint.minus(m_inputs.turnPositionRad).getCos();
+    m_io.runDriveVelocitySetpoint(
+        scaledVelocitySetpointRadsPerSec,
+        m_driveFeedforward.calculate(scaledVelocitySetpointRadsPerSec));
   }
 
   public void processInputs() {
     Logger.processInputs("Drive/Module" + moduleIndex, m_inputs);
   }
 
   public Rotation2d getAngle() {
-    return m_inputs.turnPosition;
+    return m_inputs.turnPositionRad;
   }
 
   public double getDrivePositionRad() {
@@ -106,11 +126,11 @@ public double getDriveVelocityMetersPerSec() {
   }
 
   public SwerveModulePosition getCurrentPosition() {
-    return new SwerveModulePosition(getDrivePositionMeters(), m_inputs.turnPosition);
+    return new SwerveModulePosition(getDrivePositionMeters(), m_inputs.turnPositionRad);
   }
 
   public SwerveModuleState getCurrentState() {
-    return new SwerveModuleState(getDriveVelocityMetersPerSec(), m_inputs.turnPosition);
+    return new SwerveModuleState(getDriveVelocityMetersPerSec(), m_inputs.turnPositionRad);
   }
 
   public SwerveModulePosition[] getModuleDeltas() {
@@ -120,18 +140,18 @@ public SwerveModulePosition[] getModuleDeltas() {
   public SwerveModuleState runSetpoint(SwerveModuleState setpoint) {
     var optimizedSetpoint = SwerveModuleState.optimize(setpoint, getAngle());
 
-    // TODO handle cos position error scaling
-    double setpointVelocityRadPerSec =
+    this.angleSetpoint = optimizedSetpoint.angle;
+    this.velocitySetpointRadsPerSecond =
         optimizedSetpoint.speedMetersPerSecond / DriveBase.kWheelRadiusMeters;
-    m_io.runDriveVelocitySetpoint(
-        setpointVelocityRadPerSec, m_driveFeedforward.calculate(setpointVelocityRadPerSec));
-    m_io.runTurnAngleSetpoint(optimizedSetpoint.angle);
 
     return optimizedSetpoint;
   }
 
   public void runCharacterizationVoltage(Rotation2d angleSetpoint, double voltage) {
+    this.angleSetpoint = angleSetpoint;
     m_io.runTurnAngleSetpoint(angleSetpoint);
+
+    this.velocitySetpointRadsPerSecond = null;
     m_io.runDriveVolts(voltage);
   }
 

diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
@@ -24,9 +24,6 @@ public class ModuleIOSim implements ModuleIO {
 
   private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI);
 
-  private Double velocitySetpoint = null;
-  private double driveFeedForwardVolts;
-  private Rotation2d angleSetpoint = null;
   private double driveAppliedVolts = 0.0;
   private double turnAppliedVolts = 0.0;
 
@@ -63,26 +60,6 @@ public void updateInputs(ModuleIOInputs inputs) {
               // Convert from radians to meters
               inputs.drivePositionRad * DriveBase.kWheelRadiusMeters, inputs.turnPositionRad)
         };
-
-    // PID setpoints need to be constantly calculated to mirror how the SparkMax handles the onboard
-    // PID controller
-    if (velocitySetpoint != null) {
-      runDriveVolts(
-          // PID coefficients are in terms of the motor's output, not the module as a whole, scale
-          // the
-          // inputs to account for this
-          m_driveController.calculate(m_driveSim.getAngularVelocityRadPerSec(), velocitySetpoint)
-              + driveFeedForwardVolts);
-    }
-    if (angleSetpoint != null) {
-      runTurnVolts(
-          // PID coefficients are in terms of the motor's output, not the module as a whole, scale
-          // the
-          // inputs to account for this
-          m_turnController.calculate(
-              MathUtil.angleModulus(m_turnSim.getAngularPositionRad()),
-              angleSetpoint.getRadians()));
-    }
   }
 
   @Override
@@ -109,12 +86,15 @@ public void setTurnPID(double kP, double kI, double kD) {
 
   @Override
   public void runDriveVelocitySetpoint(double velocityRadPerSec, double driveFeedForwardVolts) {
-    velocitySetpoint = velocityRadPerSec;
-    this.driveFeedForwardVolts = driveFeedForwardVolts;
+    runDriveVolts(
+        m_driveController.calculate(m_driveSim.getAngularVelocityRadPerSec(), velocityRadPerSec)
+            + driveFeedForwardVolts);
   }
 
   @Override
   public void runTurnAngleSetpoint(Rotation2d angle) {
-    angleSetpoint = angle;
+    runTurnVolts(
+        m_turnController.calculate(
+            MathUtil.angleModulus(m_turnSim.getAngularPositionRad()), angle.getRadians()));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java
@@ -200,9 +200,7 @@ public void runTurnAngleSetpoint(Rotation2d angle) {
       hasResetPosition = true;
     }
 
-    m_turnController.setReference(
-        // Convert from module relative to motor relative
-        angle.getRadians(), CANSparkBase.ControlType.kPosition);
+    m_turnController.setReference(angle.getRadians(), CANSparkBase.ControlType.kPosition);
   }
 
   @Override