Merge pull request #348 from FRC5010/dev

Author: thenetworkgrinch

src/main/java/swervelib/encoders/ThriftyNovaEncoderSwerve.java

@@ -1,5 +1,11 @@
 package swervelib.encoders;
 
+import com.thethriftybot.Conversion;
+import com.thethriftybot.Conversion.PositionUnit;
+import com.thethriftybot.Conversion.VelocityUnit;
+import com.thethriftybot.ThriftyNova;
+import com.thethriftybot.ThriftyNova.EncoderType;
+
 import swervelib.motors.SwerveMotor;
 import swervelib.motors.ThriftyNovaSwerve;
 
@@ -12,7 +18,7 @@ public class ThriftyNovaEncoderSwerve extends SwerveAbsoluteEncoder
   /**
    * The absolute encoder is directly interfaced through the Thrifty Nova motor.
    */
-  protected ThriftyNovaSwerve motor;
+  protected ThriftyNova motor;
   /**
    * Inversion state of the attached encoder.
    */
@@ -21,7 +27,15 @@ public class ThriftyNovaEncoderSwerve extends SwerveAbsoluteEncoder
    * Offset of the absolute encoder.
    */
   protected double            offset   = 0.0;
-
+  /**
+   * Position conversion object for the motor encoder
+   */
+  private       Conversion    positionConversion;
+  /**
+   * Velocity conversion object for the motor encoder
+   */
+  private       Conversion    velocityConversion;
+    
   /**
    * Create the {@link ThriftyNovaEncoderSwerve} object as an absolute encoder from the {@link ThriftyNovaSwerve}
    * motor.
@@ -30,8 +44,10 @@ public class ThriftyNovaEncoderSwerve extends SwerveAbsoluteEncoder
    */
   public ThriftyNovaEncoderSwerve(SwerveMotor motor)
   {
-    this.motor = (ThriftyNovaSwerve) motor;
-    motor.setAbsoluteEncoder(null);
+    this.motor = (ThriftyNova) motor.getMotor();
+    positionConversion = new Conversion(PositionUnit.DEGREES, EncoderType.ABS);
+    velocityConversion = new Conversion(VelocityUnit.DEGREES_PER_SEC, EncoderType.ABS);
+    this.motor.useEncoderType(EncoderType.ABS);
   }
 
   @Override
@@ -40,7 +56,6 @@ public void close()
     // ThriftyNova encoder gets closed with the motor
     // I don't think an encoder getting closed should 
     // close the entire motor so i will keep this empty
-    // sparkFlex.close();
   }
 
   /**
@@ -78,7 +93,9 @@ public void configure(boolean inverted)
   @Override
   public double getAbsolutePosition()
   {
-    return (motor.getPosition() + offset) * (inverted ? -1.0 : 1.0);
+    double rawMotor = motor.getPosition();
+    double convertedMotor = positionConversion.fromMotor(rawMotor);
+    return (convertedMotor + offset) * (inverted ? -1.0 : 1.0);
   }
 
   /**
@@ -111,6 +128,6 @@ public boolean setAbsoluteEncoderOffset(double offset)
   @Override
   public double getVelocity()
   {
-    return motor.getVelocity();
+    return velocityConversion.fromMotor(motor.getVelocity()) * (inverted ? -1.0 : 1.0);
   }
 }