woooo commands finally set up? - skeleton

src/main/java/org/sciborgs1155/robot/Climber/Climber.java

@@ -18,9 +18,11 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import monologue.Logged;
+import monologue.Monologue.LogFile;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 
-public class Climber extends SubsystemBase{
+public class Climber extends SubsystemBase implements Logged{
   //need to add measurements for clamping(don't want to overextend the climber)
 
   //sim objects
@@ -43,30 +45,31 @@ public class Climber extends SubsystemBase{
   private final PIDController pid = new PIDController(kP, kI, kD);
   private final ElevatorFeedforward ff = new ElevatorFeedforward(kS, kG, kV, kA);
 
+  //setVoltage or speed?? - DutyCyleEncoders?
   public void setMotorSpeed(double speed){
     motor.set(MathUtil.clamp(speed, -1.0, 1.0));
   }
 
-//   public Command stopExtending() {
-
-//   }
-
-//   public Command fullyExtend() {
-//     moveToSetpoint();
-//   }
-
   // run when first scheduled
   public void climberInit() {
     // sets position to be 0 (remember to keep climber retracted at first)
     encoder.setPosition(0.0);
   }
 
+  public Command stopExtending() {
+    return run(() -> setMotorSpeed(0.0));
+  }
+
+  public Command fullyExtend() {
+    moveToSetpoint(MAX_EXTENSION);
+  }
+
   public Command retract(){
     return moveToSetpoint(0);
   }
 
   public Command moveToSetpoint(double setpoint){
-    return run(() -> setMotorSpeed(ff.calculate(encoder.getVelocity()) + pid.calculate(encoder.getPosition(), setpoint)));
+    return run(() -> setMotorSpeed(ff.calculate(encoder.getVelocity()) + pid.calculate(encoder.getPosition(), setpoint))).withName("moving to setpoint");
   }
 
 

src/main/java/org/sciborgs1155/robot/Ports.java

@@ -17,5 +17,7 @@ public static final class ClimberPorts {
     public static final int kV = -1;
     public static final int kA = -1;
     public static final int kG = -1;
+
+    public static final double MAX_EXTENSION = 1;
   }
 }