From e1314a0b47e4ff1f2dba1792130a77d7bf784f80 Mon Sep 17 00:00:00 2001
From: michaelx0281 <69123340+michaelx0281@users.noreply.github.com>
Date: Wed, 10 Jan 2024 17:15:37 -0500
Subject: [PATCH] woooo commands finally set up? - skeleton

---
 .../sciborgs1155/robot/Climber/Climber.java   | 23 +++++++++++--------
 .../java/org/sciborgs1155/robot/Ports.java    |  2 ++
 2 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/src/main/java/org/sciborgs1155/robot/Climber/Climber.java b/src/main/java/org/sciborgs1155/robot/Climber/Climber.java
index c5d24203..03bbc6bb 100644
--- a/src/main/java/org/sciborgs1155/robot/Climber/Climber.java
+++ b/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");
   }
     
       
diff --git a/src/main/java/org/sciborgs1155/robot/Ports.java b/src/main/java/org/sciborgs1155/robot/Ports.java
index 81003631..29796fd3 100644
--- a/src/main/java/org/sciborgs1155/robot/Ports.java
+++ b/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;
   }
 }