Synced directions of vision and Odometry localizers

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() {
+
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java

@@ -10,18 +10,18 @@ public class TestingEngine extends CyberarmEngine {
 
     private Robot robot;
 
-    @Override
+//    @Override
-    public void init() {
+//    public void init() {
-        robot = new Robot(hardwareMap);
+//        robot = new Robot(hardwareMap);
-        robot.initHardware();
+//        robot.initHardware();
-        super.init();
+//        super.init();
-    }
+//    }
 
 
 
     @Override
     public void setup() {
-        addState(new EncoderTest(robot));
+        addState(new LauncherTest());
     }
 
 

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;

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("Odo X", robot.ticksToInches(X));
-        engine.telemetry.addData("Y", Y);
+        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);
     }
 }

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.
+        // orientation is inverted to have clockwise be positive.
-        rotation = -imu.getAngularOrientation().firstAngle;
+        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;
             }