diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 1651e93..c2b59d4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/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; + } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index cdf7f7d..0d68ed6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/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); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/Elevator.java new file mode 100644 index 0000000..4e1712e --- /dev/null +++ b/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()); + } +}