feat: manual elevator now runs motion magic PIDs

Author: Brandon-Harad

src/main/java/org/team1540/robot2024/Constants.java

@@ -129,10 +129,10 @@ public static class Elevator {
         public static final double KI = 0;
         public static final double KD = 0.1;
         public static final double KG = 0;
-        public static final int CRUISE_VELOCITY_MPS = 80;
-        public static final int MAXIMUM_ACCELERATION_MPS2 = 160;
-        public static final int JERK_MP3 = 1600;
-        public static final double SPROCKET_CIRCUMFERENCE = .0508 * Math.PI;
+        public static final int CRUISE_VELOCITY_MPS = 2;
+        public static final int MAXIMUM_ACCELERATION_MPS2 = 20;
+        public static final int JERK_MP3 = 40;
+        public static final double SPROCKET_CIRCUMFERENCE = .044 * Math.PI;
         public static final double MOTOR_ROTS_TO_METERS = GEAR_RATIO * SPROCKET_CIRCUMFERENCE;
         public static final double ERROR_TOLERANCE = 0.03;
         public static final double SIM_CARRIAGE_MASS_KG = 1.55; //TODO: check this number :)

src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java

@@ -19,7 +19,8 @@ public ElevatorManualCommand(Elevator elevator, CommandXboxController copilot) {
 
     @Override
     public void execute() {
-        elevator.setVoltage(copilot.getRightTriggerAxis() - copilot.getLeftTriggerAxis() + Constants.Elevator.KG);
+        // elevator.setVoltage(copilot.getRightTriggerAxis() - copilot.getLeftTriggerAxis() + Constants.Elevator.KG);
+        elevator.goToSetpoint(elevator.getSetpoint() + Constants.Elevator.CRUISE_VELOCITY_MPS*2/100*(copilot.getRightTriggerAxis()-copilot.getLeftTriggerAxis()));
     }
 
     @Override

src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java

@@ -18,7 +18,6 @@ public class Elevator extends SubsystemBase {
 
     public Elevator(ElevatorIO elevatorIO) {
         this.elevatorIO = elevatorIO;
-        setVoltage(10);
     }
 
     // periodic
@@ -48,4 +47,8 @@ public void setVoltage(double voltage) {
     public void stop() {
         elevatorIO.setVoltage(0.0);
     }
+
+    public double getSetpoint() {
+        return setpointMeters;
+    }
 }