From 921660660dbfba351dc2b0c60c6eb35b5bcade0d Mon Sep 17 00:00:00 2001 From: Nathaniel Palme Date: Tue, 3 Nov 2020 20:22:18 -0600 Subject: [PATCH] Created getAngleToTarget function --- .../HardwareTesting/LauncherTest.java | 3 ++ .../LocalizerTesting/VuforiaNavTesting.java | 20 ++++++--- .../org/timecrafters/UltimateGoal/Robot.java | 41 +++++++++++++++++-- 3 files changed, 54 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java index 6ab11cb..55b0dff 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java @@ -9,15 +9,18 @@ import org.timecrafters.UltimateGoal.Robot; public class LauncherTest extends CyberarmState { DcMotor LaunchMotor; + DcMotor SuckMotor; @Override public void init() { LaunchMotor = engine.hardwareMap.dcMotor.get("launcher"); + SuckMotor = engine.hardwareMap.dcMotor.get("collector"); } @Override public void exec() { LaunchMotor.setPower(1); + SuckMotor.setPower(1); } @Override diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java index 385c158..ec93568 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java @@ -10,30 +10,38 @@ public class VuforiaNavTesting extends CyberarmState { private float angle; private double X; private double Y; + private double targetX; + private double targetY; public VuforiaNavTesting(Robot robot) { this.robot = robot; } + @Override + public void init() { + targetX = robot.inchesToTicks(24); + targetY = robot.inchesToTicks(24); + } + @Override public void exec() { - robot.setDrivePower(-0.2 * engine.gamepad1.left_stick_y, -0.2 * engine.gamepad1.right_stick_y); + robot.setDrivePower(-0.5 * engine.gamepad1.left_stick_y, -0.5 * engine.gamepad1.right_stick_y); robot.updateLocation(); X = robot.getLocationX(); Y = robot.getLocationY(); angle = robot.getRotation(); + + } @Override public void telemetry() { engine.telemetry.addData("Target Visible", robot.trackableVisible); - engine.telemetry.addData("Odo X", robot.ticksToInches(X)); engine.telemetry.addData("Odo Y", robot.ticksToInches(Y)); - engine.telemetry.addData("Vis X", robot.visionX); - engine.telemetry.addData("Vis Y", robot.visionY); - - engine.telemetry.addData("Angle", angle); + engine.telemetry.addData("Robot Angle", angle); + engine.telemetry.addData("Raw Angle", robot.rawAngle); + engine.telemetry.addData("Angle to Target", robot.getAngleToPosition(targetX,targetY)); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java index 7c1adbb..d778ac3 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java @@ -28,7 +28,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ; import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX; import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC; -public class Robot { +public class Robot { private HardwareMap hardwareMap; @@ -63,13 +63,14 @@ public class Robot { // Inches Left of axis of rotation static final float CAMERA_LEFT_DISPLACEMENT = 2f; - //Robot Localizatoin + //Robot Localization private double locationX; private double locationY; private float rotation; public double visionX; public double visionY; + public float rawAngle; public double traveledLeft; @@ -171,6 +172,8 @@ public class Robot { targetsUltimateGoal.activate(); } + //TODO : Test range of Tensor Object identification. + private void initTensorFlow() { int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName()); @@ -189,8 +192,15 @@ public class Robot { public void updateLocation(){ // orientation is inverted to have clockwise be positive. float imuAngle = -imu.getAngularOrientation().firstAngle; - float rotationChange = imuAngle - rotationPrevious; + + if (rotationChange > 180) { + rotationChange -= 360; + } + if (rotationChange < -180) { + rotationChange += 360; + } + int encoderLeftCurrent = encoderLeft.getCurrentPosition(); int encoderRightCurrent = encoderRight.getCurrentPosition(); double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious; @@ -212,6 +222,10 @@ public class Robot { locationX += xChange; locationY += yChange; + //TODO : add separate odometer and vision coordinates. + + //TODO : make Odometer Coordinates set to vision coordinates on button push. + trackableVisible = false; for (VuforiaTrackable trackable : trackables) { if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) { @@ -234,7 +248,8 @@ public class Robot { // visionY = translation.get(0) / mmPerInch; - //For our tornament, it makes sence to make zero degrees towards the goal, orientation is inverted to have clockwise be positive. + //For our tournament, it makes sense to make zero degrees towards the goal. + //Orientation is inverted to have clockwise be positive. Orientation rotation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES); this.rotation = 90-rotation.thirdAngle; @@ -245,6 +260,14 @@ public class Robot { break; } } + + if (rotation > 180) { + rotation -= 360; + } + if (rotation < -180) { + rotation += 360; + } + } public float getRotation() { @@ -259,12 +282,22 @@ public class Robot { return locationY; } + public float getAngleToPosition (double x, double y) { + double differenceX = x- getLocationX(); + double differenceY = y- getLocationY(); + double angle = Math.toDegrees(Math.atan2(differenceX, differenceY )); + + return (float) angle; + + } + public double ticksToInches(double ticks) { return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION); } public double inchesToTicks(double inches) { return inches * (COUNTS_PER_REVOLUTION / ENCODER_CIRCUMFERENCE); + } public float getRelativeAngle(float reference, float current) {