A few more updates, cleaned up getting the motor

Author: kevinfrei

Path/src/main/java/com/technototes/path/subsystem/PathingMecanumDrivebaseSubsystem.java

@@ -123,10 +123,10 @@ public PathingMecanumDrivebaseSubsystem(
             c.getDouble(WheelBase.class),
             c.getDouble(LateralMult.class)
         );
-        leftFront = fl.getDevice();
-        leftRear = rl.getDevice();
-        rightRear = rr.getDevice();
-        rightFront = fr.getDevice();
+        leftFront = fl.getRawMotor(DcMotorEx.class);
+        leftRear = rl.getRawMotor(DcMotorEx.class);
+        rightRear = rr.getRawMotor(DcMotorEx.class);
+        rightFront = fr.getRawMotor(DcMotorEx.class);
 
         motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
 

RobotLibrary/src/main/java/com/technototes/library/hardware/motor/EncodedMotor.java

@@ -291,4 +291,10 @@ public EncodedMotor<T> coast() {
     public EncodedMotor<T> setLimits(double mi, double ma) {
         return (EncodedMotor<T>) super.setLimits(mi, ma);
     }
+
+    // Ah, Java, you're such a hideous language...
+    public <U extends DcMotorSimple> U getRawMotor(Class<U> type) {
+        T device = getRawDevice();
+        return (device != null && type.isInstance(device)) ? type.cast(device) : null;
+    }
 }

RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/encoder/ExternalEncoder.java

@@ -43,6 +43,10 @@ public void zeroEncoder() {
      */
     @Override
     public double getSensorValue() {
-        return getDevice().getVoltage() - zero;
+        AnalogInput device = getRawDevice();
+        if (device != null) {
+            val = device.getVoltage();
+        }
+        return val - zero;
     }
 }

RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/encoder/MotorEncoder.java

@@ -75,10 +75,6 @@ public MotorEncoder(DcMotorEx motor) {
         this(motor, new ElapsedTime());
     }
 
-    public MotorEncoder(EncodedMotor<DcMotorEx> motor) {
-        this(motor.getDevice());
-    }
-
     public MotorEncoder(String deviceName) {
         this(hardwareMap.get(DcMotorEx.class, deviceName));
     }

Vision/src/main/java/com/technototes/vision/subsystem/BasicVisionSubsystem.java

@@ -60,7 +60,7 @@ public BasicVisionSubsystem(Camera c, int w, int h, OpenCvCameraRotation rot) {
      *
      * @return the Camera device in use
      */
-    public Camera getDevice() {
+    protected Camera getRawDevice() {
         return camera;
     }
 
@@ -211,15 +211,15 @@ protected int countPixelsOfColor(HSVRange range, Mat imgHSV, Mat telemetryRGB, i
         // Check to see which pixels are between low and high, output into a boolean matrix Cr
         Mat count = new Mat();
         Core.inRange(imgHSV, low, high, count);
-        // TODO: It seems like there should be a more optimized way to do this.
-        for (int i = 0; i < count.width(); i++) {
-            for (int j = 0; j < count.height(); j++) {
-                if (count.get(j, i)[0] > 0) {
-                    totalColorCount++;
-                    // Draw a dots on the image at this point - input was put into img
-                    if (telemetryRGB != null) {
+        totalColorCount = Core.countNonZero(count);
+        if (telemetryRGB != null) {
+            // TODO: It seems like there should be a more optimized way to do this.
+            for (int i = 0; i < count.width(); i++) {
+                for (int j = 0; j < count.height(); j++) {
+                    if (count.get(j, i)[0] > 0) {
+                        // Draw a dots on the image at this point - input was put into img
                         // The color choice makes things stripey, which makes it easier to identify
-                        double[] colorToDraw = ((j + i) & 3) != 0 ? low.val : high.val;
+                        double[] colorToDraw = ((j + i) & 2) != 0 ? low.val : high.val;
                         telemetryRGB.put(j + yOffset, i + xOffset, colorToDraw);
                     }
                 }