Se/climber

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 ***/

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;
+    }
+}

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);
+    }
+}

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;
+    }
+}

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);
+    }
+}

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);
+    }
+}

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;
 

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());
+    }
 }

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<Double> 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()) {