From a7779b8229726a6a92559877c9c54fa2acec222c Mon Sep 17 00:00:00 2001 From: Joseph Arakelian Date: Sat, 18 Feb 2017 13:21:13 -0500 Subject: [PATCH 1/2] Updating TestBeaconEllipse.java to strafe towards the side of a button until with a THRESHOLD ... 20 (needs more testing) --- .../autonomous/vision/TestBeaconEllipse.java | 41 +++++++++++++++++-- .../autonomous/vision/TestBeaconRange.java | 4 +- 2 files changed, 39 insertions(+), 6 deletions(-) diff --git a/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconEllipse.java b/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconEllipse.java index d2277cd..5fc3348 100644 --- a/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconEllipse.java +++ b/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconEllipse.java @@ -10,6 +10,7 @@ import org.lasarobotics.vision.opmode.LinearVisionOpMode; import org.lasarobotics.vision.opmode.extensions.CameraControlExtension; import org.lasarobotics.vision.util.ScreenOrientation; +import org.opencv.core.Point; import org.opencv.core.Size; import chawks.hardware.Dutchess; @@ -21,10 +22,17 @@ public class TestBeaconEllipse extends LinearVisionOpMode { private final int CAMERA_WIDTH = 900; private final int CAMERA_HEIGHT = CAMERA_WIDTH / 12 * 9; private final Size CAMERA_SIZE = new Size(CAMERA_WIDTH, CAMERA_HEIGHT); + private final int THRESHOLD = 20; + + private Dutchess robot = new Dutchess(); + + private MovementController movementController; + private Thread movementThread; @Override public void runOpMode() throws InterruptedException { initializeVision(); + initHardware(); // wait for op-mode start waitForStart(); @@ -34,7 +42,17 @@ public void runOpMode() throws InterruptedException { shutDown(); } + public void initHardware() { + robot.init(hardwareMap); + movementController = new MovementController(robot, telemetry); + movementThread = new Thread(movementController); + movementThread.start(); + } + + public void shutDown() { + robot.stopAllWheels(); + movementController.stop(); } private boolean checkColors() { @@ -43,6 +61,9 @@ private boolean checkColors() { int numMeasurements = 0; double minConfidence = .9; + Point rightButtonLocation; + Point leftButtonLocation; + for (; ; ) { if (!opModeIsActive()) { return false; @@ -85,18 +106,30 @@ private boolean checkColors() { if (rightButton != null) { telemetry.addLine("RightButton Location: " + rightButton.getLocationString()); + rightButtonLocation = rightButton.center(); + double distanceFromCenter = (this.getFrameSize().width / 2) - rightButtonLocation.x; + if (Math.abs(distanceFromCenter) > THRESHOLD && isRightRed) { + movementController.strafeRight(1); + } } else { telemetry.addLine("RightButton is NULL"); } if (leftButton != null) { telemetry.addLine("LeftButton Location: " + leftButton.getLocationString()); + leftButtonLocation = leftButton.center(); + double distanceFromCenter = (this.getFrameSize().width / 2) - leftButtonLocation.x; + if (Math.abs(distanceFromCenter) > THRESHOLD && isLeftRed) { + movementController.strafeRight(1); + } else { + telemetry.addLine("LeftButton is NULL"); + } + } else { - telemetry.addLine("LeftButton is NULL"); + // TODO: Unknown what to do when unaware of beacon + telemetry.addLine("Aren't I supposed to be seeing something?"); } - } else { - // TODO: Unknown what to do when unaware of beacon + telemetry.addLine("Beacon Red and Blue: (" + beaconAnalysis.getColorString() + ")"); } - telemetry.addLine("Beacon Red and Blue: (" + beaconAnalysis.getColorString() + ")"); } } diff --git a/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconRange.java b/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconRange.java index 21011a3..5da78c4 100644 --- a/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconRange.java +++ b/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconRange.java @@ -97,9 +97,9 @@ private boolean checkColors() { } if (isRightBlue && isLeftRed) { - //movementController.turn(-5); + movementController.turn(-5); } else if (isLeftBlue && isRightRed) { - //movementController.turn(5); + movementController.turn(5); } From 8bac3c0a4e8d889788a27dcf5da6ba232f88d353 Mon Sep 17 00:00:00 2001 From: Joseph Arakelian Date: Sat, 18 Feb 2017 14:29:20 -0500 Subject: [PATCH 2/2] working with strafe in testbeaconellipse --- .../autonomous/vision/TestBeaconEllipse.java | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconEllipse.java b/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconEllipse.java index 5fc3348..35b1664 100644 --- a/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconEllipse.java +++ b/FtcRobotController/src/main/java/chawks/autonomous/vision/TestBeaconEllipse.java @@ -111,6 +111,9 @@ private boolean checkColors() { if (Math.abs(distanceFromCenter) > THRESHOLD && isRightRed) { movementController.strafeRight(1); } + telemetry.addData("Distance: %.2f", distanceFromCenter); + telemetry.addData("Frame Center X: %.2f", this.getFrameSize().width / 2); + telemetry.addData("Right Button X: %.2f", rightButtonLocation.x); } else { telemetry.addLine("RightButton is NULL"); } @@ -120,16 +123,18 @@ private boolean checkColors() { double distanceFromCenter = (this.getFrameSize().width / 2) - leftButtonLocation.x; if (Math.abs(distanceFromCenter) > THRESHOLD && isLeftRed) { movementController.strafeRight(1); - } else { - telemetry.addLine("LeftButton is NULL"); } - + telemetry.addData("Distance: %.2f", distanceFromCenter); + telemetry.addData("Frame Center X: %.2f", this.getFrameSize().width / 2); + telemetry.addData("Right Button X: %.2f", leftButtonLocation.x); } else { - // TODO: Unknown what to do when unaware of beacon - telemetry.addLine("Aren't I supposed to be seeing something?"); + telemetry.addLine("LeftButton is NULL"); } - telemetry.addLine("Beacon Red and Blue: (" + beaconAnalysis.getColorString() + ")"); + } else { + // TODO: Unknown what to do when unaware of beacon + telemetry.addLine("Aren't I supposed to be seeing something?"); } + telemetry.addLine("Beacon Red and Blue: (" + beaconAnalysis.getColorString() + ")"); } } @@ -146,7 +151,7 @@ private void initializeVision() throws InterruptedException { enableExtensions(); /** Beacon Analysis Method : COMMAND/CONTROL CLICK "AnalysisMethod" TO VIEW OTHER RENDERS */ - beacon.setAnalysisMethod(Beacon.AnalysisMethod.FAST); + beacon.setAnalysisMethod(Beacon.AnalysisMethod.REALTIME); /** * Set color tolerances