wokr in progress

diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index 87420488e0f30114ba4bb23e00422933c58b346c..64374b142633cc385d5793eb397f1666bebcaa11 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -133,11 +133,7 @@ public class Intake extends SubsystemBase {
      * @param setpoint
      */
     public void setPosition(double setpoint) {
-        double teethRackPinion = 10;
-        // setpoint is in inches, need to convert to rotations
-        double teethRackDistance = setpoint / (IntakeConstants.rackPitch * Math.PI);
-        double rotationsRackPinion = teethRackDistance / teethRackPinion;
-        double motorRotations = rotationsRackPinion * IntakeConstants.gearRatio;
+        double motorRotations =inchesToRotations(setpoint);
         rightMotor.setControl(voltageRequest.withPosition(motorRotations));
     }
 
@@ -147,11 +143,37 @@ public class Intake extends SubsystemBase {
     public double getPosition(){
         // TODO: IDK if this is correct, so check that it's correct!!
         double motorRotations = rightMotor.getPosition().getValueAsDouble();
-        double teethRackDistance = motorRotations / (IntakeConstants.rackPitch * Math.PI);
+        return rotationsToInches(motorRotations);
+
+    }
+
+    /**
+     * convert rotations to inches
+     * @param rotations
+     * @return
+     */
+    public double rotationsToInches(double rotations){
+        double teethRackDistance = rotations / (IntakeConstants.rackPitch * Math.PI);
         double positionInches = teethRackDistance/10;
         return positionInches;
     }
 
+    /**
+     * convert inches to rotations
+     * @param inches
+     * @return
+     */
+    public double inchesToRotations(double inches){
+        double teethRackPinion = 10;
+        // setpoint is in inches, need to convert to rotations
+        double teethRackDistance = inches / (IntakeConstants.rackPitch * Math.PI);
+        double rotationsRackPinion = teethRackDistance / teethRackPinion;
+        double motorRotations = rotationsRackPinion * IntakeConstants.gearRatio;
+        return motorRotations;
+    }
+
+
+
     public boolean atMaxExtension(){
         return distance == IntakeConstants.maxExtension; //  TODO add tolerance for distance
     }
@@ -172,6 +194,12 @@ public class Intake extends SubsystemBase {
         
     }
 
+    public void close() {
+        leftMotor.close();
+        rightMotor.close();
+        rollerMotor.close();
+    }
+
 }
     
 

diff --git a/src/test/java/frc/robot/subsystems/Intake/IntakeTest.java b/src/test/java/frc/robot/subsystems/Intake/IntakeTest.java
new file mode 100644 (file)
index 0000000..484b184
--- /dev/null
+++ b/src/test/java/frc/robot/subsystems/Intake/IntakeTest.java
@@ -0,0 +1,26 @@
+package frc.robot.subsystems.Intake;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.Test;
+
+public class IntakeTest {
+    Intake intake = new Intake();
+
+    @AfterEach
+    public void cleanup() {
+        intake.close();
+    }
+
+    @Test
+    public void conversionTest() {
+        System.out.println("rotations to inches: "  + intake.rotationsToInches(3.0));
+        assertEquals(3.14159, intake.rotationsToInches(3.0), 0.0001);
+        
+        assertEquals(3.0, intake.inchesToRotations(3.14159), 0.0001);
+
+        assertEquals(15.0, intake.rotationsToInches(intake.inchesToRotations(15.0)), 0.0001);
+    }
+    
+}