generated from SciBorgs/Hydrogen
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #2 from SciBorgs/elevator
elevator
- Loading branch information
Showing
14 changed files
with
437 additions
and
13 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
113 changes: 113 additions & 0 deletions
113
src/main/java/org/sciborgs1155/robot/elevator/Elevator.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,113 @@ | ||
| package org.sciborgs1155.robot.elevator; | ||
|
|
||
| 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.sciborgs1155.robot.Constants.*; | ||
| import static org.sciborgs1155.robot.Constants.Field.*; | ||
| import static org.sciborgs1155.robot.elevator.ElevatorConstants.*; | ||
|
|
||
| import edu.wpi.first.math.MathUtil; | ||
| import edu.wpi.first.math.controller.ElevatorFeedforward; | ||
| import edu.wpi.first.math.controller.ProfiledPIDController; | ||
| import edu.wpi.first.math.trajectory.TrapezoidProfile; | ||
| import edu.wpi.first.wpilibj.util.Color8Bit; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import monologue.Annotations.Log; | ||
| import monologue.Logged; | ||
| import org.sciborgs1155.robot.Constants.Field.Level; | ||
| import org.sciborgs1155.robot.Robot; | ||
|
|
||
| public class Elevator extends SubsystemBase implements Logged, AutoCloseable { | ||
| public static Elevator create() { | ||
| return Robot.isReal() ? new Elevator(new RealElevator()) : new Elevator(new SimElevator()); | ||
| } | ||
|
|
||
| public static Elevator none() { | ||
| return new Elevator(new NoElevator()); | ||
| } | ||
|
|
||
| private final ElevatorIO hardware; | ||
|
|
||
| // private final SysIdRoutine sysIdRoutine; | ||
|
|
||
| @Log.NT | ||
| private final ProfiledPIDController pid = | ||
| new ProfiledPIDController( | ||
| kP, | ||
| kI, | ||
| kD, | ||
| new TrapezoidProfile.Constraints( | ||
| MAX_VELOCITY.in(MetersPerSecond), MAX_ACCEL.in(MetersPerSecondPerSecond))); | ||
|
|
||
| @Log.NT private final ElevatorFeedforward ff = new ElevatorFeedforward(kS, kG, kV, kA); | ||
|
|
||
| @Log.NT | ||
| private final ElevatorVisualizer setpoint = new ElevatorVisualizer(new Color8Bit(0, 0, 255)); | ||
|
|
||
| @Log.NT | ||
| private final ElevatorVisualizer measurement = new ElevatorVisualizer(new Color8Bit(255, 0, 0)); | ||
|
|
||
| public Elevator(ElevatorIO hardware) { | ||
| this.hardware = hardware; | ||
| setDefaultCommand(retract()); | ||
| // sysIdRoutine = | ||
| // new SysIdRoutine( | ||
| // new SysIdRoutine.Config(), | ||
| // new SysIdRoutine.Mechanism(v -> pivot.setVoltage(v.in(Volts)), null, this)); | ||
| } | ||
|
|
||
| public Command extend() { | ||
| return run(() -> update(MAX_HEIGHT.in(Meters))); | ||
| } | ||
|
|
||
| public Command retract() { | ||
| return run(() -> update(MIN_HEIGHT.in(Meters))); | ||
| } | ||
|
|
||
| public Command scoreLevel(Level level) { | ||
| return run(() -> update(level.getHeight())); | ||
| } | ||
|
|
||
| @Log.NT | ||
| public double position() { | ||
| return hardware.position(); | ||
| } | ||
|
|
||
| @Log.NT | ||
| public double velocity() { | ||
| return hardware.velocity(); | ||
| } | ||
|
|
||
| @Log.NT | ||
| public double positionSetpoint() { | ||
| return pid.getSetpoint().position; | ||
| } | ||
|
|
||
| @Log.NT | ||
| public double velocitySetpoint() { | ||
| return pid.getSetpoint().velocity; | ||
| } | ||
|
|
||
| private void update(double position) { | ||
| position = MathUtil.clamp(position, MIN_HEIGHT.in(Meters), MAX_HEIGHT.in(Meters)); | ||
|
|
||
| double lastVelocity = pid.getSetpoint().velocity; | ||
| double feedback = pid.calculate(hardware.position(), position); | ||
| double feedforward = ff.calculateWithVelocities(lastVelocity, pid.getSetpoint().velocity); | ||
|
|
||
| hardware.setVoltage(feedforward + feedback); | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| setpoint.setLength(positionSetpoint()); | ||
| measurement.setLength(position()); | ||
| } | ||
|
|
||
| @Override | ||
| public void close() throws Exception { | ||
| hardware.close(); | ||
| } | ||
| } |
35 changes: 35 additions & 0 deletions
35
src/main/java/org/sciborgs1155/robot/elevator/ElevatorConstants.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,35 @@ | ||
| package org.sciborgs1155.robot.elevator; | ||
|
|
||
| import static edu.wpi.first.units.Units.Inches; | ||
| 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 edu.wpi.first.units.measure.Distance; | ||
| import edu.wpi.first.units.measure.LinearAcceleration; | ||
| import edu.wpi.first.units.measure.LinearVelocity; | ||
| import edu.wpi.first.units.measure.Mass; | ||
|
|
||
| public class ElevatorConstants { | ||
| public static final double kP = 10; | ||
| public static final double kI = 0; | ||
| public static final double kD = 10; | ||
| public static final double kS = .1; | ||
| public static final double kG = 1; | ||
| public static final double kV = 1; | ||
| public static final double kA = 0; | ||
|
|
||
| public static final Distance MIN_HEIGHT = Meters.of(.2); | ||
| public static final Distance MAX_HEIGHT = Meters.of(2); | ||
|
|
||
| public static final LinearVelocity MAX_VELOCITY = MetersPerSecond.of(1); | ||
| public static final LinearAcceleration MAX_ACCEL = MetersPerSecondPerSecond.of(1); | ||
|
|
||
| public static final Mass WEIGHT = Kilograms.of(20); | ||
| public static final Distance DRUM_RADIUS = Meters.of(.1); | ||
| public static final double GEARING = 20 / 1; | ||
| public static final Distance SPROCKET_RADIUS = Inches.of(1); | ||
| public static final Distance SPROCKET_CIRCUMFRENCE = SPROCKET_RADIUS.times(2 * Math.PI); | ||
| public static final double CONVERSION_FACTOR = SPROCKET_CIRCUMFRENCE.in(Meters) * 2 / GEARING; | ||
| } |
9 changes: 9 additions & 0 deletions
9
src/main/java/org/sciborgs1155/robot/elevator/ElevatorIO.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,9 @@ | ||
| package org.sciborgs1155.robot.elevator; | ||
|
|
||
| public interface ElevatorIO extends AutoCloseable { | ||
| public void setVoltage(double voltage); | ||
|
|
||
| public double position(); | ||
|
|
||
| public double velocity(); | ||
| } |
28 changes: 28 additions & 0 deletions
28
src/main/java/org/sciborgs1155/robot/elevator/ElevatorVisualizer.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,28 @@ | ||
| package org.sciborgs1155.robot.elevator; | ||
|
|
||
| import static edu.wpi.first.units.Units.Meters; | ||
| import static org.sciborgs1155.robot.elevator.ElevatorConstants.MIN_HEIGHT; | ||
|
|
||
| import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; | ||
| import edu.wpi.first.wpilibj.util.Color8Bit; | ||
| import monologue.Annotations.Log; | ||
| import monologue.Logged; | ||
|
|
||
| public class ElevatorVisualizer implements Logged { | ||
| @Log.NT private final Mechanism2d mech; | ||
| private final MechanismLigament2d elevator; | ||
|
|
||
| public ElevatorVisualizer(Color8Bit color) { | ||
| mech = new Mechanism2d(50, 50); | ||
| MechanismRoot2d chassis = mech.getRoot("chassis", 25, 10); | ||
| elevator = | ||
| chassis.append( | ||
| new MechanismLigament2d("elevator", MIN_HEIGHT.in(Meters) * 10, 90, 3, color)); | ||
| } | ||
|
|
||
| public void setLength(double length) { | ||
| elevator.setLength(length * 10); | ||
| } | ||
| } |
Oops, something went wrong.