maybe it works?

CurtinFRC · Jan 23, 2025 · d84b442 · d84b442
1 parent 2e9af7a
commit d84b442
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 71 deletions.
diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java
@@ -2,29 +2,27 @@
 
 public class ElevatorConstants {
   // reduction for elevator neo is 10.7 : 1
+  // TODO: GET VALUES FOR SOME OF THESE
   public static int motorCount = 1; // Number of motors in the elevator system
-  public static int distanceSensorPort = 0; // Port for distance sensor (if used)
+  public static int distanceSensorPort = 0; // Port for distance sensor (if used) 
   public static int motorPort = 1; // Motor controller port
   public static double tolerance = 0.01; // Velocity tolerance (rotations per second)
   public static double positionTolerance = 0.002; // Position tolerance (rotations)
-  public static double friction = 0.05; // Estimated system friction (Nm)
+  public static double friction = 0.05; // Estimated system friction (Nm) TODO
   public static double stableVelocityThreshold =
       0.005; // Threshold for velocity to be considered stable
 
   // Motion profile constraints
   public static double maxVel = 5676 / 10.7; // Max velocity in RPM
-  public static double maxAccel = 38197; // Max acceleration in RPM per second
+  public static double maxAccel = 38197; // Max acceleration in RPM per second 
   public static double allowedErr = 0.002; // Allowable position error for stability
 
   // Model Predictive Control Gains
-  public static double kP = 6; // Proportional gain
-  public static double kI = 0.01; // Integral gain
-  public static double kD = 0.1; // Derivative gain
+  public static double kP = 6; // Proportional gain TODO
+  public static double kI = 0.01; // Integral gain TODO
+  public static double kD = 0.1; // Derivative gain TODO
   public static double kA = 0.3;
   public static double kMinOutput = -12.0; // Minimum voltage output
   public static double kMaxOutput = 12.0; // Maximum voltage output
   public static double kV = 473; // Voltage constant (i for it from the datasheet)
-
-  public static double fallbackKP = 0.1; // Fallback proportional gain
-  public static double fallbackKD = 0.02; // Fallback derivative gain
 }
diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java
@@ -19,7 +19,6 @@ public static class ElevatorIOInputs {
     public double velocityError;
     public boolean stable;
     public int predictionHorizon;
-    public boolean useFallbackPID;
     public double predictedPosition;
     public double predictedVelocity;
   }

diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMPC.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIONeoMPC.java
@@ -4,9 +4,8 @@
 import org.curtinfrc.frc2025.Constants.Setpoints;
 import org.curtinfrc.frc2025.util.Util;
 
-/** Simulated Elevator implementation using Model Predictive Control (MPC). */
 public class ElevatorIONeoMPC extends ElevatorIONeo {
-  private final double controlLoopPeriod = 0.02; // 50 Hz simulation step
+  private final double controlLoopPeriod = 0.02; 
 
   private Setpoints set = Setpoints.COLLECT;
   private double computedVoltage = 0.0;
@@ -16,43 +15,32 @@ public class ElevatorIONeoMPC extends ElevatorIONeo {
 
   @Override
   public void updateInputs(ElevatorIOInputs inputs) {
-    // Simulate sensor readings and MPC state
-    inputs.distanceSensorReading = 0.0; // Distance sensor is not used in this simulation
-    inputs.encoderReading = elevatorEncoder.getPosition(); // Simulated encoder reading
-    inputs.point = set; // Current setpoint
+    inputs.distanceSensorReading = 0.0; 
+    inputs.encoderReading = elevatorEncoder.getPosition(); 
+    inputs.point = set; 
     inputs.pointRot =
-        ElevatorIONeoMaxMotion.convertSetpoint(set.elevatorSetpoint()); // Setpoint in rotations
+        ElevatorIONeoMaxMotion.convertSetpoint(set.elevatorSetpoint()); 
 
-    // Voltage applied to the motor (computed by the MPC)
     inputs.motorVoltage = computedVoltage;
 
-    // Simulated motor properties
-    inputs.motorCurrent = elevatorMotor.getOutputCurrent(); // Simulated motor current
-    inputs.motorTemp = 25.0; // Assume constant motor temperature for the simulation
-    inputs.motorVelocity = elevatorEncoder.getVelocity() / 60.0; // Convert RPM to RPS
+    inputs.motorCurrent = elevatorMotor.getOutputCurrent(); 
+    inputs.motorTemp = elevatorMotor.getMotorTemperature(); 
+    inputs.motorVelocity = elevatorEncoder.getVelocity() / 60.0; 
 
-    // Position error: difference between current position and target
     inputs.positionError =
         Math.abs(
             elevatorEncoder.getPosition()
                 - ElevatorIONeoMaxMotion.convertSetpoint(set.elevatorSetpoint()));
 
-    // Stability check based on position and velocity thresholds
     inputs.stable = isStable();
 
-    // Prediction horizon used by the MPC
     inputs.predictionHorizon = predictionHorizon;
 
-    // Fallback PID status (disabled in this simulation)
-    inputs.useFallbackPID = false;
-
-    // Simulated predicted position and velocity
     inputs.predictedPosition =
-        elevatorEncoder.getPosition(); // Predicted position from simulation
+        elevatorEncoder.getPosition(); 
     inputs.predictedVelocity =
-        elevatorEncoder.getVelocity() / 60.0; // Predicted velocity in RPS
+        elevatorEncoder.getVelocity() / 60.0; 
 
-    // Velocity error (difference between desired and actual velocity)
     inputs.velocityError = velocityError;
   }
 
@@ -62,14 +50,13 @@ public void goToSetpoint(Setpoints point) {
 
     double targetPosition = ElevatorIONeoMaxMotion.convertSetpoint(point.elevatorSetpoint());
     double currentPosition = elevatorEncoder.getPosition();
-    double velocity = elevatorEncoder.getVelocity() / 60.0; // Convert RPM to RPS
+    double velocity = elevatorEncoder.getVelocity() / 60.0; 
 
     adjustPredictionHorizon(currentPosition, targetPosition);
     generateReferenceTrajectory(targetPosition);
 
     computedVoltage = computeMPCControl(currentPosition, velocity);
 
-    // Apply clamping and deadband logic
     computedVoltage =
         clampVoltageForDeadband(
             computedVoltage, Math.abs(targetPosition - currentPosition), velocity);
@@ -78,7 +65,7 @@ public void goToSetpoint(Setpoints point) {
     // reduceVoltageNearSetpoint(computedVoltage, Math.abs(targetPosition - currentPosition));
 
     if (Math.abs(targetPosition - currentPosition) < ElevatorConstants.positionTolerance) {
-      computedVoltage = 0; // Force motor to stop
+      computedVoltage = 0; 
     }
 
     elevatorMotor.setVoltage(computedVoltage);
@@ -103,7 +90,7 @@ private void generateReferenceTrajectory(double targetPosition) {
     for (int i = 0; i < predictionHorizon; i++) {
       double factor = (double) i / (predictionHorizon - 1);
       referenceTrajectory[i] =
-          currentPosition + distance * Math.pow(factor, 2); // Quadratic slowdown
+          currentPosition + distance * Math.pow(factor, 2); 
     }
   }
 
@@ -133,7 +120,7 @@ private double computeMPCControl(double currentPosition, double velocity) {
       if (Math.abs(positionError) < ElevatorConstants.positionTolerance) {
         stepVoltage *= Math.abs(positionError) / ElevatorConstants.positionTolerance;
         if (Math.abs(stepVoltage) < 0.1) {
-          stepVoltage = 0; // Stop applying voltage when near the setpoint
+          stepVoltage = 0; 
         }
       }
 
@@ -149,7 +136,7 @@ private double computeMPCControl(double currentPosition, double velocity) {
 
       if (Math.abs(predictedPosition[i] - referenceTrajectory[i])
           < ElevatorConstants.positionTolerance) {
-        predictedVelocity[i] = 0; // Stop when the position is close to the target
+        predictedVelocity[i] = 0; 
       }
 
       totalVoltage = stepVoltage;
@@ -177,18 +164,18 @@ private boolean isWithinDeadband(double positionError, double velocity) {
 
   private double clampVoltageForDeadband(double voltage, double positionError, double velocity) {
     if (isWithinDeadband(positionError, velocity)) {
-      return 0.0; // Stop applying voltage when within deadband
+      return 0.0; 
     }
     return voltage;
   }
 
   private double reduceVoltageNearSetpoint(double voltage, double positionError) {
     if (positionError < ElevatorConstants.positionTolerance) {
-      return 0.0; // Stop voltage entirely near the setpoint
+      return 0.0; 
     }
     double scalingFactor =
         Math.max(0.1, 1.0 - Math.abs(positionError) / (2 * ElevatorConstants.positionTolerance));
-    return voltage * scalingFactor; // Scale voltage more aggressively near setpoint
+    return voltage * scalingFactor; 
   }
 
   private double clampVoltage(double voltage) {

diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSimMPC.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSimMPC.java
@@ -6,19 +6,17 @@
 import org.curtinfrc.frc2025.Constants.Setpoints;
 import org.curtinfrc.frc2025.util.Util;
 
-/** Simulated Elevator implementation using Model Predictive Control (MPC). */
 public class ElevatorIOSimMPC implements ElevatorIO {
   private final DCMotor motor = DCMotor.getNEO(ElevatorConstants.motorCount);
   private final DCMotorSim elevatorSim;
-  private final double controlLoopPeriod = 0.02; // 50 Hz simulation step
+  private final double controlLoopPeriod = 0.02; 
 
   private Setpoints set = Setpoints.COLLECT;
   private double computedVoltage = 0.0;
   private int predictionHorizon = 20;
   private double[] referenceTrajectory = new double[20];
   private double velocityError = 0;
 
-  /** Constructor to initialize the simulated elevator with MPC. */
   public ElevatorIOSimMPC() {
     elevatorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(motor, 0.025, 4.0), motor);
   }
@@ -27,43 +25,32 @@ public ElevatorIOSimMPC() {
   public void updateInputs(ElevatorIOInputs inputs) {
     elevatorSim.update(controlLoopPeriod);
 
-    // Simulate sensor readings and MPC state
-    inputs.distanceSensorReading = 0.0; // Distance sensor is not used in this simulation
-    inputs.encoderReading = elevatorSim.getAngularPositionRotations(); // Simulated encoder reading
-    inputs.point = set; // Current setpoint
+    inputs.distanceSensorReading = 0.0; 
+    inputs.encoderReading = elevatorSim.getAngularPositionRotations(); 
+    inputs.point = set; 
     inputs.pointRot =
-        ElevatorIONeoMaxMotion.convertSetpoint(set.elevatorSetpoint()); // Setpoint in rotations
+        ElevatorIONeoMaxMotion.convertSetpoint(set.elevatorSetpoint()); 
 
-    // Voltage applied to the motor (computed by the MPC)
     inputs.motorVoltage = computedVoltage;
 
-    // Simulated motor properties
-    inputs.motorCurrent = elevatorSim.getCurrentDrawAmps(); // Simulated motor current
-    inputs.motorTemp = 25.0; // Assume constant motor temperature for the simulation
+    inputs.motorCurrent = elevatorSim.getCurrentDrawAmps(); 
+    inputs.motorTemp = 25.0; 
     inputs.motorVelocity = elevatorSim.getAngularVelocityRPM() / 60.0; // Convert RPM to RPS
 
-    // Position error: difference between current position and target
     inputs.positionError =
         Math.abs(
             elevatorSim.getAngularPositionRotations()
                 - ElevatorIONeoMaxMotion.convertSetpoint(set.elevatorSetpoint()));
 
-    // Stability check based on position and velocity thresholds
     inputs.stable = isStable();
 
-    // Prediction horizon used by the MPC
     inputs.predictionHorizon = predictionHorizon;
 
-    // Fallback PID status (disabled in this simulation)
-    inputs.useFallbackPID = false;
-
-    // Simulated predicted position and velocity
     inputs.predictedPosition =
-        elevatorSim.getAngularPositionRotations(); // Predicted position from simulation
+        elevatorSim.getAngularPositionRotations(); 
     inputs.predictedVelocity =
-        elevatorSim.getAngularVelocityRPM() / 60.0; // Predicted velocity in RPS
+        elevatorSim.getAngularVelocityRPM() / 60.0; 
 
-    // Velocity error (difference between desired and actual velocity)
     inputs.velocityError = velocityError;
   }
 
@@ -80,7 +67,6 @@ public void goToSetpoint(Setpoints point) {
 
     computedVoltage = computeMPCControl(currentPosition, velocity);
 
-    // Apply clamping and deadband logic
     computedVoltage =
         clampVoltageForDeadband(
             computedVoltage, Math.abs(targetPosition - currentPosition), velocity);
@@ -89,7 +75,7 @@ public void goToSetpoint(Setpoints point) {
     // reduceVoltageNearSetpoint(computedVoltage, Math.abs(targetPosition - currentPosition));
 
     if (Math.abs(targetPosition - currentPosition) < ElevatorConstants.positionTolerance) {
-      computedVoltage = 0; // Force motor to stop
+      computedVoltage = 0; 
     }
 
     elevatorSim.setInputVoltage(computedVoltage);
@@ -114,7 +100,7 @@ private void generateReferenceTrajectory(double targetPosition) {
     for (int i = 0; i < predictionHorizon; i++) {
       double factor = (double) i / (predictionHorizon - 1);
       referenceTrajectory[i] =
-          currentPosition + distance * Math.pow(factor, 2); // Quadratic slowdown
+          currentPosition + distance * Math.pow(factor, 2); 
     }
   }
 
@@ -144,7 +130,7 @@ private double computeMPCControl(double currentPosition, double velocity) {
       if (Math.abs(positionError) < ElevatorConstants.positionTolerance) {
         stepVoltage *= Math.abs(positionError) / ElevatorConstants.positionTolerance;
         if (Math.abs(stepVoltage) < 0.1) {
-          stepVoltage = 0; // Stop applying voltage when near the setpoint
+          stepVoltage = 0; 
         }
       }
 
@@ -160,7 +146,7 @@ private double computeMPCControl(double currentPosition, double velocity) {
 
       if (Math.abs(predictedPosition[i] - referenceTrajectory[i])
           < ElevatorConstants.positionTolerance) {
-        predictedVelocity[i] = 0; // Stop when the position is close to the target
+        predictedVelocity[i] = 0; 
       }
 
       totalVoltage = stepVoltage;
@@ -188,18 +174,18 @@ private boolean isWithinDeadband(double positionError, double velocity) {
 
   private double clampVoltageForDeadband(double voltage, double positionError, double velocity) {
     if (isWithinDeadband(positionError, velocity)) {
-      return 0.0; // Stop applying voltage when within deadband
+      return 0.0; 
     }
     return voltage;
   }
 
   private double reduceVoltageNearSetpoint(double voltage, double positionError) {
     if (positionError < ElevatorConstants.positionTolerance) {
-      return 0.0; // Stop voltage entirely near the setpoint
+      return 0.0; 
     }
     double scalingFactor =
         Math.max(0.1, 1.0 - Math.abs(positionError) / (2 * ElevatorConstants.positionTolerance));
-    return voltage * scalingFactor; // Scale voltage more aggressively near setpoint
+    return voltage * scalingFactor; 
   }
 
   private double clampVoltage(double voltage) {