conversion methods

jerry wanted some comments also

diff --git a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
index 8d07719518e481debba0305eeffb66166743b103..dc48183212a58f1710e14a4cc196df281a5dd020 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java
@@ -7,8 +7,11 @@ public class ClimbConstants {
     // CHANGE LATER
     // gear ratio for converting motor rotations to linear distance
     public final static double CLIMB_GEAR_RATIO = 45.0;
+    /** Winch spool radius in meters */
     public final static double WHEEL_RADIUS = Units.inchesToMeters(0.334);
+    /** climber stowed ? position in meters */
     public final static double BOTTOM_POSITION = Units.inchesToMeters(-8);
+    /**  position that should put the robot off the ground? in meters.  */
     public final static double CLIMB_POSITION = Units.inchesToMeters(-6);
     public final static double UP_POSITION = 0.0;
 
@@ -20,6 +23,7 @@ public class ClimbConstants {
     public final static double CLIMB_CURRENT = 42.0;
 
     // PID Constants
+    // TODO: what are the units? Inches? Meters?
     public final static double PID_P = 0.1;
     public final static double PID_I = 0.0;
     public final static double PID_D = 0.0;

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index 140161ae43b79e4ac943dda0520a791484eae724..34914828b9bbb213e73b9f401dff5a9c8614f078 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -17,19 +17,29 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
 
+/**
+ * Climber subsystem
+ */
 public class LinearClimb extends SubsystemBase implements LinearClimbIO{
+    /** climber motor */
     private final TalonFX motor;
+    /** whether the subsysgtem is calibrating */
     private boolean calibrating = false;
+
+    // why not use the ClimbConstants directly?
     private double downPosition = ClimbConstants.BOTTOM_POSITION;
     private double upPosition = ClimbConstants.UP_POSITION;
     private double climbPosition = ClimbConstants.CLIMB_POSITION;
 
+    /** should the subsystem perform calibration automatically */
     private boolean calibrateOnStartUp = false;
 
     private double MAX_POWER = 0.2;
 
+    // logging information
     private LinearClimbIOInputs inputs = new LinearClimbIOInputs();
 
+    // TODO: what units?
     private final PIDController pid = new PIDController(
             ClimbConstants.PID_P,
             ClimbConstants.PID_I,
@@ -102,7 +112,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
      */
     public void goUp() {
         MAX_POWER = 0.8;
-        setSetpoint((Units.radiansToRotations(upPosition / ClimbConstants.WHEEL_RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
+        setSetpoint(metersToRotations(ClimbConstants.UP_POSITION));
     }
 
     /**
@@ -110,7 +120,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
      */
     public void goDown() {
         MAX_POWER = 0.2;
-        setSetpoint((Units.radiansToRotations(downPosition / ClimbConstants.WHEEL_RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
+        setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION));
     }
 
     /**
@@ -118,7 +128,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
      */
     public void climb() {
         MAX_POWER = 0.8;
-        setSetpoint((Units.radiansToRotations(climbPosition / ClimbConstants.WHEEL_RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
+        setSetpoint(metersToRotations(ClimbConstants.CLIMB_POSITION));
     }
 
     @Override
@@ -134,6 +144,27 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
         
         Logger.recordOutput("LinearClimb/setpointMeters", Units.rotationsToRadians(pid.getSetpoint()) * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO);
     }
+    /**
+     * converts motor rotations to meters
+     * @param motorRotations
+     * @return
+     */
+    public double rotationsToMeters(double motorRotations){
+        double circ = 2 * Math.PI * ClimbConstants.WHEEL_RADIUS;
+        double meters = motorRotations / ClimbConstants.CLIMB_GEAR_RATIO * circ;
+        return meters;
+    }
+
+    /**
+     * converts meters to motor rotations
+     * @param meters
+     * @return
+     */
+    public double metersToRotations(double meters){
+        double circ = 2 * Math.PI * ClimbConstants.WHEEL_RADIUS;
+        double motorRotations = meters / circ * ClimbConstants.CLIMB_GEAR_RATIO;
+        return motorRotations;
+    }
 
     /**
      * sets supply and stator current limits

diff --git a/src/test/java/frc/robot/subsystems/Intake/ClimbTest.java b/src/test/java/frc/robot/subsystems/Intake/ClimbTest.java
new file mode 100644 (file)
index 0000000..687b9f4
--- /dev/null
+++ b/src/test/java/frc/robot/subsystems/Intake/ClimbTest.java
@@ -0,0 +1,23 @@
+package frc.robot.subsystems.Intake;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.Climb.LinearClimb;
+
+public class ClimbTest {
+
+    private LinearClimb climb = new LinearClimb();
+
+
+    @Test
+    public void conversionTest() {
+        
+
+        assertEquals(2 * Math.PI * Units.inchesToMeters(0.334), climb.rotationsToMeters(45), 0.0001);
+        // the methods should be inverses of each other
+        assertEquals(15.0, climb.rotationsToMeters(climb.metersToRotations(15.0)), 0.0001);
+    }
+}