fixed the conversion

diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index 64374b142633cc385d5793eb397f1666bebcaa11..3a54c21a67075c3d6c0b6d1e49a15a725ecb1df6 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -141,7 +141,6 @@ public class Intake extends SubsystemBase {
      * gets motor position in inches
      */
     public double getPosition(){
-        // TODO: IDK if this is correct, so check that it's correct!!
         double motorRotations = rightMotor.getPosition().getValueAsDouble();
         return rotationsToInches(motorRotations);
 
@@ -152,10 +151,11 @@ public class Intake extends SubsystemBase {
      * @param rotations
      * @return
      */
-    public double rotationsToInches(double rotations){
-        double teethRackDistance = rotations / (IntakeConstants.rackPitch * Math.PI);
-        double positionInches = teethRackDistance/10;
-        return positionInches;
+    public double rotationsToInches(double motorRotations){
+        double circ = 2 * Math.PI * 0.5;
+        double pinionRotations = motorRotations / IntakeConstants.gearRatio;
+        double inches = pinionRotations * circ;
+        return inches; 
     }
 
     /**
@@ -164,16 +164,12 @@ public class Intake extends SubsystemBase {
      * @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;
+        double circ = 2 * Math.PI * 0.5;
+        double pinionRotations = inches / circ;
+        double motorRotations = pinionRotations * IntakeConstants.gearRatio;
         return motorRotations;
     }
 
-
-
     public boolean atMaxExtension(){
         return distance == IntakeConstants.maxExtension; //  TODO add tolerance for distance
     }

diff --git a/src/test/java/frc/robot/subsystems/Intake/IntakeTest.java b/src/test/java/frc/robot/subsystems/Intake/IntakeTest.java
index 484b18494bdc54dc5ca3076a5012fc5816c641f1..af5e271e003b5b29b9aec6886e18feaf5c0e6c11 100644 (file)
--- a/src/test/java/frc/robot/subsystems/Intake/IntakeTest.java
+++ b/src/test/java/frc/robot/subsystems/Intake/IntakeTest.java
@@ -16,7 +16,9 @@ public class IntakeTest {
     @Test
     public void conversionTest() {
         System.out.println("rotations to inches: "  + intake.rotationsToInches(3.0));
-        assertEquals(3.14159, intake.rotationsToInches(3.0), 0.0001);
+        assertEquals(Math.PI, intake.rotationsToInches(3.0), 0.0001);
+
+        System.out.println("2: inches to rotations: "  + intake.inchesToRotations(Math.PI));
         
         assertEquals(3.0, intake.inchesToRotations(3.14159), 0.0001);