Elevator subsystem

Co-authored-by: ambers7 <[email protected]>
Co-authored-by: reyamiller <[email protected]>
Co-authored-by: ilovemarmot <[email protected]>

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -12,4 +12,12 @@ public interface Gamepad {
         int OPERATOR = 1;
         int DEBUGGER = 2;
     }
+
+    public interface Elevator {
+        int LEFT = 0;
+        int RIGHT = 0;
+
+        int TOP_LIMIT_SWITCH = 0;
+        int BOTTOM_LIMIT_SWITCH = 0;
+    }
 }

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -5,7 +5,6 @@
 
 package com.stuypulse.robot.constants;
 
-import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 
 /*-
@@ -14,4 +13,29 @@
  * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable
  * values that we can edit on Shuffleboard.
  */
-public interface Settings {}
+public interface Settings {
+    public interface Elevator {
+        double MIN_HEIGHT = 0.0;
+        double MAX_HEIGHT = 0.0;
+
+        SmartNumber VEL_LIMIT = new SmartNumber("Velocity Limit", 0);
+        SmartNumber ACC_LIMIT = new SmartNumber("Acceleration Limit",0);
+
+        public interface Encoder {
+            double ENCODER_MULTIPLIER = 1;
+        }
+    }
+
+    public interface PID {
+        SmartNumber kP = new SmartNumber("kP", 0);
+        SmartNumber kI = new SmartNumber("kI", 0);
+        SmartNumber kD = new SmartNumber("kD", 0);
+    }
+
+    public interface FeedForward {
+        SmartNumber kG = new SmartNumber("kG", 0);
+        SmartNumber kS = new SmartNumber("kS", 0);
+        SmartNumber kV = new SmartNumber("kV", 0);
+        SmartNumber kA = new SmartNumber("kA", 0);
+    }
+}

src/main/java/com/stuypulse/robot/subsystems/Elevator.java

@@ -0,0 +1,116 @@
+package com.stuypulse.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.stuylib.control.Controller;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.control.feedforward.ElevatorFeedforward;
+import com.stuypulse.stuylib.network.SmartNumber;
+import com.stuypulse.stuylib.streams.filters.MotionProfile;
+
+import com.revrobotics.RelativeEncoder;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import static com.stuypulse.robot.constants.Settings.Elevator.*;
+import static com.stuypulse.robot.constants.Settings.Elevator.Encoder.*;
+import static com.stuypulse.robot.constants.Settings.PID.*;
+
+import static com.stuypulse.robot.constants.Settings.FeedForward.*;
+
+public class Elevator {
+    // hardware
+    public CANSparkMax leftMotor;
+    public CANSparkMax rightMotor;
+
+    public RelativeEncoder leftEncoder;
+    public RelativeEncoder rightEncoder;
+
+    public DigitalInput topLimit;
+    public DigitalInput bottomLimit;
+
+    // control
+    public Controller position;
+
+    public SmartNumber targetHeight;
+
+    public Elevator() {
+        // motors
+        leftMotor = new CANSparkMax(Ports.Elevator.LEFT, MotorType.kBrushless);
+        rightMotor = new CANSparkMax(Ports.Elevator.RIGHT, MotorType.kBrushless);
+
+        // encoders
+        leftEncoder = leftMotor.getEncoder();
+        rightEncoder = rightMotor.getEncoder();
+
+        // limit switches
+        topLimit = new DigitalInput(Ports.Elevator.TOP_LIMIT_SWITCH);
+        bottomLimit = new DigitalInput(Ports.Elevator.BOTTOM_LIMIT_SWITCH);
+
+        // control
+        position = new PIDController(kP, kI, kD)
+            .add(new ElevatorFeedforward(kG))
+            .setOutputFilter(new MotionProfile(VEL_LIMIT, ACC_LIMIT));
+
+        targetHeight = new SmartNumber("Target Height", MIN_HEIGHT);
+    }
+
+    public boolean atTop() {
+        return !topLimit.get();
+    }
+
+    public boolean atBottom() {
+        return !bottomLimit.get();
+    }
+
+    public double getTargetHeight() {
+        return targetHeight.get();
+    }
+
+    public void setTargetHeight(double height) {
+        targetHeight.set(height);
+    }
+
+    public double getVelocity() { // average of the two? 
+        return (rightEncoder.getVelocity() + leftEncoder.getVelocity()) / 2 * ENCODER_MULTIPLIER;
+    }
+
+    public double getHeight() {
+        return (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2 * ENCODER_MULTIPLIER;
+    }
+
+    public void setVoltage(double voltage) {
+        if (atTop() && voltage > 0) {
+            DriverStation.reportWarning("Top Limit Reached", false);
+
+            voltage = 0.0;
+
+            leftEncoder.setPosition(MAX_HEIGHT);
+            rightEncoder.setPosition(MAX_HEIGHT);
+        } else if (atBottom() && voltage < 0) {
+            DriverStation.reportWarning("Bottom Limit Reached", false);
+
+            voltage = 0.0;
+
+            leftEncoder.setPosition(MIN_HEIGHT);
+            rightEncoder.setPosition(MIN_HEIGHT);
+        }
+
+        rightMotor.setVoltage(voltage);
+        leftMotor.setVoltage(voltage);
+    }
+
+    public void periodic() {
+        setVoltage(position.update(getHeight(), getTargetHeight()));
+
+        SmartDashboard.putBoolean("Elevator/at Top", atTop());
+        SmartDashboard.putBoolean("Elevator/at Bottom", atBottom());
+
+        SmartDashboard.putNumber("Elevator/Height", getHeight());
+        SmartDashboard.putNumber("Elevator/Velocity", getVelocity());
+    }
+}