Added IMU assisted motion states for Localization testing. Fixed bug in ActionConfig

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/LocalizerTesting/IMUDrive.java

@@ -0,0 +1,64 @@
+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;
+
+    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();
+    }
+
+    @Override
+    public void start() {
+        angleTarget=robot.getRotation();
+    }
+
+    @Override
+    public void exec() {
+
+        robot.updateLocation();
+
+        if (Math.abs(robot.encoderRight.getCurrentPosition()) > tickTarget) {
+            robot.encoderRight.setPower(0);
+            robot.encoderLeft.setPower(0);
+            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.encoderRight.setPower(rightPower * powerAdjust);
+            robot.encoderLeft.setPower(leftPower * powerAdjust);
+        }
+    }
+}

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

@@ -0,0 +1,66 @@
+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 double 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 exec() {
+        robot.updateLocation();
+
+        int OptimalDirection;
+
+        if (angleTarget > 180 + robot.getRotation()){
+            OptimalDirection = -1;
+        } else if (angleTarget < -180 + robot.getRotation()){
+            OptimalDirection = 1;
+        } else if (angleTarget > robot.getRotation()){
+            OptimalDirection = 1;
+        }else {
+            OptimalDirection = -1;
+        }
+
+        if (!useOptimalDirection && OptimalDirection == turnDirection){
+            useOptimalDirection = true;
+        }
+
+        if (useOptimalDirection){
+            turnDirection = OptimalDirection;
+        }
+
+        robot.encoderLeft.setPower(-power * turnDirection);
+        robot.encoderRight.setPower(power * turnDirection);
+
+        if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) {
+            robot.encoderRight.setPower(0);
+            robot.encoderLeft.setPower(0);
+            setHasFinished(true);
+        }
+
+    }
+}

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

@@ -0,0 +1,24 @@
+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"));
+    }
+}

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
@@ -92,7 +93,7 @@ public class Robot {
 
     }
 
-    public double getRotation() {
+    public float getRotation() {
         return rotation;
     }
 
@@ -111,4 +112,17 @@ public class Robot {
     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())) {