Make getDriveCurrent and getCurrentVelocity thread-safe

robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk1SwerveModule.java

@@ -4,8 +4,8 @@
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-import org.frcteam2910.common.math.Vector2;
 import org.frcteam2910.common.drivers.SwerveModule;
+import org.frcteam2910.common.math.Vector2;
 
 import static org.frcteam2910.common.robot.Constants.CAN_TIMEOUT_MS;
 
@@ -118,6 +118,16 @@ public double readDistance() {
         return driveMotor.getSelectedSensorPosition() / driveTicksPerUnit;
     }
 
+    @Override
+    protected double readDriveCurrent() {
+        return driveMotor.getSupplyCurrent();
+    }
+
+    @Override
+    protected double readVelocity() {
+        return driveMotor.getSelectedSensorVelocity() / driveTicksPerUnit * (1000.0 / 100.0);
+    }
+
     @Override
     protected void setTargetAngle(double angle) {
         angleMotor.set(ControlMode.Position, angle * ANGLE_TICKS_PER_RADIAN);

robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk2SwerveModule.java

@@ -107,29 +107,18 @@ protected double readDistance() {
         }
     }
 
-    protected double readVelocity() {
-        synchronized (canLock) {
-            return driveVelocity;
-        }
-    }
-
+    @Override
     protected double readDriveCurrent() {
-        double localDriveCurrent;
         synchronized (canLock) {
-            localDriveCurrent = driveCurrent;
+            return driveCurrent;
         }
-
-        return localDriveCurrent;
     }
 
     @Override
-    public double getCurrentVelocity() {
-        return readVelocity();
-    }
-
-    @Override
-    public double getDriveCurrent() {
-        return readDriveCurrent();
+    protected double readVelocity() {
+        synchronized (canLock) {
+            return driveVelocity;
+        }
     }
 
     @Override

robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk2SwerveModuleBuilder.java

@@ -370,10 +370,6 @@ public SwerveModule build() {
     }
 
     private final class SwerveModuleImpl extends SwerveModule {
-        private final Object sensorLock = new Object();
-        private double currentDraw = 0.0;
-        private double velocity = 0.0;
-
         public SwerveModuleImpl() {
             super(modulePosition);
 
@@ -387,7 +383,8 @@ protected double readAngle() {
             return angleSupplier.getAsDouble();
         }
 
-        protected double readCurrentDraw() {
+        @Override
+        protected double readDriveCurrent() {
             if (currentDrawSupplier == null) {
                 return Double.NaN;
             }
@@ -404,6 +401,7 @@ protected double readDistance() {
             return distanceSupplier.getAsDouble();
         }
 
+        @Override
         protected double readVelocity() {
             if (velocitySupplier == null) {
                 return Double.NaN;
@@ -412,20 +410,6 @@ protected double readVelocity() {
             return velocitySupplier.getAsDouble();
         }
 
-        @Override
-        public double getCurrentVelocity() {
-            synchronized (sensorLock) {
-                return velocity;
-            }
-        }
-
-        @Override
-        public double getDriveCurrent() {
-            synchronized (sensorLock) {
-                return currentDraw;
-            }
-        }
-
         @Override
         protected void setTargetAngle(double angle) {
             targetAngleConsumer.accept(angle);
@@ -436,19 +420,6 @@ protected void setDriveOutput(double output) {
             driveOutputConsumer.accept(output);
         }
 
-        @Override
-        public void updateSensors() {
-            super.updateSensors();
-
-            double newCurrentDraw = readCurrentDraw();
-            double newVelocity = readVelocity();
-
-            synchronized (sensorLock) {
-                currentDraw = newCurrentDraw;
-                velocity = newVelocity;
-            }
-        }
-
         @Override
         public void updateState(double dt) {
             super.updateState(dt);