Merge remote-tracking branch 'origin/master'

TeamCode/build.gradle

@@ -20,4 +20,5 @@ repositories {
 
 dependencies {
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
+    implementation project(path: ':TimeCraftersConfigurationTool')
 }

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

@@ -22,7 +22,7 @@ public class EncoderTest extends CyberarmState {
 
         robot.setDrivePower(-0.2 * engine.gamepad1.left_stick_y, -0.2 * engine.gamepad1.right_stick_y);
 
-        ticksLeft=robot.encoderLeft.getCurrentPosition();
+        ticksLeft= robot.encoderLeft.getCurrentPosition();
         ticksRight=robot.encoderRight.getCurrentPosition();
 
 
@@ -50,16 +50,16 @@ public class EncoderTest extends CyberarmState {
     @Override
     public void telemetry() {
 
-        engine.telemetry.addData("controler", engine.gamepad1.left_stick_y);
+        engine.telemetry.addData("controller", engine.gamepad1.left_stick_y);
         engine.telemetry.addLine("Latency Values");
-        engine.telemetry.addData("Y", robot.getLocationY());
+        engine.telemetry.addData("Y", robot.ticksToInches(robot.getLocationY()));
-        engine.telemetry.addData("X", robot.getLocationX());
+        engine.telemetry.addData("X", robot.ticksToInches(robot.getLocationX()));
         engine.telemetry.addLine();
         engine.telemetry.addData("Rotation", robot.getRotation());
         engine.telemetry.addLine();
         engine.telemetry.addLine("Actual Values");
-        engine.telemetry.addData("Left", ticksLeft);
+        engine.telemetry.addData("Left", robot.ticksToInches(ticksLeft));
-        engine.telemetry.addData("Right", ticksRight);
+        engine.telemetry.addData("Right", robot.ticksToInches(ticksRight));
 
 //        engine.telemetry.addLine("");
 //        engine.telemetry.addData("Front", robot.encoderFront);

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

@@ -0,0 +1,86 @@
+package org.timecrafters.UltimateGoal.LocalizerTesting;
+
+import android.util.Log;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.TimeCraftersConfigurationTool.backend.Backend;
+import org.timecrafters.UltimateGoal.Robot;
+
+public class IMUDrive extends CyberarmState {
+
+    private Robot robot;
+    private String actionName;
+    private String groupName;
+    private double power;
+    private int tickTarget;
+    private float angleRelative;
+    private float angleTarget;
+    private int tickStart;
+    private long finishDelay;
+
+    public IMUDrive(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.actionName = actionName;
+        this.groupName = groupName;
+    }
+
+    @Override
+    public void init() {
+        Log.i("Config", Backend.instance().gsonForConfig().toJson(robot.stateConfiguration.getConfig()));
+
+        power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
+        double inchesTarget = robot.stateConfiguration.variable(groupName, actionName, "inches").value();
+        tickTarget = (int) robot.inchesToTicks(inchesTarget);
+        angleTarget = robot.stateConfiguration.variable(groupName, actionName, "angle").value();
+        finishDelay = robot.stateConfiguration.variable(groupName,actionName,"delay").value();
+    }
+
+    @Override
+    public void start() {
+        if (!robot.stateConfiguration.action(groupName,actionName).enabled) {
+            setHasFinished(true);
+        }
+
+        angleTarget=robot.getRotation();
+        tickStart = robot.encoderRight.getCurrentPosition();
+    }
+
+    @Override
+    public void exec() {
+
+        robot.updateLocation();
+
+        int ticksTraveled = Math.abs( robot.encoderRight.getCurrentPosition()-tickStart);
+        if (ticksTraveled > tickTarget) {
+            robot.setDrivePower(0,0);
+            sleep(finishDelay);
+            setHasFinished(true);
+        } else {
+
+            angleRelative = robot.getRelativeAngle(angleTarget, robot.getRotation());
+
+            double turnPowerCorrection = Math.pow(0.03 * angleRelative, 3) + 0.01 * angleRelative;
+
+            double rightPower = power + turnPowerCorrection;
+            double leftPower = power - turnPowerCorrection;
+
+            double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
+
+            robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower);
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Measured Values");
+        engine.telemetry.addData("Y", robot.ticksToInches(robot.getLocationY()));
+        engine.telemetry.addData("X", robot.ticksToInches(robot.getLocationX()));
+        engine.telemetry.addLine();
+        engine.telemetry.addData("Rotation", robot.getRotation());
+        engine.telemetry.addLine();
+        engine.telemetry.addLine("Total Travel");
+        engine.telemetry.addData("Left", robot.ticksToInches(robot.traveledLeft));
+        engine.telemetry.addData("Right", robot.ticksToInches(robot.traveledRight));
+
+    }
+}

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

@@ -0,0 +1,86 @@
+package org.timecrafters.UltimateGoal.LocalizerTesting;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Robot;
+
+public class IMUTurn extends CyberarmState {
+
+    private Robot robot;
+    private String actionName;
+    private String groupName;
+    private double power;
+    private float angleTarget;
+    private int turnDirection;
+    private float angleAllowance;
+    private boolean useOptimalDirection;
+
+    public IMUTurn(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.actionName = actionName;
+        this.groupName = groupName;
+    }
+
+    @Override
+    public void init() {
+        power = robot.stateConfiguration.variable(groupName,actionName, "power").value();
+        angleTarget = robot.stateConfiguration.variable(groupName,actionName, "angle").value();
+        angleAllowance = robot.stateConfiguration.variable(groupName,actionName, "allowance").value();
+        turnDirection = robot.stateConfiguration.variable(groupName, actionName, "direction").value();
+        useOptimalDirection = (turnDirection==0);
+    }
+
+    @Override
+    public void start() {
+        if (!robot.stateConfiguration.action(groupName,actionName).enabled) {
+            setHasFinished(true);
+        }
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        int OptimalDirection = getOptimalDirection(angleTarget, robot.getRotation());
+
+        if (!useOptimalDirection && OptimalDirection == turnDirection){
+            useOptimalDirection = true;
+        }
+
+        if (useOptimalDirection){
+            turnDirection = OptimalDirection;
+        }
+
+        robot.setDrivePower(power * turnDirection,-power * turnDirection);
+
+        if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) {
+            robot.setDrivePower(0,0);
+            setHasFinished(true);
+        }
+
+
+
+    }
+
+    private int getOptimalDirection(float angleTarget, float angleCurrent){
+
+        if (angleCurrent < 0) {
+            angleCurrent += 360;
+        }
+
+        if (angleTarget < 0) {
+            angleTarget += 360;
+        }
+
+        float degreeDifferance = angleTarget - angleCurrent;
+        if (degreeDifferance > 180 || (degreeDifferance < 0 && degreeDifferance > -180)) {
+            return -1;
+        } else {
+            return 1;
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("rotation", robot.getRotation());
+    }
+}

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

@@ -0,0 +1,27 @@
+package org.timecrafters.UltimateGoal.LocalizerTesting;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Robot;
+
+@Autonomous (name = "Localizer Test")
+public class LocalizerTestingEngine extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        addState(new IMUDrive(robot,"group", "010_drive"));
+        addState(new IMUTurn(robot,"group", "020_turn"));
+        addState(new IMUDrive(robot,"group", "030_drive"));
+        addState(new IMUTurn(robot,"group", "040_turn"));
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java

@@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.cyberarm.NeXT.StateConfiguration;
+import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration;
 
 public class Robot {
 
@@ -15,7 +16,7 @@ public class Robot {
         this.hardwareMap = hardwareMap;
     }
 
-    public StateConfiguration stateConfiguration = new StateConfiguration();
+    public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
     public BNO055IMU imu;
 
     //drive system
@@ -25,14 +26,20 @@ public class Robot {
     public DcMotor encoderBack;
     public DcMotor encoderRight;
 
-    double BIAS_LEFT = 1.0;
+    double BIAS_LEFT = -1.0;
-    double BIAS_RIGHT = 0.87;
+    double BIAS_RIGHT = -0.87;
+
+    double Circumference_Encoder = Math.PI * 4;
+    int Counts_Per_Revolution = 8192;
 
     //Robot Localizatoin
     private double locationX;
     private double locationY;
     private float rotation;
 
+    public double traveledLeft;
+    public double traveledRight;
+
     private int encoderFrontPrevious = 0;
     private int encoderLeftPrevious = 0;
     private int encoderBackPrevious = 0;
@@ -46,7 +53,12 @@ public class Robot {
 //        encoderBack = hardwareMap.dcMotor.get("encoderBack");
         encoderRight = hardwareMap.dcMotor.get("encoderRight");
 
-        encoderLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        encoderRight.setDirection(DcMotorSimple.Direction.REVERSE);
         
         encoderLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         encoderRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -68,13 +80,18 @@ public class Robot {
     }
 
     public void updateLocation(){
-        rotation = imu.getAngularOrientation().firstAngle;
+        // IMU orientation is inverted to have clockwise be positive.
+        rotation = -imu.getAngularOrientation().firstAngle;
+
         float rotationChange = rotation - rotationPrevious;
         int encoderLeftCurrent = encoderLeft.getCurrentPosition();
         int encoderRightCurrent = encoderRight.getCurrentPosition();
         double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
         double encoderRightChange = encoderRightCurrent - encoderRightPrevious;
 
+        traveledLeft += Math.abs(encoderLeftChange);
+        traveledRight += Math.abs(encoderRightChange);
+
         encoderLeftPrevious = encoderLeftCurrent;
         encoderRightPrevious = encoderRightCurrent;
         rotationPrevious = rotation;
@@ -89,7 +106,7 @@ public class Robot {
 
     }
 
-    public double getRotation() {
+    public float getRotation() {
         return rotation;
     }
 
@@ -100,4 +117,25 @@ public class Robot {
     public double getLocationY() {
         return locationY;
     }
+
+    public double ticksToInches(double ticks) {
+        return ticks * (Circumference_Encoder / Counts_Per_Revolution);
+    }
+
+    public double inchesToTicks(double inches) {
+        return inches * (Counts_Per_Revolution / Circumference_Encoder);
+    }
+
+    public float getRelativeAngle(float reference, float current) {
+        float relative = current - reference;
+
+        if (relative < -180) {
+            relative += 360;
+        }
+
+        if (relative > 180) {
+            relative -= 360;
+        }
+        return relative;
+    }
 }

TimeCraftersConfigurationTool/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/TimeCraftersConfiguration.java

@@ -70,7 +70,7 @@ public class TimeCraftersConfiguration {
     }
 
     public Variable variable(String groupName, String actionName, String variableName) {
-        final Action action = action(groupName, groupName);
+        final Action action = action(groupName, actionName);
 
         for (Variable variable : action.getVariables()) {
             if (variable.name.trim().equals(variableName.trim())) {