diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c88ca1ab..3f9ec85c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -6,8 +6,10 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.climber.ClimberDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.amper.Amper; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -24,6 +26,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Trigger; public class RobotContainer { @@ -65,7 +68,11 @@ private void configureDefaultCommands() { /*** BUTTONS ***/ /***************/ - private void configureButtonBindings() {} + private void configureButtonBindings() { + // manual climber control + new Trigger(() -> operator.getLeftStick().magnitude() > Settings.Operator.DEADBAND.get()) + .whileTrue(new ClimberDrive(operator)); + } /**************/ /*** AUTONS ***/ diff --git a/src/main/java/com/stuypulse/robot/commands/climber/ClimberDrive.java b/src/main/java/com/stuypulse/robot/commands/climber/ClimberDrive.java new file mode 100644 index 00000000..108130a3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climber/ClimberDrive.java @@ -0,0 +1,42 @@ +package com.stuypulse.robot.commands.climber; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.climber.Climber; + +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.streams.numbers.IStream; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class ClimberDrive extends Command { + + private final Climber climber; + private final IStream voltage; + + public ClimberDrive(Gamepad gamepad) { + climber = Climber.getInstance(); + + voltage = IStream.create(gamepad::getLeftY) + .filtered(x -> x * Settings.Climber.MAX_DRIVE_VOLTAGE.get()); + + addRequirements(climber); + } + + @Override + public void execute() { + climber.setVoltageOverride(voltage.get()); + + SmartDashboard.putNumber("Climber/Gamepad Velocity", voltage.get()); + } + + @Override + public void end(boolean interrupted) { + climber.setTargetHeight(climber.getHeight()); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/climber/ClimberToBottom.java b/src/main/java/com/stuypulse/robot/commands/climber/ClimberToBottom.java new file mode 100644 index 00000000..00abd784 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climber/ClimberToBottom.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.climber; + +import com.stuypulse.robot.constants.Settings; + +public class ClimberToBottom extends ClimberToHeight { + public ClimberToBottom() { + super(Settings.Climber.MIN_HEIGHT); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeight.java b/src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeight.java new file mode 100644 index 00000000..30ecbb2e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeight.java @@ -0,0 +1,28 @@ +package com.stuypulse.robot.commands.climber; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.climber.Climber; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ClimberToHeight extends Command { + private final Climber climber; + private final double height; + + public ClimberToHeight(double height) { + climber = Climber.getInstance(); + this.height = height; + + addRequirements(climber); + } + + @Override + public void initialize() { + climber.setTargetHeight(height); + } + + @Override + public boolean isFinished() { + return Math.abs(climber.getTargetHeight() - climber.getHeight()) < Settings.Climber.BangBang.THRESHOLD; + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeightInstant.java b/src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeightInstant.java new file mode 100644 index 00000000..2720c65b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climber/ClimberToHeightInstant.java @@ -0,0 +1,23 @@ +package com.stuypulse.robot.commands.climber; + +import com.stuypulse.robot.subsystems.climber.Climber; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ClimberToHeightInstant extends InstantCommand { + + private final Climber climber; + private final double height; + + public ClimberToHeightInstant(double height) { + climber = Climber.getInstance(); + this.height = height; + + addRequirements(climber); + } + + @Override + public void initialize() { + climber.setTargetHeight(height); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/climber/ClimberToTop.java b/src/main/java/com/stuypulse/robot/commands/climber/ClimberToTop.java new file mode 100644 index 00000000..7c7c4dfd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climber/ClimberToTop.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.climber; + +import com.stuypulse.robot.constants.Settings; + +public class ClimberToTop extends ClimberToHeight { + public ClimberToTop() { + super(Settings.Climber.MAX_HEIGHT); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 27c507aa..e41be042 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -21,24 +21,29 @@ * values that we can edit on Shuffleboard. */ public interface Settings { - double DT = 1.0 / 50.0; public interface Climber { double MIN_HEIGHT = 0.0; - double MAX_HEIGHT = 0.0; + double MAX_HEIGHT = 0.444; + + double MASS = Units.lbsToKilograms(2.173979); + double DRUM_RADIUS = Units.inchesToMeters(1.025); - double AT_HEIGHT_THRESHOLD = Units.inchesToMeters(1); + SmartNumber MAX_DRIVE_VOLTAGE = new SmartNumber("Climber/Velocity Limit", 8.0); - double BANGBANG_VOLTAGE = 8; + public interface BangBang { + double CONTROLLER_VOLTAGE = 8.0; + double THRESHOLD = 0.03; + } public interface Encoder { - double GEAR_RATIO = 0.0; + double GEAR_RATIO = 12.0; - double POSITION_CONVERSION = 0.0; + double POSITION_CONVERSION = 1.0; double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0; } - } + } public interface Amper { public interface Score { @@ -215,6 +220,10 @@ public interface Turn { } } + public interface Operator { + SmartNumber DEADBAND = new SmartNumber("Operator Settings/Manual Climb Deadband", 0.02); + } + public interface Shooter { double MOMENT_OF_INERTIA = 1; diff --git a/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java b/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java index 116d014b..98e938dc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java @@ -1,22 +1,33 @@ package com.stuypulse.robot.subsystems.climber; +import java.util.Optional; + +import com.stuypulse.robot.util.ClimberVisualizer; import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Climber extends SubsystemBase { + private static final Climber instance; - - private final SmartNumber targetHeight; static { - instance = new ClimberImpl(); + if (RobotBase.isReal()) { + instance = new ClimberImpl(); + } else { + instance = new SimClimber(); + } } public static Climber getInstance() { return instance; } + private final SmartNumber targetHeight; + + private final ClimberVisualizer climberVisualizer = new ClimberVisualizer(); + public Climber() { targetHeight = new SmartNumber("Climber/Target Height", 0.0); } @@ -25,15 +36,20 @@ public void setTargetHeight(double height) { targetHeight.set(height); } - public double getTargetHeight() { + public final double getTargetHeight() { return targetHeight.get(); } public abstract double getHeight(); public abstract double getVelocity(); - public abstract void setVoltage(double voltage); - public abstract boolean atTop(); public abstract boolean atBottom(); + + public abstract void setVoltageOverride(double voltage); + + @Override + public void periodic() { + climberVisualizer.setHeight(getHeight()); + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java index 1b1fa556..6035e7cc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java @@ -1,6 +1,9 @@ package com.stuypulse.robot.subsystems.climber; import com.revrobotics.CANSparkMax; + +import java.util.Optional; + import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; @@ -13,6 +16,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ClimberImpl extends Climber { + private final CANSparkMax rightMotor; private final CANSparkMax leftMotor; @@ -24,6 +28,8 @@ public class ClimberImpl extends Climber { private final DigitalInput bottomRightLimit; private final DigitalInput bottomLeftLimit; + private Optional voltageOverride; + protected ClimberImpl() { rightMotor = new CANSparkMax(Ports.Climber.RIGHT_MOTOR, MotorType.kBrushless); leftMotor = new CANSparkMax(Ports.Climber.LEFT_MOTOR, MotorType.kBrushless); @@ -41,11 +47,25 @@ protected ClimberImpl() { topLeftLimit = new DigitalInput(Ports.Climber.TOP_LEFT_LIMIT); bottomRightLimit = new DigitalInput(Ports.Climber.BOTTOM_RIGHT_LIMIT); bottomLeftLimit = new DigitalInput(Ports.Climber.BOTTOM_LEFT_LIMIT); + + voltageOverride = Optional.empty(); Motors.Climber.LEFT_MOTOR.configure(leftMotor); Motors.Climber.RIGHT_MOTOR.configure(rightMotor); } + @Override + public void setTargetHeight(double height) { + super.setTargetHeight(height); + + voltageOverride = Optional.empty(); + } + + @Override + public void setVoltageOverride(double voltage) { + voltageOverride = Optional.of(voltage); + } + @Override public double getHeight() { return (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2; @@ -66,7 +86,6 @@ public boolean atBottom() { return !bottomRightLimit.get() || !bottomLeftLimit.get(); } - @Override public void setVoltage(double voltage) { if (atTop() && voltage > 0) { DriverStation.reportWarning("Climber Top Limit Reached", false); @@ -84,17 +103,21 @@ public void setVoltage(double voltage) { public void periodic() { super.periodic(); - if (Math.abs(getHeight() - getTargetHeight()) < Settings.Climber.AT_HEIGHT_THRESHOLD) { - setVoltage(0.0); - } else if (getHeight() > getTargetHeight()) { - setVoltage(-Settings.Climber.BANGBANG_VOLTAGE); + if (voltageOverride.isPresent()) { + setVoltage(voltageOverride.get()); } else { - setVoltage(+Settings.Climber.BANGBANG_VOLTAGE); + + if (Math.abs(getHeight() - getTargetHeight()) < Settings.Climber.BangBang.THRESHOLD) { + setVoltage(0.0); + } else if (getHeight() > getTargetHeight()) { + setVoltage(-Settings.Climber.BangBang.CONTROLLER_VOLTAGE); + } else { + setVoltage(Settings.Climber.BangBang.CONTROLLER_VOLTAGE); + } + } - SmartDashboard.putNumber("Climber/Target Height", getTargetHeight()); SmartDashboard.putNumber("Climber/Height", getHeight()); - SmartDashboard.putNumber("Climber/Velocity", getVelocity()); if (atBottom()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/climber/SimClimber.java b/src/main/java/com/stuypulse/robot/subsystems/climber/SimClimber.java new file mode 100644 index 00000000..aae8590f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/climber/SimClimber.java @@ -0,0 +1,93 @@ +package com.stuypulse.robot.subsystems.climber; + +import edu.wpi.first.math.system.plant.DCMotor; + +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import java.util.Optional; + +import com.stuypulse.robot.constants.Settings; + +public class SimClimber extends Climber { + + private final ElevatorSim sim; + + private Optional voltageOverride; + + public SimClimber() { + sim = new ElevatorSim( + DCMotor.getNEO(2), + Settings.Climber.Encoder.GEAR_RATIO, + Settings.Climber.MASS, + Settings.Climber.DRUM_RADIUS, + Settings.Climber.MIN_HEIGHT, + Settings.Climber.MAX_HEIGHT, + true, + Settings.Climber.MIN_HEIGHT + ); + + voltageOverride = Optional.empty(); + } + + @Override + public void setTargetHeight(double height) { + super.setTargetHeight(height); + + voltageOverride = Optional.empty(); + } + + @Override + public void setVoltageOverride(double voltage) { + voltageOverride = Optional.of(voltage); + } + + @Override + public double getHeight() { + return sim.getPositionMeters(); + } + + @Override + public double getVelocity() { + return sim.getVelocityMetersPerSecond(); + } + + @Override + public boolean atTop() { + return sim.hasHitUpperLimit(); + } + + @Override + public boolean atBottom() { + return sim.hasHitLowerLimit(); + } + + public void setVoltage(double voltage) { + sim.setInputVoltage(voltage); + } + + @Override + public void periodic() { + super.periodic(); + + if (voltageOverride.isPresent()) { + setVoltage(voltageOverride.get()); + } else { + if (Math.abs(getHeight() - getTargetHeight()) < Settings.Climber.BangBang.THRESHOLD) { + setVoltage(0.0); + } else if (getHeight() > getTargetHeight()) { + setVoltage(-Settings.Climber.BangBang.CONTROLLER_VOLTAGE); + } else { + setVoltage(Settings.Climber.BangBang.CONTROLLER_VOLTAGE); + } + } + + SmartDashboard.putNumber("Climber/Height", getHeight()); + SmartDashboard.putNumber("Climber/Velocity", getVelocity()); + } + + @Override + public void simulationPeriodic() { + sim.update(Settings.DT); + } +} diff --git a/src/main/java/com/stuypulse/robot/util/ClimberVisualizer.java b/src/main/java/com/stuypulse/robot/util/ClimberVisualizer.java new file mode 100644 index 00000000..3b2ba1e3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/ClimberVisualizer.java @@ -0,0 +1,99 @@ +package com.stuypulse.robot.util; + +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.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color8Bit; + +import com.stuypulse.robot.constants.Settings; + +public class ClimberVisualizer { + private final double WINDOW_WIDTH = 6; + private final double WINDOW_HEIGHT = 15; + private final double WINDOW_X_PADDING = 1; + + private final double OUTER_STAGE_HEIGHT = 6; + + private final int LINE_WIDTH = 8; + + private final Mechanism2d climber; + + // ligaments + private MechanismLigament2d outerLeftLigament; + private MechanismLigament2d outerRightLigament; + + private MechanismLigament2d topLigament; + + private MechanismLigament2d leftLigament; + private MechanismLigament2d rightLigament; + + // roots + private MechanismRoot2d topRoot; + + private MechanismRoot2d leftRoot; + private MechanismRoot2d rightRoot; + + private MechanismRoot2d rightBottomRoot; + private MechanismRoot2d leftBottomRoot; + + private double leftRootX; + private double rightRootX; + + // colors + private Color8Bit blue = new Color8Bit(0, 0, 255); + private Color8Bit red = new Color8Bit(255, 0, 0); + + private MechanismLigament2d getLigament(String name, double length, double angle, Color8Bit color) { + return new MechanismLigament2d(name, length, angle, LINE_WIDTH, color); + } + + public ClimberVisualizer() { + climber = new Mechanism2d(WINDOW_WIDTH, WINDOW_HEIGHT); + + leftRootX = WINDOW_X_PADDING; + rightRootX = WINDOW_WIDTH - WINDOW_X_PADDING; + + // root nodes + + // outer shell + leftRoot = climber.getRoot("left root", leftRootX,0); + rightRoot = climber.getRoot("right root", rightRootX,0); + topRoot = climber.getRoot("top root", leftRootX, OUTER_STAGE_HEIGHT); + + // inner shell + leftBottomRoot = climber.getRoot("left bottom root", leftRootX, 0); + rightBottomRoot = climber.getRoot("right bottom root", rightRootX, 0); + + // ligaments + + //outer shell + topLigament = getLigament("top ligament", WINDOW_WIDTH - 2 * WINDOW_X_PADDING, 0, blue); + outerRightLigament = getLigament("outer right ligament", OUTER_STAGE_HEIGHT, 90, blue); + outerLeftLigament = getLigament("outer left ligament", OUTER_STAGE_HEIGHT, 90, blue); + + // inner shell + leftLigament = new MechanismLigament2d("climber left ligament ", OUTER_STAGE_HEIGHT - 5, 90, 12, red); + rightLigament = new MechanismLigament2d("climber right ligament ", OUTER_STAGE_HEIGHT - 5, 90, 12, red); + + //outer shell + leftRoot.append(outerLeftLigament); + rightRoot.append(outerRightLigament); + topRoot.append(topLigament); + + // inner shell + leftBottomRoot.append(leftLigament); + rightBottomRoot.append(rightLigament); + + SmartDashboard.putData("Climber", climber); + } + + public void setHeight(double newHeight) { + double percentDone = newHeight / Settings.Climber.MAX_HEIGHT; + + double stageBottomY = OUTER_STAGE_HEIGHT * percentDone; + + leftBottomRoot.setPosition(leftRootX, stageBottomY); + rightBottomRoot.setPosition(rightRootX, stageBottomY); + } +} \ No newline at end of file