Shenanigans and Navigation

TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java

@@ -106,6 +106,8 @@ public abstract class CyberarmEngine extends OpMode {
     for (CyberarmState state: cyberarmStates) {
       stopState(state);
     }
+
+
   }
 
   /**

TeamCode/src/main/java/org/samples/ConceptTensorFlowObjectDetectionWebcam.java

@@ -145,18 +145,17 @@ public class ConceptTensorFlowObjectDetectionWebcam extends LinearOpMode {
      * Initialize the Vuforia localization engine.
      */
     private void initVuforia() {
-        /*
+        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
-         * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
+        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
-         */
-        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
 
-        parameters.vuforiaLicenseKey = VUFORIA_KEY;
+        parameters.vuforiaLicenseKey = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
-        parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
+        parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");;
+        parameters.useExtendedTracking = false;
 
 
-        //  Instantiate the Vuforia engine
         vuforia = ClassFactory.getInstance().createVuforia(parameters);
 
+
         // Loading trackables is not necessary for the TensorFlow Object Detection engine.
     }
 
@@ -170,5 +169,6 @@ public class ConceptTensorFlowObjectDetectionWebcam extends LinearOpMode {
        tfodParameters.minResultConfidence = 0.8f;
        tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
        tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT);
+
     }
 }

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

@@ -21,7 +21,7 @@ public class TestingEngine extends CyberarmEngine {
 
     @Override
     public void setup() {
-        addState(new LauncherTest());
+        addState(new WelcomeToJankyTown());
     }
 
 

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

@@ -0,0 +1,30 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+
+public class WelcomeToJankyTown extends CyberarmState {
+
+    DcMotor left;
+    DcMotor right;
+
+    @Override
+    public void init() {
+        left = engine.hardwareMap.dcMotor.get("left");
+        right = engine.hardwareMap.dcMotor.get("right");
+    }
+
+    @Override
+    public void exec() {
+        left.setPower(-engine.gamepad1.left_stick_y);
+        right.setPower(engine.gamepad1.right_stick_y);
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Welcome to Janky Town!");
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java

@@ -0,0 +1,66 @@
+package org.timecrafters.UltimateGoal.LocalizerTesting;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Robot;
+
+public class DriveToPosition extends CyberarmState {
+
+    private Robot robot;
+    private String groupName;
+    private String actionName;
+    private double targetX;
+    private double targetY;
+    private double translation;
+    private double tolerance;
+    private double power;
+
+    public DriveToPosition(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+    }
+
+    @Override
+    public void init() {
+//        translation = robot.inchesToTicks(4);
+        targetX = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"x").value());
+        targetY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"y").value());
+        tolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"tolerance").value());
+        power = robot.stateConfiguration.variable(groupName,actionName,"power").value();
+    }
+
+    @Override
+    public void start() {
+
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+        robot.record();
+
+        double distanceRemaining = Math.hypot(targetX - robot.getLocationX(), targetY - robot.getLocationY());
+
+        //Math.pow(3, distanceRemaining - translation) + 0.3;
+
+//            if (power > 0.65) {
+//                power = 0.65;
+//            }
+
+        if (distanceRemaining < tolerance) {
+            robot.setDrivePower(0, 0);
+            setHasFinished(true);
+        } else {
+            robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power);
+        }
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Target Visible", robot.trackableVisible);
+        engine.telemetry.addData("Odo X", robot.ticksToInches(robot.getLocationX()));
+        engine.telemetry.addData("Odo Y", robot.ticksToInches(robot.getLocationY()));
+        engine.telemetry.addData(" Rotation", robot.getRotation());
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java

@@ -21,6 +21,15 @@ public class LocalizerTestingEngine extends CyberarmEngine {
     @Override
     public void setup() {
         addState(new VuforiaNavTesting(robot));
+//        addState(new DriveToPosition(robot, "DtoP", "d1"));
+//        addState(new DriveToPosition(robot, "DtoP", "d2"));
 
     }
+
+    @Override
+    public void stop() {
+        super.stop();
+        robot.saveRecording();
+        robot.deactivateVuforia();
+    }
 }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java

@@ -10,28 +10,36 @@ public class VuforiaNavTesting extends CyberarmState {
     private float angle;
     private double X;
     private double Y;
-    private double targetX;
+    private float joystickDegrees;
-    private double targetY;
+    private double power;
 
     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.5 * engine.gamepad1.left_stick_y, -0.5 * engine.gamepad1.right_stick_y);
+        if (engine.gamepad1.x) {
+            robot.syncWithVuforia();
+        }
+        
         robot.updateLocation();
+        robot.record();
+
+        double joystickX = engine.gamepad1.right_stick_x;
+        double joystickY = engine.gamepad1.right_stick_y;
+
+        joystickDegrees = (float) Math.toDegrees(Math.atan2(joystickX, -joystickY));
+        power = 0.3 * Math.hypot(joystickX, joystickY);
+
+        robot.driveAtAngle(joystickDegrees, power);
+
         X = robot.getLocationX();
         Y = robot.getLocationY();
         angle = robot.getRotation();
 
 
+
     }
 
     @Override
@@ -40,8 +48,11 @@ public class VuforiaNavTesting extends CyberarmState {
         engine.telemetry.addData("Odo X", robot.ticksToInches(X));
         engine.telemetry.addData("Odo Y", robot.ticksToInches(Y));
 
+        engine.telemetry.addData("Joystick", joystickDegrees);
+//        engine.telemetry.addData("Vis X", robot.ticksToInches(robot.visionX));
+//        engine.telemetry.addData("Vis Y", robot.ticksToInches(robot.visionY));
+
         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

@@ -1,6 +1,8 @@
 package org.timecrafters.UltimateGoal;
 
 import android.app.Activity;
+import android.os.Environment;
+import android.util.Log;
 
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -19,7 +21,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefau
 import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
 import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
 import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration;
+import org.timecrafters.TimeCraftersConfigurationTool.backend.Backend;
+import org.timecrafters.TimeCraftersConfigurationTool.backend.TAC;
 
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -57,20 +64,21 @@ public class   Robot {
     static final float mmPerInch = 25.4f;
 
     // Inches Forward of axis of rotation
-    static final float CAMERA_FORWARD_DISPLACEMENT  = 4.25f;
+    static final float CAMERA_FORWARD_DISPLACEMENT  = 13f;
     // Inches above Ground
     static final float CAMERA_VERTICAL_DISPLACEMENT = 4.5f;
     // Inches Left of axis of rotation
     static final float CAMERA_LEFT_DISPLACEMENT     = 2f;
 
     //Robot Localization
-    private double locationX;
+    public double locationX;
-    private double locationY;
+    public double locationY;
     private float rotation;
 
     public double visionX;
     public double visionY;
     public float rawAngle;
+    private String TestingRecord = "X,Y,Angle";
 
 
     public double traveledLeft;
@@ -122,6 +130,10 @@ public class   Robot {
         imu.initialize(parameters);
 
         initVuforia();
+
+        rotation = stateConfiguration.variable("system", "startPos", "direction").value();
+        locationX = stateConfiguration.variable("system", "startPos", "x").value();
+        locationY = stateConfiguration.variable("system", "startPos", "y").value();
     }
 
     private void initVuforia() {
@@ -192,7 +204,7 @@ public class   Robot {
     public void updateLocation(){
         // orientation is inverted to have clockwise be positive.
         float imuAngle = -imu.getAngularOrientation().firstAngle;
-        float rotationChange = imuAngle - rotationPrevious;
+        double rotationChange = imuAngle - rotationPrevious;
 
         if (rotationChange > 180) {
             rotationChange -= 360;
@@ -222,17 +234,22 @@ 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.
+        if (rotation > 180) {
+            rotation -= 360;
+        }
+        if (rotation < -180) {
+            rotation += 360;
+        }
 
+    }
+
+    public void syncWithVuforia() {
         trackableVisible = false;
         for (VuforiaTrackable trackable : trackables) {
             if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
                 OpenGLMatrix robotLocation = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
 
-
-
                 //this is used for debugging purposes.
                 trackableVisible = true;
 
@@ -244,8 +261,7 @@ public class   Robot {
                 VectorF translation = lastConfirmendLocation.getTranslation();
                 locationX = -inchesToTicks(translation.get(1) / mmPerInch);
                 locationY = inchesToTicks( translation.get(0) / mmPerInch);
-//                visionX = -translation.get(1) / mmPerInch;
+
-//                visionY = translation.get(0) / mmPerInch;
 
 
                 //For our tournament, it makes sense to make zero degrees towards the goal.
@@ -260,14 +276,6 @@ public class   Robot {
                 break;
             }
         }
-
-        if (rotation > 180) {
-            rotation -= 360;
-        }
-        if (rotation < -180) {
-            rotation += 360;
-        }
-
     }
 
     public float getRotation() {
@@ -282,6 +290,12 @@ public class   Robot {
         return locationY;
     }
 
+    public void setCurrentPosition(float rotation, double x, double y) {
+        this.rotation = rotation;
+        locationX = x;
+        locationY = y;
+    }
+
     public float getAngleToPosition (double x, double y) {
         double differenceX = x- getLocationX();
         double differenceY = y- getLocationY();
@@ -313,7 +327,53 @@ public class   Robot {
         return relative;
     }
 
-    public void deactivateVuforia() {
+    public void driveAtAngle(float angle, double power) {
 
+        double relativeAngle = getRelativeAngle(angle, getRotation());
+
+        //calculate how the power of each motor should be adjusted to make the robot curve
+        //towards the target angle
+        //--------------------------------------------------------------------------------------
+
+        double turnPowerCorrection = Math.pow(0.03 * relativeAngle, 3) + 0.02 * relativeAngle;
+
+        //Adjusts power based on degrees off from target.
+        double leftPower = power - turnPowerCorrection;
+        double rightPower = power + turnPowerCorrection;
+        //--------------------------------------------------------------------------------------
+
+
+        //calculates speed adjuster that slows the motors to be closer to the BasePower while
+        // maintaining the power ratio nesesary to execute the turn.
+        double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
+
+        setDrivePower(leftPower * powerAdjust, rightPower * powerAdjust);
+    }
+
+    public void record() {
+        TestingRecord+="\n"+locationX+","+locationY+","+rotation;
+    }
+
+    public void saveRecording() {
+        writeToFile(Environment.getExternalStorageDirectory().getAbsolutePath()+File.separator+"TimeCrafters_TestingRecord"+File.separator+"RobotTestingRecord.txt", TestingRecord);
+    }
+
+    public boolean writeToFile(String filePath, String content) {
+        try {
+
+            FileWriter writer = new FileWriter(filePath);
+            writer.write(content);
+            writer.close();
+
+            return true;
+
+        } catch (IOException e) {
+            Log.e("RecordTest", e.getLocalizedMessage());
+            return false;
+        }
+    }
+
+    public void deactivateVuforia() {
+        targetsUltimateGoal.deactivate();
     }
 }