Started implementing extender step function

- Marz
frc937 · Nov 27, 2023 · 5a1b842 · 5a1b842
1 parent f843639
commit 5a1b842
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -201,6 +201,10 @@ public static class Arm {
     public static final double SPEED_ARM_SHOULDER_HOMING = 0.5;
     public static final double SPEED_ARM_BASE_HOMING = 0.5;
 
+    public static class StepThresholds {
+
+    }
+
     public static class Intake {
       public static final int ID_TALON_ARM_CLAW = 0; /* This is actually a Spark */
       public static final double SPEED_ARM_CLAW = 1;

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmExtender.java b/src/main/java/frc/robot/subsystems/arm/ArmExtender.java
@@ -81,6 +81,10 @@ public boolean isExtenderAtSetpoint() {
     return extenderAtSetpoint;
   }
 
+  private double extenderCompensatedFeedForward(ArmExtender armExtender) {
+    if (armExtender.getLength() >= )
+  }
+
   /**
    * Directs arm towards setpoint. Since this is the periodic method, this is called every time the
    * scheduler runs.
@@ -104,6 +108,8 @@ public void periodic() {
     if (percentOutput != null) {
       winch.set(rateLimiter.calculate(percentOutput));
     }
+
+    System.out.println("Extender length: " + getLength());
   }
 
   public void setArmSpeed(double speed) {