constants, sparkmax

Author: AlbertAlwaysWins

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -9,7 +9,7 @@
 
 
 
-public class Elevator extends SubsystemBase{
+public class Elevator extends SubsystemBase {
 
 
   private final ElevatorIO io;
@@ -18,7 +18,28 @@ public class Elevator extends SubsystemBase{
 
 
   public Elevator(ElevatorIO io) {
+    
     this.io = io;
+
+    SysId =
+    new SysIdRoutine(
+        new SysIdRoutine.Config(
+            Volts.per(Second).of(ElevatorConstants.SYSID_RAMP_RATE),
+            Volts.of(ElevatorConstants.SYSID_STEP_VOLTAGE),
+            Seconds.of(ElevatorConstants.SYSID_TIME),
+            (state) -> Logger.recordOutput("Elevator/SysIdTestState", state.toString())),
+        new SysIdRoutine.Mechanism(
+            v -> io.setVoltage(v.in(Volts)),
+            (sysidLog) -> {
+              sysidLog
+                  .motor("Elevator")
+                  .voltage(m_appliedVoltage.mut_replace(inputs.appliedVoltage, Volts))
+                  .linearPosition(m_position.mut_replace(inputs.positionMeters, Meters))
+                  .linearVelocity(
+                      m_velocity.mut_replace(inputs.velocityMetersPerSec, MetersPerSecond));
+            },
+            this));
+}
   }
 
 
@@ -27,6 +48,15 @@ public void periodic() {
   io.updateInputs(ElevatorInputs);
 }
 
+// Sets the PID setpoint 
+public void setPID(double setpoint) {
+  if (setpoint > ElevatorConstants.ELEVATOR_MAX_HEIGHT)
+    setpoint = ElevatorConstants.ELEVATOR_MAX_HEIGHT;
+  else if (setpoint < ElevatorConstants.ELEVATOR_MIN_HEIGHT)
+    setpoint = ElevatorConstants.ELEVATOR_MIN_HEIGHT;
+  io.setSetpoint(setpoint);
+}
+
 public Command runVoltageCommand(Supplier<Voltage> voltage) {
     return run(() -> io.setVoltage(voltage.get())).withName("Voltage");
   }
@@ -38,4 +68,3 @@ public Command runRPMCommand(Supplier<AngularVelocity> rpm) {
 
 
 
-}

src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java

@@ -11,6 +11,12 @@ public class ElevatorConstants {
     public static final int LIMIT_SWITCH_CHANNEL = 0;
     public static double POSITION_CONVERSION_FACTOR = 0.0;
     public static final boolean MOTOR_DEFAULT_IDLE_MODE = true;
+    // MAX AND MIN HEIGHTS
+    public static final double ELEVATOR_MAX_HEIGHT = 1.0; // meters
+    public static final double ELEVATOR_MIN_HEIGHT = 0.005; 
 
+    // MAX velocity and acceleration
+    public static final double MAX_ACCELERATION = 4.0; // m/s^2
+    public static final double MAX_VELOCITY = 2.0; 
 
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java

@@ -41,6 +41,10 @@ default double getVelocity(){
         return 0.0;
     }
 
+    default double getAppliedVoltage(){
+        return 0.0;
+    }
+
     default void setRPM(double rpm){}
 
     // Checking if elevator goes above limit height

src/main/java/frc/robot/subsystems/elevator/ElevatorIOSparkMax

@@ -39,10 +39,12 @@ public class ElevatorIOSparkMax {
   public ElevatorIOSparkMax() {
     leftMotor = new SparkMax(ElevatorConstants.LEFT_MOTOR_ID, MotorType.kBrushless);
     rightMotor = new SparkMax(ElevatorConstants.RIGHT_MOTOR_ID, MotorType.kBrushless);
-
+    // encoders have the same height, since elevator moves up and down
     leftEncoder = leftMotor.getEncoder();
     leftEncoder.setPosition(0);
 
+
+    // Configs were borrowed from previous code :)
     SparkMaxConfig leftConfig = new SparkMaxConfig();
     leftConfig
         .smartCurrentLimit(Constants.NEO_CURRENT_LIMIT)
@@ -57,12 +59,6 @@ public class ElevatorIOSparkMax {
     SparkMaxConfig rightConfig = new SparkMaxConfig();
     rightConfig.apply(leftConfig);
     rightConfig.follow(leftMotor, true);
-
-    // reset safe kResetSafeParameters switches the motor to default paramaters, then adds the
-    // changes from the config object
-    // persist paramaters saves these changes to the motor memory so it doesn't get cooked during
-    // brownouts
-    // only use persist in the BEGINNING, not later
     leftMotor.configure(leftConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
     rightMotor.configure(
         rightConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
@@ -74,11 +70,64 @@ public class ElevatorIOSparkMax {
   }
 
   private void updateMotorConfig(SparkMaxConfig config) {
-    // DO NOT RESET paramaters becasue we only want to change some paramaters, not all
-    // DO NOT PERSIST because this is a temporary change that we don't want to save to memory
     leftMotor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
     rightMotor.configure(
         config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
   }
+
+  @Override
+  public void updateInputs(ElevatorIOInputs inputs) {
+    inputs.setpointMeters = getSetpoint();
+    inputs.positionMeters = getPosition();
+    inputs.velocityMetersPerSec = getVelocity();
+    inputs.appliedVoltage = getAppliedVoltage();
+    inputs.isLimitSwitchPressed = isLimitSwitchPressed();
+    
+  }
+
+  @Override 
+  public double getSetpoint() {
+    return pidController.getSetpoint().position;
+}
+
+  @Override 
+  public double getPosition {
+    return leftEncoder.getPosition();
+  }
+
+  @Override 
+  public double getVelocity {
+    return leftEncoder.getVelocity();
+  }
+
+  @Override 
+  public double getAppliedVoltage {
+    return leftMotor.getAppliedOutput() * leftMotor.getBusVoltage();
+  }
+
+  @Override
+  public boolean isLimitSwitchPressed() {
+    return !limitSwitch.get();
+  }
+
+
+  @Override 
+  public void setSetpoint(double setpoint) {
+    pidController.setGoal(MathUtil.clamp(setpoint, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT));
+  }
+
+  @Override 
+  public void goToSetpoint() { 
+    double pidOutput = pid.Controller.calculate(getPosition());
+   
+    double ffOutput = feedforward.calculate(pidController.getSetpoint().velocity, pidController.getSetpoint().acceleration);
+
+    double acceleration = pidController.getSetpoint().acceleration;
+
+    setVoltage(math.util.clamp(pidOutput, -ElevatorConstants.MAX_VOLTAGE, ElevatorConstants.MAX_VOLTAGE));
+  }
+
+  
+  
 }
 }