diff --git a/Path/src/main/java/com/technototes/path/subsystem/PathingMecanumDrivebaseSubsystem.java b/Path/src/main/java/com/technototes/path/subsystem/PathingMecanumDrivebaseSubsystem.java index 20c7b813..fecd2046 100644 --- a/Path/src/main/java/com/technototes/path/subsystem/PathingMecanumDrivebaseSubsystem.java +++ b/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); diff --git a/RobotLibrary/src/main/java/com/technototes/library/hardware/motor/EncodedMotor.java b/RobotLibrary/src/main/java/com/technototes/library/hardware/motor/EncodedMotor.java index 9159bac8..bf52cee5 100644 --- a/RobotLibrary/src/main/java/com/technototes/library/hardware/motor/EncodedMotor.java +++ b/RobotLibrary/src/main/java/com/technototes/library/hardware/motor/EncodedMotor.java @@ -291,4 +291,10 @@ public EncodedMotor coast() { public EncodedMotor setLimits(double mi, double ma) { return (EncodedMotor) super.setLimits(mi, ma); } + + // Ah, Java, you're such a hideous language... + public U getRawMotor(Class type) { + T device = getRawDevice(); + return (device != null && type.isInstance(device)) ? type.cast(device) : null; + } } diff --git a/RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/encoder/ExternalEncoder.java b/RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/encoder/ExternalEncoder.java index d3ce9a95..6f1cd74b 100644 --- a/RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/encoder/ExternalEncoder.java +++ b/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; } } diff --git a/RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/encoder/MotorEncoder.java b/RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/encoder/MotorEncoder.java index 15eb8f36..140c71a4 100644 --- a/RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/encoder/MotorEncoder.java +++ b/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 motor) { - this(motor.getDevice()); - } - public MotorEncoder(String deviceName) { this(hardwareMap.get(DcMotorEx.class, deviceName)); } diff --git a/Vision/src/main/java/com/technototes/vision/subsystem/BasicVisionSubsystem.java b/Vision/src/main/java/com/technototes/vision/subsystem/BasicVisionSubsystem.java index 3f09882f..c5613a83 100644 --- a/Vision/src/main/java/com/technototes/vision/subsystem/BasicVisionSubsystem.java +++ b/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); } }