feat: add an elevatorIO method for setting setpoint pose /w setpoint accel

Signed-off-by: Dasun Abeykoon <[email protected]>

Author: dabeycorn

src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/ElevatorIO.java

@@ -73,7 +73,14 @@ public interface ElevatorIO extends AutoCloseable {
    * Set parent/child motors position setpoint /w acceleration setpoint
    *
    * @param poseMeters desired position setpoint in meters
-   * @param accelerationMpsSqrd desired acceleration setpoint in meters per second per second
+   * @param accelMpsSqrd desired acceleration setpoint in meters per second per second
    */
-  public void setPose(double poseMeters, double accelerationMpsSqrd);
+  public void setPose(double poseMeters, double accelMpsSqrd);
+
+  /**
+   * Set parent/child motors position setpoint
+   *
+   * @param poseMeters desired position setpoint in meters
+   */
+  public void setPose(double poseMeters);
 }

src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/ElevatorIONone.java

@@ -55,7 +55,10 @@ public void resetEncoders(double poseMeters) {}
   public void setVolts(double volts) {}
 
   @Override
-  public void setPose(double poseMeters, double accelerationMpsSqrd) {}
+  public void setPose(double poseMeters, double accelMpsSqrd) {}
+
+  @Override
+  public void setPose(double poseMeters) {}
 
   @Override
   public void close() throws Exception {}

src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/ElevatorIOSim.java

@@ -8,7 +8,7 @@
 
 import static edu.wpi.first.units.Units.Kilograms;
 import static edu.wpi.first.units.Units.Meters;
-import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
 import static org.frc6423.robot.subsystems.superstructure.elevator.Elevator.*;
 
 import edu.wpi.first.math.MathUtil;
@@ -37,7 +37,7 @@ public class ElevatorIOSim implements ElevatorIO {
 
   private final ElevatorFeedforward feedforward = new ElevatorFeedforward(0.0, 0.1265, 0.4, 0.0);
   private final ProfiledPIDController feedback =
-      new ProfiledPIDController(15.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0));
+      new ProfiledPIDController(15.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2.25, 10.0));
 
   public ElevatorIOSim() {
     SmartDashboard.putData(feedback);
@@ -92,17 +92,14 @@ public void setVolts(double volts) {
     appliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
   }
 
-  @Override
-  public void setPose(double poseMeters, double accelerationMpsSqrd) {
+  public void setPose(double poseMeters, double accelMpsSqrd) {
     // Clamp pose in range
     poseMeters = MathUtil.clamp(poseMeters, 0.0, MAX_EXTENSION_HEIGHT.in(Meters));
     // Get current setpoint velocity
     var currentVel = feedback.getSetpoint().velocity;
 
     // Give feedback controller new accel
-    feedback.setConstraints(
-        new TrapezoidProfile.Constraints(
-            MAX_VELOCITY.in(MetersPerSecond) / 2, accelerationMpsSqrd));
+    feedback.setConstraints(new TrapezoidProfile.Constraints(2.25, accelMpsSqrd));
     // Calculate next feedback setpoint
     var fbOut = feedback.calculate(getParentPoseMeters(), poseMeters);
     // Get next velocity
@@ -111,9 +108,15 @@ public void setPose(double poseMeters, double accelerationMpsSqrd) {
     // Calculate feedforward velocity output
     var ffOut = feedforward.calculateWithVelocities(currentVel, nextVel);
 
+    // Combine output
     setVolts(ffOut + fbOut);
   }
 
+  @Override
+  public void setPose(double poseMeters) {
+    setPose(poseMeters, MAX_ACCELERATION.in(MetersPerSecondPerSecond));
+  }
+
   @Override
   public void close() throws Exception {}
 }