Created getAngleToTarget function

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

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));
     }
 }

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) {