Added FTCLib, implemented Arm PID(f) controller

TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java

@@ -14,7 +14,7 @@ import org.timecrafters.Autonomous.States.PathDecision;
 import org.timecrafters.Autonomous.States.RotationState;
 import org.timecrafters.Autonomous.States.ServoCameraRotate;
 import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
 
 //@Autonomous (name = "Left Side")
 @Disabled

TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftStateAutoEngine.java

@@ -3,7 +3,7 @@ package org.timecrafters.Autonomous.Engines;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
 
 @Autonomous(name = "Left Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
 public class LeftStateAutoEngine extends CyberarmEngine {

TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java

@@ -1,7 +1,5 @@
 package org.timecrafters.Autonomous.Engines;
 
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.Autonomous.States.BottomArm;
 import org.timecrafters.Autonomous.States.CollectorDistanceState;
@@ -15,7 +13,7 @@ import org.timecrafters.Autonomous.States.PathDecision;
 import org.timecrafters.Autonomous.States.RotationState;
 import org.timecrafters.Autonomous.States.ServoCameraRotate;
 import org.timecrafters.Autonomous.States.TopArm;
-import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
 
 //@Autonomous (name = "left 2 cone auto")
 

TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java

@@ -5,7 +5,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
 
 public class JunctionAllignmentAngleState extends CyberarmState {
     private final boolean stateDisabled;

TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java

@@ -2,7 +2,7 @@ package org.timecrafters.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
 
 public class JunctionAllignmentDistanceState extends CyberarmState {
     private final boolean stateDisabled;

TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java

@@ -1,7 +1,7 @@
 package org.timecrafters.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
-import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
 
 public class PathDecision extends CyberarmState {
     PhoenixBot1 robot;

TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixTeleOP.java

@@ -1,11 +1,11 @@
-package org.timecrafters.Autonomous.TeleOp.engine;
+package org.timecrafters.TeleOp.engine;
 
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
-import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
-import org.timecrafters.Autonomous.TeleOp.states.TeleOPTankDriver;
-import org.timecrafters.Autonomous.TeleOp.states.TeleOPArmDriver;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
+import org.timecrafters.TeleOp.states.TeleOPArmDriver;
+import org.timecrafters.TeleOp.states.TeleOPTankDriver;
 
 @TeleOp (name = "APhoenixTeleOP")
 

TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixWingEngine.java

@@ -1,4 +1,4 @@
-package org.timecrafters.Autonomous.TeleOp.engine;
+package org.timecrafters.TeleOp.engine;
 
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.hardware.CRServo;

TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserServoHeight.java

@@ -1,4 +1,4 @@
-package org.timecrafters.Autonomous.TeleOp.states;
+package org.timecrafters.TeleOp.states;
 
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
 import com.qualcomm.robotcore.hardware.Servo;

TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.Autonomous.TeleOp.states;
+package org.timecrafters.TeleOp.states;
 
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;

TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java

@@ -1,4 +1,4 @@
-package org.timecrafters.Autonomous.TeleOp.states;
+package org.timecrafters.TeleOp.states;
 
 import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
 import com.qualcomm.hardware.bosch.BNO055IMU;

TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.Autonomous.TeleOp.states;
+package org.timecrafters.TeleOp.states;
 
 import android.annotation.SuppressLint;
 

TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java

@@ -1,4 +1,4 @@
-package org.timecrafters.Autonomous.TeleOp.states;
+package org.timecrafters.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
 

TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixWingState.java

@@ -1,4 +1,4 @@
-package org.timecrafters.Autonomous.TeleOp.states;
+package org.timecrafters.TeleOp.states;
 
 import org.cyberarm.engine.V2.CyberarmState;
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java

@@ -0,0 +1,625 @@
+package org.timecrafters.minibots.cyberarm.phoenix;
+
+import android.annotation.SuppressLint;
+import android.util.Log;
+
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.Blinker;
+import com.qualcomm.robotcore.hardware.CRServoImplEx;
+import com.qualcomm.robotcore.hardware.ColorSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.ServoImplEx;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.firstinspires.ftc.robotcore.external.ClassFactory;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
+
+import java.util.ArrayList;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.CopyOnWriteArrayList;
+import java.util.concurrent.TimeUnit;
+
+public class Robot {
+    private static final String TAG = "Phoenix | Robot";
+    public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
+//    public final ServoImplEx gripper;
+    public final IMU imu;
+    public LynxModule expansionHub;
+
+    public final double imuAngleOffset, initialFacing;
+    public boolean armManuallyControlled = false;
+    public boolean automaticAntiTipActive = false;
+    public boolean hardwareFault = false;
+    public String hardwareFaultMessage = "";
+
+    private Status status = Status.OKAY, lastStatus = Status.OKAY;
+    private final CopyOnWriteArrayList<Status> reportedStatuses = new CopyOnWriteArrayList<>();
+    private final ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>();
+    private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>();
+    private final ConcurrentHashMap<String, Double> motorTargetVelocity = new ConcurrentHashMap<>();
+
+    public enum ArmPosition {
+        COLLECT,
+        GROUND,
+        LOW,
+        MEDIUM,
+        HIGH
+
+    }
+
+    public enum Status {
+        OKAY,
+        MONITORING,
+        WARNING,
+        DANGER
+    }
+
+    private final CyberarmEngine engine;
+    private TimeCraftersConfiguration configuration;
+    private final double radius, diameter;
+
+    private final double wheelRadius, wheelGearRatio, armGearRatio;
+    private final int wheelTicksPerRevolution, armTicksPerRevolution;
+
+    private final VuforiaLocalizer vuforia;
+    private final TFObjectDetector tfod;
+
+    private boolean LEDStatusToggle = false;
+    private double lastLEDStatusAnimationTime = 0;
+
+    public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration) {
+        this.engine = engine;
+        this.configuration = configuration;
+
+        radius = tuningConfig("field_localizer_robot_radius").value();
+        diameter = radius * 2;
+
+        imuAngleOffset = hardwareConfig("imu_angle_offset").value();
+
+        wheelRadius = tuningConfig("wheel_radius").value();
+        wheelGearRatio = tuningConfig("wheel_gear_ratio").value();
+        wheelTicksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value();
+
+        armGearRatio = tuningConfig("arm_gear_ratio").value();
+        armTicksPerRevolution = tuningConfig("arm_ticks_per_revolution").value();
+
+        // FIXME: Rename motors in configuration
+        // Define hardware
+        backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Left");     // MOTOR PORT: 2 - CONTROL HUB
+        frontRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Right"); // MOTOR PORT: 1 - CONTROL HUB
+
+        frontLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Left");   // MOTOR PORT: 3 - CONTROL HUB
+        backRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Right");   // MOTOR PORT: 0 - CONTROL HUB
+
+        // FIXME: Rename lift_drive to arm in hardware config
+        arm = engine.hardwareMap.get(DcMotorEx.class, "ArmMotor");          // MOTOR PORT: 2 - EXPANSION HUB
+
+//        gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper");       // SERVO PORT: ?
+
+        imu = engine.hardwareMap.get(IMU.class, "imu");
+
+
+        // Configure hardware
+        //   MOTORS
+        //      DIRECTION
+        frontLeftDrive.setDirection(hardwareConfig("front_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+        frontRightDrive.setDirection(hardwareConfig("front_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+
+        backLeftDrive.setDirection(hardwareConfig("back_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+        backRightDrive.setDirection(hardwareConfig("back_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+
+        arm.setDirection(hardwareConfig("arm_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
+
+        //      RUNMODE
+        frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        arm.setTargetPosition(0);
+        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        //      ZERO POWER BEHAVIOR
+        frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        //      MOTOR POWER
+        arm.setPower(tuningConfig("arm_automatic_power").value());
+
+        //      IMU
+        IMU.Parameters parameters = new IMU.Parameters(
+                new RevHubOrientationOnRobot(
+                    RevHubOrientationOnRobot.LogoFacingDirection.UP,
+                    RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
+
+        imu.initialize(parameters);
+
+        // Preserve our "initial" facing, since we transform it from zero.
+        initialFacing = facing();
+
+        // BulkRead from Hubs
+        for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+
+            if (!hub.isParent() && expansionHub == null) {
+                expansionHub = hub;
+            }
+        }
+
+        // Set LED pattern
+        if (expansionHub != null) {
+            expansionHub.setPattern(ledPatternOkay());
+        }
+
+        // Webcam
+        vuforia = initVuforia();
+        tfod = initTfod();
+
+        // Drive Encoder Error Setup
+        engine.blackboardSet("left_drive_error", 0);
+        engine.blackboardSet("right_drive_error", 0);
+    }
+
+    private VuforiaLocalizer initVuforia() {
+        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
+
+        parameters.vuforiaLicenseKey = hardwareConfig("vuforia_license_key").value();
+        parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
+
+         return ClassFactory.getInstance().createVuforia(parameters);
+    }
+
+    private TFObjectDetector initTfod() {
+        int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
+                "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+        TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
+        tfodParameters.minResultConfidence = 0.75f;
+        tfodParameters.isModelTensorFlow2 = true;
+        tfodParameters.inputSize = 300;
+        TFObjectDetector tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
+
+        tfod.loadModelFromAsset("PowerPlay.tflite", "1 Bolt", "2 Bulb", "3 Panel");
+
+        return tfod;
+    }
+
+    public void reloadConfig() {
+        String name = configuration.getConfig().getName();
+        configuration = new TimeCraftersConfiguration(name);
+    }
+
+    public void standardTelemetry() {
+        engine.telemetry.addLine();
+
+        // STATUS
+        engine.telemetry.addLine("DATA");
+        engine.telemetry.addData("      Robot Status", status);
+        engine.telemetry.addData("      Hardware Fault", hardwareFault);
+        engine.telemetry.addData("      Hardware Fault Message", hardwareFaultMessage);
+        engine.telemetry.addLine();
+
+        // Motor Powers
+        engine.telemetry.addLine("Motor Powers");
+        engine.telemetry.addData("      Front Left Drive", frontLeftDrive.getPower());
+        engine.telemetry.addData("      Front Right Drive", frontRightDrive.getPower());
+
+        engine.telemetry.addData("      Back Left Drive", backLeftDrive.getPower());
+        engine.telemetry.addData("      Back Right Drive", backRightDrive.getPower());
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", arm.getPower());
+
+        engine.telemetry.addLine();
+
+        // Motor Positions
+        engine.telemetry.addLine("Motor Positions");
+        engine.telemetry.addData("      Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getCurrentPosition()));
+        engine.telemetry.addData("      Front Right Drive", "%d (%8.2f in)", frontRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getCurrentPosition()));
+
+        engine.telemetry.addData("      Back Left Drive", "%d (%8.2f in)", backLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getCurrentPosition()));
+        engine.telemetry.addData("      Back Right Drive", "%d (%8.2f in)", backRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getCurrentPosition()));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", "%d (%8.2f degrees)", arm.getCurrentPosition(), ticksToAngle(arm.getCurrentPosition()));
+
+        engine.telemetry.addLine();
+
+        // Motor Target Positions
+        engine.telemetry.addLine("Motor Target Positions");
+        engine.telemetry.addData("      Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getTargetPosition()));
+        engine.telemetry.addData("      Front Right Drive", "%d (%8.2f in)", frontRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getTargetPosition()));
+
+        engine.telemetry.addData("      Back Left Drive", "%d (%8.2f in)", backLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getTargetPosition()));
+        engine.telemetry.addData("      Back Right Drive", "%d (%8.2f in)", backRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getTargetPosition()));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition()));
+
+        // Motor Velocity
+        engine.telemetry.addLine("Motor Velocity");
+        engine.telemetry.addData("      Front Left Drive", "%8.2f (%8.2f in)", frontLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontLeftDrive.getVelocity()));
+        engine.telemetry.addData("      Front Right Drive", "%8.2f (%8.2f in)", frontRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontRightDrive.getVelocity()));
+
+        engine.telemetry.addData("      Back Left Drive", "%8.2f (%8.2f in)", backLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backLeftDrive.getVelocity()));
+        engine.telemetry.addData("      Back Right Drive", "%8.2f (%8.2f in)", backRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backRightDrive.getVelocity()));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", "%8.2f (%8.2f degrees)", arm.getVelocity(), ticksToAngle((int)arm.getVelocity()));
+
+        engine.telemetry.addLine();
+
+        // Motor Currents
+        engine.telemetry.addLine("Motor Currents (AMPS)");
+        engine.telemetry.addData("      Front Left Drive", frontLeftDrive.getCurrent(CurrentUnit.AMPS));
+        engine.telemetry.addData("      Front Right Drive", frontRightDrive.getCurrent(CurrentUnit.AMPS));
+
+        engine.telemetry.addData("      Back Left Drive", backLeftDrive.getCurrent(CurrentUnit.AMPS));
+        engine.telemetry.addData("      Back Right Drive", backRightDrive.getCurrent(CurrentUnit.AMPS));
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", arm.getCurrent(CurrentUnit.AMPS));
+
+        engine.telemetry.addLine();
+
+        // Motor Directions
+        engine.telemetry.addLine("Motor Directions");
+        engine.telemetry.addData("      Front Left Drive", frontLeftDrive.getDirection());
+        engine.telemetry.addData("      Front Right Drive", frontRightDrive.getDirection());
+
+        engine.telemetry.addData("      Back Left Drive", backLeftDrive.getDirection());
+        engine.telemetry.addData("      Back Right Drive", backRightDrive.getDirection());
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("      Arm", arm.getDirection());
+
+        engine.telemetry.addLine();
+
+        // Sensors / IMU
+        engine.telemetry.addLine("IMU");
+        engine.telemetry.addData("      Facing (Degrees)", facing());
+        engine.telemetry.addData("      Heading (Radians)", heading());
+        engine.telemetry.addData("      Turn Rate", turnRate());
+        engine.telemetry.addData("      Angle Offset (Degrees)", imuAngleOffset);
+
+        engine.telemetry.addLine();
+    }
+
+    private ArrayList<Blinker.Step> ledPatternStandby() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0x008000, 750, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x005000, 750, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x002000, 750, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x001000, 250, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternOkay() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0x00aa00, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x00aa44, 500, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternMonitoring() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xaaaaaa, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x000000, 250, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternWarning() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xffff00, 500, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0xff8800, 500, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    private ArrayList<Blinker.Step> ledPatternDanger() {
+        final ArrayList<Blinker.Step> steps = new ArrayList<>();
+
+        steps.add(new Blinker.Step(0xff0000, 250, TimeUnit.MILLISECONDS));
+        steps.add(new Blinker.Step(0x000000, 100, TimeUnit.MILLISECONDS));
+
+        return steps;
+    }
+
+    public void reportStatus(Status status) {
+        reportedStatuses.add(status);
+    }
+
+    public void update() {
+        for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
+            hub.clearBulkCache();
+        }
+
+        double voltage = getVoltage();
+        if (voltage < 9.75) {
+            reportStatus(Status.DANGER);
+            hardwareFaultMessage = "Battery voltage to low! (" + voltage + " volts)";
+
+            hardwareFault = true;
+        }
+
+        status = Status.OKAY;
+        for (Status s : reportedStatuses) {
+            if (s.ordinal() > status.ordinal()) {
+                status = s;
+            }
+        }
+
+        reportedStatuses.clear();
+
+        if (status != lastStatus) {
+            lastStatus = status;
+
+            if (expansionHub != null) {
+                if (lastStatus == Status.OKAY) { expansionHub.setPattern(ledPatternOkay()); }
+                if (lastStatus == Status.MONITORING) { expansionHub.setPattern(ledPatternMonitoring()); }
+                if (lastStatus == Status.WARNING) { expansionHub.setPattern(ledPatternWarning()); }
+                if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); }
+            }
+        }
+    }
+
+    public void stop() {
+        if (expansionHub != null) {
+            expansionHub.setPattern(ledPatternStandby());
+        }
+    }
+
+    public void armPosition(ArmPosition position) {
+        if (hardwareFault) {
+            return;
+        }
+
+        reportStatus(Status.WARNING);
+
+        switch (position) {
+            case COLLECT:
+                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
+                break;
+
+            case GROUND:
+                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value()));
+                break;
+
+            case LOW:
+                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
+                break;
+
+            case MEDIUM:
+                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
+                break;
+
+            case HIGH:
+                arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_high").value()));
+                break;
+
+            default:
+                throw new RuntimeException("Unexpected arm position!");
+        }
+    }
+
+    // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
+    public double angleDiff(double from, double to) {
+        double value = (to - from + 180);
+
+        double fmod = (value - 0.0) % (360.0 - 0.0);
+
+        return (fmod < 0 ? fmod + 360.0 : fmod +  0.0) - 180;
+    }
+
+    public double lerp(double min, double max, double t)
+    {
+        return min + (max - min) * t;
+    }
+
+    public Status getStatus() { return status; }
+
+    public double getRadius() { return radius; }
+
+    public double getDiameter() { return diameter; }
+
+    public double getVoltage() {
+        return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
+    }
+
+    public TFObjectDetector getTfod() { return tfod; }
+
+    public VuforiaLocalizer getVuforia() { return vuforia; }
+
+    public TimeCraftersConfiguration getConfiguration() { return configuration; }
+
+    // For: Drive Wheels
+    public int unitToTicks(DistanceUnit unit, double distance) {
+        double fI = (wheelGearRatio * wheelTicksPerRevolution) / (wheelRadius * 2 * Math.PI * (wheelGearRatio * wheelTicksPerRevolution) / (wheelGearRatio * wheelTicksPerRevolution));
+
+        double inches = unit.toInches(unit.fromUnit(unit, distance));
+
+        double ticks = fI * inches;
+
+        return (int)ticks;
+    }
+
+    // For: Drive Wheels
+    public double ticksToUnit(DistanceUnit unit, int ticks) {
+        // Convert to inches, then to unit.
+        double inches = wheelRadius * 2 * Math.PI * ticks / (wheelGearRatio * wheelTicksPerRevolution);
+
+        return unit.fromUnit(DistanceUnit.INCH, inches);
+    }
+
+    // For: Arm
+    public int angleToTicks(double angle) {
+        double d = (armGearRatio * armTicksPerRevolution) / 360.0;
+
+        // Casting to float so that the int version of Math.round is used.
+        return Math.round((float)d * (float)angle);
+    }
+
+    // For: Arm
+    public double ticksToAngle(int ticks) {
+        double oneDegree = 360.0 / (armGearRatio * armTicksPerRevolution);
+
+        return oneDegree * ticks;
+    }
+
+    public Variable hardwareConfig(String variableName) {
+        Action hardwareConfiguration = configuration.action("Robot", "Hardware");
+
+        for (Variable v : hardwareConfiguration.getVariables()) {
+            if (variableName.trim().equals(v.name)) {
+                return v;
+            }
+        }
+
+        throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Hardware");
+    }
+
+    public Variable tuningConfig(String variableName) {
+        Action action = configuration.action("Robot", "Tuning");
+
+        for (Variable v : action.getVariables()) {
+            if (variableName.trim().equals(v.name)) {
+                return v;
+            }
+        }
+
+        throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Tuning");
+    }
+
+    @SuppressLint("NewApi")
+    public void controlMotorPIDF(DcMotorEx motor, String motorName, double targetVelocity, double feedForward) {
+        Action action = configuration.action("Robot", "Tuning_PIDF_" + motorName);
+        double proportional = 0, integral = 0, derivative = 0;
+
+        for (Variable v : action.getVariables()) {
+            switch (v.name.trim()) {
+                case "proportional":
+                    proportional = v.value();
+                    break;
+                case "integral":
+                    integral = v.value();
+                    break;
+                case "derivative":
+                    derivative = v.value();
+                    break;
+            }
+        }
+
+        double interval = (engine.getRuntime() - motorVelocityLastTiming.getOrDefault(motorName, 0.0));
+
+        double distance = motor.getTargetPosition() - motor.getCurrentPosition();
+
+        if (Math.abs(distance) < Math.abs(targetVelocity)) {
+            if ((targetVelocity < 0 && distance > 0) || (targetVelocity > 0 && distance < 0)) {
+                targetVelocity = -distance;
+            } else {
+                targetVelocity = distance;
+            }
+        }
+
+        double error = targetVelocity - motor.getVelocity();
+        double deltaError = error - motorVelocityError.getOrDefault(motorName, 0.0);
+
+        double kIntegral = error * interval;
+        double kDerivative = deltaError / interval;
+
+        double kp = proportional * error;
+        double ki = integral * kIntegral;
+        double kd = derivative * kDerivative;
+
+        motorVelocityError.put(motorName, error);
+        motorVelocityLastTiming.put(motorName, engine.getRuntime());
+
+        double newTargetVelocity = kp + ki + kd + targetVelocity;
+
+        motor.setVelocity(newTargetVelocity);
+
+        Log.d(TAG, "Interval: " + interval + "ms, Error: " + error + " ticks, deltaError: " + deltaError + " ticks, distance: " +
+                distance + " ticks, kIntegral: " + kIntegral + ", kDerivative: " + kDerivative + ", proportional: " + proportional +
+                ", integral: " + integral + ", derivative: " + derivative + ", kp: " + kp + ", ki: " + ki + ", kd: " + kd +
+                ", targetVelocity: " + targetVelocity + " ticks, new Target Velocity: " + newTargetVelocity + " ticks, + motorVelocity: " + motor.getVelocity() + " ticks.");
+    }
+
+    @SuppressLint("NewApi")
+    public void controlArmMotor(double targetVelocity) {
+//        double time = System.currentTimeMillis();
+//        double newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity);
+//        double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time);
+//        double deltaTime = (time - lastTiming) * 0.001;
+//
+//        double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition();
+//        double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity;
+//
+//        double error = adjustedTargetVelocity - arm.getVelocity();
+//        double kp = 0.9;
+//
+//        newTargetVelocity += error * kp * deltaTime;
+//
+//        motorTargetVelocity.put("Arm", newTargetVelocity);
+//        motorVelocityLastTiming.put("Arm", time);
+
+//        arm.setVelocity(newTargetVelocity);
+
+        arm.setPower(tuningConfig("arm_automatic_power").value());
+    }
+
+    public double initialFacing() {
+        return initialFacing;
+    }
+
+    public double facing() {
+        double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+        return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
+    }
+
+    public double heading() {
+        return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0);
+//        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+    }
+
+    public double turnRate() {
+        return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
+    }
+
+    public boolean isBetween(double value, double min, double max) {
+        return value >= min && value <= max;
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/engines/TeleOp.java

@@ -0,0 +1,47 @@
+package org.timecrafters.minibots.cyberarm.phoenix.engines;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.minibots.cyberarm.phoenix.Robot;
+
+@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Mentor Phoenix | TeleOp", group = "Mentor Phoenix")
+public class TeleOp extends CyberarmEngine {
+    private Robot robot;
+    private TimeCraftersConfiguration configuration;
+    final private String configurationName = "MentorPhoenix", actionsGroupName = "TeleOpStates";
+
+    @Override
+    public void setup() {
+        configuration = new TimeCraftersConfiguration(configurationName);
+
+        robot = new Robot(
+                this,
+                configuration
+        );
+
+        robot.imu.resetYaw();
+
+        setupFromConfig(
+                robot.getConfiguration(),
+                "org.timecrafters.minibots.cyberarm.phoenix.states.teleop",
+                robot,
+                Robot.class,
+                actionsGroupName);
+    }
+
+    @Override
+    public void loop() {
+        robot.update();
+
+        super.loop();
+
+        robot.standardTelemetry();
+    }
+
+    @Override
+    public void stop() {
+        robot.stop();
+
+        super.stop();
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java

@@ -0,0 +1,81 @@
+package org.timecrafters.minibots.cyberarm.phoenix.states.teleop;
+
+import com.arcrobotics.ftclib.controller.PIDController;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
+import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
+import org.timecrafters.minibots.cyberarm.phoenix.Robot;
+
+public class ArmController extends CyberarmState {
+    private final Robot robot;
+    private final String groupName, actionName;
+    private PIDController pidController;
+    private double p = 0, i = 0, d = 0, f = 0;
+    private final double ticksInDegree = 700 / 180;
+
+    public ArmController(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        pidController = new PIDController(p, i, d);
+    }
+
+    @Override
+    public void exec() {
+        Action action = robot.getConfiguration().action("Robot", "Tuning_PIDF_Arm");
+
+        for (Variable v : action.getVariables()) {
+            switch (v.name.trim()) {
+                case "proportional":
+                    p = v.value();
+                    break;
+                case "integral":
+                    i = v.value();
+                    break;
+                case "derivative":
+                    d = v.value();
+                    break;
+                case "feed":
+                    f = v.value();
+                    break;
+            }
+        }
+
+        pidController.setPID(p, i, d);
+        int armPos = robot.arm.getCurrentPosition();
+        double pid = pidController.calculate(armPos, robot.arm.getTargetPosition());
+        double ff = Math.cos(Math.toRadians(robot.arm.getTargetPosition() / ticksInDegree)) * f;
+
+        double power = pid + ff;
+
+        robot.arm.setPower(power);
+    }
+
+    @Override
+    public void buttonDown(Gamepad gamepad, String button) {
+        if (gamepad != engine.gamepad2) {
+            return;
+        }
+
+        switch (button) {
+            case "guide":
+                robot.reloadConfig();
+                break;
+            case "a":
+                robot.armPosition(Robot.ArmPosition.COLLECT);
+                break;
+            case "x":
+                robot.armPosition(Robot.ArmPosition.GROUND);
+                break;
+            case "b":
+                robot.armPosition(Robot.ArmPosition.LOW);
+                break;
+            case "y":
+                robot.armPosition(Robot.ArmPosition.MEDIUM);
+                break;
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/engines/Mini2023Engine.java

@@ -0,0 +1,22 @@
+package org.timecrafters.minibots.engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.minibots.states.Mini2023Bot;
+import org.timecrafters.minibots.states.Mini2023State;
+
+@TeleOp (name= "2023Mini")
+
+public class Mini2023Engine extends CyberarmEngine {
+
+    Mini2023Bot robot;
+
+    @Override
+    public void setup() {
+
+        robot = new Mini2023Bot(this);
+        addState(new Mini2023State(robot));
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023Bot.java

@@ -0,0 +1,36 @@
+package org.timecrafters.minibots.states;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
+import org.timecrafters.minibots.engines.Mini2023Engine;
+
+public class Mini2023Bot {
+
+    private final Mini2023Engine engine;
+    public TimeCraftersConfiguration configuration;
+
+        public DcMotor Opportunity, Spirit, Ingenuity;
+        public CRServo servoA, servoB, servoC;
+
+    public Mini2023Bot(Mini2023Engine engine) {
+        this.engine = engine;
+        setupRobot();
+    }
+
+    private void setupRobot() {
+
+        Opportunity = engine.hardwareMap.get(DcMotor.class, "Left Wheel");
+        Spirit = engine.hardwareMap.get(DcMotor.class, "Right Wheel");
+
+        servoA = engine.hardwareMap.get(CRServo.class, "Servo 1");
+        servoB = engine.hardwareMap.get(CRServo.class, "Servo 2");
+        servoC = engine.hardwareMap.get(CRServo.class, "Servo 3");
+
+
+
+//        configuration = new TimeCraftersConfiguration("2023 Mini");
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023State.java

@@ -0,0 +1,46 @@
+package org.timecrafters.minibots.states;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.minibots.engines.Mini2023Engine;
+
+public class Mini2023State extends CyberarmState {
+    private final Mini2023Bot robot;
+
+    public double driveSpeed, orbitSpeed;
+
+    public Mini2023State(Mini2023Bot robot) {this.robot = robot;}
+
+@Override
+public void init() {
+    driveSpeed = 0;
+    orbitSpeed = 0;
+    robot.servoA.setPower(0);
+    robot.servoB.setPower(0);
+    robot.servoC.setPower(0);
+    robot.Opportunity.setPower(driveSpeed);
+    robot.Spirit.setPower(driveSpeed);
+
+}
+
+@Override
+public void exec() {
+
+    if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
+        orbitSpeed = engine.gamepad2.left_stick_y;
+        robot.Opportunity.setPower(driveSpeed);
+        robot.Spirit.setPower(driveSpeed);
+    }
+
+    if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
+        orbitSpeed = engine.gamepad1.right_stick_x * 0.75;
+        robot.servoA.setPower(orbitSpeed);
+        robot.servoB.setPower(orbitSpeed);
+        robot.servoC.setPower(orbitSpeed);
+    } else {
+        orbitSpeed = 0;
+    }
+
+
+
+}
+}

build.common.gradle

@@ -51,7 +51,7 @@ android {
     defaultConfig {
         signingConfig signingConfigs.debug
         applicationId 'com.qualcomm.ftcrobotcontroller'
-        minSdkVersion 23
+        minSdkVersion 24
         //noinspection ExpiredTargetSdkVersion
         targetSdkVersion 28
 

build.dependencies.gradle

@@ -22,5 +22,6 @@ dependencies {
     implementation 'org.apache.commons:commons-math3:3.6.1'
     implementation 'com.acmerobotics.roadrunner:core:0.5.6'
     implementation 'com.acmerobotics.dashboard:dashboard:0.4.8'
+    implementation 'org.ftclib.ftclib:core:2.1.1'
 }