Climber API for moving climber between setpoint positions

Team2337 · Mar 9, 2022 · ad5b5c0 · ad5b5c0
1 parent c40898c
commit ad5b5c0
Show file tree

Hide file tree

Showing 4 changed files with 302 additions and 36 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -238,7 +238,7 @@ public static final class Auto {
     public static final PolarCoordinate kBallR1RunOver = new PolarCoordinate(
       Constants.Auto.kBallR1.getRadiusMeters() - Units.inchesToMeters(kRunOverDistanceInches),
       Constants.Auto.kBallR1.getTheta()
-    ); 
+    );
     //Shoot postition between ball R2 and ball D2
     public static final PolarCoordinate kFourBallShootPosition = new PolarCoordinate(
       Units.inchesToMeters(153),
@@ -355,6 +355,13 @@ public static final class Vision {
     public static final double VISION_TARGET_OFFSET_FROM_HUB_CENTER_METERS = Units.feetToMeters(2);
   }
 
+  public static enum ClimberSetpoint {
+    START,
+    RICKABOOT
+    // LOW_RUNG,
+    // MID_RUNG
+  }
+
   public static final int CLIMBER_LEFT_MOTOR_ID = 16;
   public static final int CLIMBER_RIGHT_MOTOR_ID = 3;
   public static final int CLIMBER_STRING_POT_ID = 3; // DIO

diff --git a/src/main/java/frc/robot/commands/climber/ClimberJoystickCommand.java b/src/main/java/frc/robot/commands/climber/ClimberJoystickCommand.java
@@ -35,7 +35,7 @@ public void execute() {
     if (joystick == 0) {
       // If our joystick is not being moved, hold our climber in it's current position
       if (shouldHoldPositionWhenStopped) {
-        climber.hold(climber.getPosition());
+        climber.holdAtCurrentPosition();
         shouldHoldPositionWhenStopped = false;
       }
     } else {

diff --git a/src/main/java/frc/robot/commands/climber/ClimberSetpointCommand.java b/src/main/java/frc/robot/commands/climber/ClimberSetpointCommand.java
@@ -0,0 +1,29 @@
+package frc.robot.commands.climber;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants.ClimberSetpoint;
+import frc.robot.subsystems.Climber;
+
+public class ClimberSetpointCommand extends CommandBase {
+
+  private final ClimberSetpoint setpoint;
+  private final Climber climber;
+
+  public ClimberSetpointCommand(ClimberSetpoint setpoint, Climber climber) {
+    this.setpoint = setpoint;
+    this.climber = climber;
+
+    addRequirements(climber);
+  }
+
+  @Override
+  public void initialize() {
+    climber.moveToSetpoint(setpoint);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    climber.stop();
+  }
+
+}