From 8ca11b3b253dd580a597599d67b8d86b58030264 Mon Sep 17 00:00:00 2001 From: Nathaniel Palme Date: Tue, 27 Oct 2020 20:11:51 -0500 Subject: [PATCH] Synced directions of vision and Odometry localizers --- .../HardwareTesting/LauncherTest.java | 28 +++++++++++++++ .../HardwareTesting/TestingEngine.java | 14 ++++---- .../LocalizerTestingEngine.java | 3 +- .../LocalizerTesting/VuforiaNavTesting.java | 9 +++-- .../org/timecrafters/UltimateGoal/Robot.java | 34 ++++++++++++++----- 5 files changed, 69 insertions(+), 19 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java new file mode 100644 index 0000000..6ab11cb --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java @@ -0,0 +1,28 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Robot; + + +public class LauncherTest extends CyberarmState { + + DcMotor LaunchMotor; + + @Override + public void init() { + LaunchMotor = engine.hardwareMap.dcMotor.get("launcher"); + } + + @Override + public void exec() { + LaunchMotor.setPower(1); + } + + @Override + public void telemetry() { + + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java index 8e8f22e..6ef9a23 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java @@ -10,18 +10,18 @@ public class TestingEngine extends CyberarmEngine { private Robot robot; - @Override - public void init() { - robot = new Robot(hardwareMap); - robot.initHardware(); - super.init(); - } +// @Override +// public void init() { +// robot = new Robot(hardwareMap); +// robot.initHardware(); +// super.init(); +// } @Override public void setup() { - addState(new EncoderTest(robot)); + addState(new LauncherTest()); } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java index 532e78c..c1f9486 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java @@ -1,11 +1,12 @@ package org.timecrafters.UltimateGoal.LocalizerTesting; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.UltimateGoal.Robot; -@Autonomous (name = "Localizer Test") +@TeleOp(name = "Localizer Test") public class LocalizerTestingEngine extends CyberarmEngine { private Robot robot; 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 da9cff9..385c158 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java @@ -17,6 +17,7 @@ public class VuforiaNavTesting extends CyberarmState { @Override public void exec() { + robot.setDrivePower(-0.2 * engine.gamepad1.left_stick_y, -0.2 * engine.gamepad1.right_stick_y); robot.updateLocation(); X = robot.getLocationX(); Y = robot.getLocationY(); @@ -27,8 +28,12 @@ public class VuforiaNavTesting extends CyberarmState { public void telemetry() { engine.telemetry.addData("Target Visible", robot.trackableVisible); - engine.telemetry.addData("X", X); - engine.telemetry.addData("Y", Y); + 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); } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java index 2685483..7c1adbb 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java @@ -57,17 +57,21 @@ public class Robot { static final float mmPerInch = 25.4f; // Inches Forward of axis of rotation - static final float CAMERA_FORWARD_DISPLACEMENT = 4.0f; + static final float CAMERA_FORWARD_DISPLACEMENT = 4.25f; // Inches above Ground - static final float CAMERA_VERTICAL_DISPLACEMENT = 8.0f; + static final float CAMERA_VERTICAL_DISPLACEMENT = 4.5f; // Inches Left of axis of rotation - static final float CAMERA_LEFT_DISPLACEMENT = 0; + static final float CAMERA_LEFT_DISPLACEMENT = 2f; //Robot Localizatoin private double locationX; private double locationY; private float rotation; + public double visionX; + public double visionY; + + public double traveledLeft; public double traveledRight; @@ -183,10 +187,10 @@ public class Robot { } public void updateLocation(){ - // IMU orientation is inverted to have clockwise be positive. - rotation = -imu.getAngularOrientation().firstAngle; + // orientation is inverted to have clockwise be positive. + float imuAngle = -imu.getAngularOrientation().firstAngle; - float rotationChange = rotation - rotationPrevious; + float rotationChange = imuAngle - rotationPrevious; int encoderLeftCurrent = encoderLeft.getCurrentPosition(); int encoderRightCurrent = encoderRight.getCurrentPosition(); double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious; @@ -194,10 +198,11 @@ public class Robot { traveledLeft += Math.abs(encoderLeftChange); traveledRight += Math.abs(encoderRightChange); + rotation += rotationChange; encoderLeftPrevious = encoderLeftCurrent; encoderRightPrevious = encoderRightCurrent; - rotationPrevious = rotation; + rotationPrevious = imuAngle; double average = (encoderLeftChange+encoderRightChange)/2; @@ -212,7 +217,7 @@ public class Robot { if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) { OpenGLMatrix robotLocation = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); - //TODO : make rotation update with Vuforia Nav. + //this is used for debugging purposes. trackableVisible = true; @@ -223,8 +228,19 @@ public class Robot { VectorF translation = lastConfirmendLocation.getTranslation(); - locationX = inchesToTicks(translation.get(1) / mmPerInch); + locationX = -inchesToTicks(translation.get(1) / mmPerInch); locationY = inchesToTicks( translation.get(0) / mmPerInch); +// visionX = -translation.get(1) / mmPerInch; +// 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. + Orientation rotation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES); + this.rotation = 90-rotation.thirdAngle; + + if (this.rotation > 180) { + this.rotation -= -180; + } break; }