Bugfixes for Vision, UserSelection. Confirmed UserSelection SAFE for …

…use.
Murray-Bridge-Bunyips · Nov 3, 2023 · 8d22c25 · 8d22c25
1 parent d6cb8cb
commit 8d22c25
Show file tree

Hide file tree

Showing 11 changed files with 83 additions and 47 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/BunyipsOpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/BunyipsOpMode.kt
@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.common
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
 import org.firstinspires.ftc.robotcore.external.Telemetry
 import org.firstinspires.ftc.robotcore.external.Telemetry.Item
+import java.util.Locale
 import kotlin.math.roundToInt
 
 /**
@@ -179,6 +180,11 @@ abstract class BunyipsOpMode : LinearOpMode() {
         return addTelemetry(value, false)
     }
 
+    fun addTelemetry(formatString: String, vararg objs: Any): Item {
+        // FIXME: Broken! May have to do with Java interop
+        return addTelemetry(String.format(Locale.getDefault(), formatString, objs))
+    }
+
     /**
      * Log a message to the telemetry log
      * @param message The message to log

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/ErrorUtil.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/ErrorUtil.kt
@@ -7,7 +7,7 @@ import java.io.StringWriter
  * Util to prevent unhandled exceptions from crashing the app
  */
 object ErrorUtil {
-    private const val MAX_STACKTRACE_CHARS = 150
+    private const val MAX_STACKTRACE_CHARS = 200
 
     @Throws(InterruptedException::class)
     fun handleCatchAllException(e: Throwable, log: (msg: String) -> Unit) {

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/GearedPivotMotor.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/GearedPivotMotor.kt
@@ -35,11 +35,11 @@ class GearedPivotMotor(
     }
 
     fun setDegrees(degrees: Double) {
-        motor.targetPosition = ((degrees / 360) * ticksPerRevolution).toInt()
+        motor.targetPosition = ((degrees / 360) * ticksPerRevolution / gearRatio).toInt()
     }
 
     fun setRadians(radians: Double) {
-        motor.targetPosition = ((radians / (2 * Math.PI)) * ticksPerRevolution).toInt()
+        motor.targetPosition = ((radians / (2 * Math.PI)) * ticksPerRevolution / gearRatio).toInt()
     }
 
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/UserSelection.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/UserSelection.kt
@@ -53,7 +53,11 @@ class UserSelection<T>(
      */
     override fun run() {
         if (opmodes.isEmpty()) {
-            callback(null)
+            try {
+                callback(null)
+            } catch (e: Exception) {
+                ErrorUtil.handleCatchAllException(e, opMode::log)
+            }
         }
 
         val buttons: HashMap<T, Controller> = Controller.mapArgs(opmodes)
@@ -119,6 +123,10 @@ class UserSelection<T>(
         opMode.telemetry.update()
         opMode.setTelemetryAutoClear(true)
 
-        callback(selectedOpMode)
+        try {
+            callback(selectedOpMode)
+        } catch (e: Exception) {
+            ErrorUtil.handleCatchAllException(e, opMode::log)
+        }
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/Vision.java
@@ -35,22 +35,6 @@ public Vision(@NonNull BunyipsOpMode opMode, WebcamName webcam) {
         this.webcam = webcam;
     }
 
-    /**
-     * Builds the VisionPortal after the VisionPortal has been constructed.
-     *
-     * @param builder Processor-rich builder pattern for the VisionPortal
-     * @return VisionPortalImpl
-     */
-    private VisionPortal constructVisionPortal(VisionPortal.Builder builder) {
-        return builder
-                .setCamera(webcam)
-                .setCameraResolution(new Size(CAMERA_WIDTH, CAMERA_HEIGHT))
-                .enableLiveView(true)
-                .setAutoStopLiveView(true)
-                // Set any additional VisionPortal settings here
-                .build();
-    }
-
     /**
      * Get all VisionProcessors attached to the VisionPortal.
      */
@@ -99,7 +83,13 @@ public void init(Processor... processors) {
             builder.addProcessor(processor);
         }
 
-        visionPortal = constructVisionPortal(builder);
+        visionPortal = builder
+                            .setCamera(webcam)
+                            .setCameraResolution(new Size(CAMERA_WIDTH, CAMERA_HEIGHT))
+                            .enableLiveView(true)
+                            .setAutoStopLiveView(true)
+                            // Set any additional VisionPortal settings here
+                            .build();
 
         // Disable the vision processors by default. The OpMode must call start() to enable them.
         for (Processor processor : this.processors) {

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/TeamProp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/TeamProp.java
@@ -17,7 +17,7 @@
  * Detection for a custom team prop based on colour ranges,
  * refactored to work with our vision system
  *
- * @author FTC 14133, <a href="https://github.com/FTC14133/FTC14133-2023-2024/blob/Detection-TeamElement/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/TeamElementDetection/TeamElementSubsystem.java">...</a>
+ * @author FTC 14133, <a href="https://github.com/FTC14133/FTC14133-2023-2024/blob/Detection-TeamElement/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/TeamElementDetection/Pipeline/SplitAveragePipeline.java">...</a>
  */
 public class TeamProp extends Processor<TeamPropData> {
     private final List<Integer> ELEMENT_COLOR;
@@ -60,16 +60,17 @@ public Object processFrame(Mat frame, long captureTimeNanos) {
         zone2.setTo(avgColor2);
         zone3.setTo(avgColor3);
 
-        double distance1 = color_distance(avgColor1, ELEMENT_COLOR);
-        double distance2 = color_distance(avgColor2, ELEMENT_COLOR);
-        double distance3 = color_distance(avgColor3, ELEMENT_COLOR);
-        double max_distance = Math.min(distance3, Math.min(distance1, distance2));
+        distance1 = color_distance(avgColor1, ELEMENT_COLOR);
+        distance2 = color_distance(avgColor2, ELEMENT_COLOR);
+        distance3 = color_distance(avgColor3, ELEMENT_COLOR);
+        max_distance = Math.min(distance3, Math.min(distance1, distance2));
 
         return frame;
     }
 
     @Override
     public void init(int width, int height, CameraCalibration calibration) {
+        // noop
     }
 
     @SuppressWarnings("rawtypes")
@@ -88,6 +89,7 @@ public double color_distance(Scalar color1, List color2){
 
     @Override
     public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
+        // noop
     }
 
     @Override
@@ -97,12 +99,13 @@ public String getName() {
 
     @Override
     public void tick() {
+        data.clear();
         if (max_distance == distance1) {
-            data.add(new TeamPropData(Positions.LEFT));
+            data.add(new TeamPropData(Positions.LEFT, distance1, distance2, distance3, max_distance));
         } else if (max_distance == distance2) {
-            data.add(new TeamPropData(Positions.CENTER));
+            data.add(new TeamPropData(Positions.CENTER, distance1, distance2, distance3, max_distance));
         } else {
-            data.add(new TeamPropData(Positions.RIGHT));
+            data.add(new TeamPropData(Positions.RIGHT, distance1, distance2, distance3, max_distance));
         }
     }
 

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/data/TeamPropData.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/data/TeamPropData.kt
@@ -10,5 +10,21 @@ data class TeamPropData(
     /**
      * Position of the prop in the image.
      */
-    val position: TeamProp.Positions
+    val position: TeamProp.Positions,
+    /**
+     * Colour distance of section 1.
+     */
+    val section1: Double,
+    /**
+     * Colour distance of section 2.
+     */
+    val section2: Double,
+    /**
+     * Colour distance of section 3.
+     */
+    val section3: Double,
+    /**
+     * Maximum colour distance of all three sections.
+     */
+    val maxDistance: Double
 ) : VisionData()
diff --git a/...Code/src/main/java/org/firstinspires/ftc/teamcode/glados/components/GLaDOSConfigCore.java b/...Code/src/main/java/org/firstinspires/ftc/teamcode/glados/components/GLaDOSConfigCore.java
@@ -6,6 +6,7 @@
 import com.qualcomm.robotcore.hardware.IMU;
 
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.common.GearedPivotMotor;
 import org.firstinspires.ftc.teamcode.common.PivotMotor;
 import org.firstinspires.ftc.teamcode.common.RobotConfig;
 
@@ -27,12 +28,13 @@ public class GLaDOSConfigCore extends RobotConfig {
     public DcMotorEx bl;
     // Control 0: Suspender Actuator "sa"
     public DcMotorEx sa;
-    // Control 1: Suspender Rotation "sr", ?->90T
-    public PivotMotor sr;
+    // Control 1: Suspender Rotation "sr", 45T->90T
+    public GearedPivotMotor sr;
 
     public IMU imu;
 
     protected static final double CORE_HEX_TICKS_PER_REVOLUTION = 288;
+    protected static final double SR_GEAR_RATIO = 45.0 / 90.0;
 
     @Override
     protected void init() {
@@ -43,7 +45,7 @@ protected void init() {
         bl = (DcMotorEx) getHardware("bl", DcMotorEx.class);
         DcMotorEx SRmotor = (DcMotorEx) getHardware("sr", DcMotorEx.class);
         if (SRmotor != null) {
-            sr = new PivotMotor(SRmotor, CORE_HEX_TICKS_PER_REVOLUTION);
+            sr = new GearedPivotMotor(SRmotor, CORE_HEX_TICKS_PER_REVOLUTION, SR_GEAR_RATIO);
         }
         sa = (DcMotorEx) getHardware("sa", DcMotorEx.class);
         imu = (IMU) getHardware("imu", IMU.class);

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/glados/debug/GLaDOSPivotRunner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/glados/debug/GLaDOSPivotRunner.java
@@ -13,8 +13,7 @@
  *
  * @author Lucas Bubner, 2023
  */
-@TeleOp(name = "GLaDOS: Pivot Motor Debug", group = "GLaDOS")
-@Disabled
+@TeleOp(name = "GLaDOS: Rotator Motor Runner", group = "GLaDOS")
 public class GLaDOSPivotRunner extends BunyipsOpMode {
     private GLaDOSConfigCore config = new GLaDOSConfigCore();
 

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/glados/debug/GLaDOSRotateTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/glados/debug/GLaDOSRotateTest.java
@@ -5,34 +5,33 @@
 
 import org.firstinspires.ftc.teamcode.common.BunyipsOpMode;
 import org.firstinspires.ftc.teamcode.common.RobotConfig;
+import org.firstinspires.ftc.teamcode.common.Text;
 import org.firstinspires.ftc.teamcode.glados.components.GLaDOSConfigCore;
 
 /**
  * Test arm rotation tracking and control
  *
  * @author Lucas Bubner, 2023
  */
-@TeleOp(name="GLaDOS: Rotator Test", group="GLaDOS")
+@TeleOp(name="GLaDOS: Rotator Motor Degrees Runner", group="GLaDOS")
 public class GLaDOSRotateTest extends BunyipsOpMode {
     private GLaDOSConfigCore config = new GLaDOSConfigCore();
-    double target;
+    double target = 0.0;
 
     @Override
     protected void onInit() {
         config = (GLaDOSConfigCore) RobotConfig.newConfig(this, config, hardwareMap);
+        config.sr.reset();
         config.sr.track();
         config.sr.setTargetPosition(0);
         config.sr.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        config.sr.setPower(0.3);
+        config.sr.setPower(1);
     }
 
     @Override
     protected void activeLoop() {
-        target += gamepad1.left_stick_y * 2;
-        if (gamepad1.a) {
-            target = 0.0;
-        }
-        addTelemetry(String.valueOf(config.sr.getDegrees()));
+        target -= gamepad1.left_stick_y / 2;
+        addTelemetry("Degrees: %d", config.sr.getDegrees());
         config.sr.setDegrees(target);
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/glados/debug/GLaDOSVisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/glados/debug/GLaDOSVisionTest.java
@@ -2,6 +2,7 @@
 
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
+import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.common.BunyipsOpMode;
 import org.firstinspires.ftc.teamcode.common.RobotConfig;
 import org.firstinspires.ftc.teamcode.common.UserSelection;
@@ -22,17 +23,17 @@
  *
  * @author Lucas Bubner, 2023
  */
-@TeleOp(name = "GLaDOS: New Vision Test", group = "GLaDOS")
+@TeleOp(name = "GLaDOS: Vision Test", group = "GLaDOS")
 public class GLaDOSVisionTest extends BunyipsOpMode {
     private GLaDOSConfigCore config = new GLaDOSConfigCore();
 
     private final UserSelection<Procs> proc = new UserSelection<>(this, this::callback, Procs.values());
     private Vision vision;
+    private Telemetry.Item i;
 
     @SuppressWarnings("rawtypes")
     private Unit callback(Procs selection) {
         if (selection == null) {
-            vision.terminate();
             return Unit.INSTANCE;
         }
         ArrayList<Processor> processors = new ArrayList<>();
@@ -60,21 +61,33 @@ private Unit callback(Procs selection) {
         }
         vision.init(processors.toArray(new Processor[0]));
         vision.start(processors.toArray(new Processor[0]));
+        i = addTelemetry("Camera Stream available.", true);
         return Unit.INSTANCE;
     }
 
+    @Override
+    protected boolean onInitLoop() {
+        return !proc.isAlive();
+    }
+
     @Override
     protected void onInit() {
         config = (GLaDOSConfigCore) RobotConfig.newConfig(this, config, hardwareMap);
         vision = new Vision(this, config.webcam);
         proc.start();
     }
 
+    @Override
+    protected void onStart() {
+        removeTelemetryItems(i);
+    }
+
     @Override
     protected void activeLoop() {
         if (vision == null) return;
         vision.tickAll();
-        addTelemetry(String.valueOf(vision.getAllData()));
+//        addTelemetry(String.valueOf(vision.getAllData()));
+        addTelemetry(String.valueOf(vision.getAttachedProcessors().get(0).getData()));
     }
 
     private enum Procs {