Refactored driver control into 2 seperate states, one for drivetrain and one for arm, wrist, and gripper control. misc. tweaks.

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

@@ -1,5 +1,7 @@
 package org.timecrafters.minibots.cyberarm.chiron;
 
+import static org.timecrafters.minibots.cyberarm.chiron.Robot.Status;
+
 import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.ColorSensor;
@@ -53,6 +55,9 @@ public class Robot {
     private final FieldLocalizer fieldLocalizer;
     private final double radius, diameter;
 
+    private boolean LEDStatusToggle = false;
+    private double lastLEDStatusAnimationTime = 0;
+
     public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration, FieldLocalizer fieldLocalizer) {
         this.engine = engine;
         this.configuration = configuration;
@@ -117,7 +122,7 @@ public class Robot {
         arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         //      MOTOR POWER
-        arm.setPower(0.35);
+        arm.setPower(tuningConfig("arm_power").value());
 
         //   SERVOS (POSITIONAL)
         //      Gripper
@@ -253,6 +258,48 @@ public class Robot {
         engine.telemetry.addLine();
     }
 
+    public void update() {
+        status = Status.OKAY;
+
+        // TODO: Handle status priority; That is, store reported statuses and select the "worst" one as status
+
+        automaticLEDStatus();
+    }
+
+    private void automaticLEDStatus() {
+        switch (status) {
+            case OKAY:
+                indicatorA.enableLed(false);
+                indicatorB.enableLed(false);
+                break;
+
+            case MONITORING:
+                indicatorA.enableLed(true);
+                indicatorB.enableLed(true);
+                break;
+
+            case WARNING:
+                if (System.currentTimeMillis() - lastLEDStatusAnimationTime >= 500){
+                    lastLEDStatusAnimationTime = System.currentTimeMillis();
+                    LEDStatusToggle = !LEDStatusToggle;
+
+                    indicatorA.enableLed(LEDStatusToggle);
+                    indicatorA.enableLed(!LEDStatusToggle);
+                }
+                break;
+
+            case DANGER:
+                if (System.currentTimeMillis() - lastLEDStatusAnimationTime >= 200){
+                    lastLEDStatusAnimationTime = System.currentTimeMillis();
+                    LEDStatusToggle = !LEDStatusToggle;
+
+                    indicatorA.enableLed(LEDStatusToggle);
+                    indicatorA.enableLed(LEDStatusToggle);
+                }
+                break;
+        }
+    }
+
     public double getRadius() { return radius; }
 
     public double getDiameter() { return diameter; }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java

@@ -37,4 +37,11 @@ public class AutonomousRedLeftSideEngine extends CyberarmEngine {
                 Robot.class,
                 "AutonomousRedLeftSide");
     }
+
+    @Override
+    public void loop() {
+        super.loop();
+
+        robot.standardTelemetry();
+    }
 }

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java

@@ -5,7 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 import org.timecrafters.minibots.cyberarm.chiron.Robot;
-import org.timecrafters.minibots.cyberarm.chiron.states.teleop.DriverControlState;
+import org.timecrafters.minibots.cyberarm.chiron.states.teleop.DrivetrainDriverControl;
 import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
 
 @TeleOp(name = "CHIRON | TeleOp", group = "CHIRON")
@@ -15,7 +15,14 @@ public class TeleOpEngine extends CyberarmEngine {
     public void setup() {
         this.robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"), new FieldLocalizer());
 
-        addState(new DriverControlState(robot));
+        addTask(robot.getFieldLocalizer());
+
+        setupFromConfig(
+                robot.getConfiguration(),
+                "org.timecrafters.minibots.cyberarm.chiron.states.teleop",
+                robot,
+                Robot.class,
+                "TeleOpStates");
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java

@@ -0,0 +1,215 @@
+package org.timecrafters.minibots.cyberarm.chiron.states.teleop;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.minibots.cyberarm.chiron.Robot;
+
+public class ArmDriverControl extends CyberarmState {
+    private final Robot robot;
+    private final String groupName, actionName;
+
+    private Gamepad controller;
+
+    private double lastArmManualControlTime = 0, lastWristManualControlTime = 0;
+
+    private final double gripperOpenConfirmationDelay;
+    private double gripperReleaseTriggeredTime = 0;
+
+    public ArmDriverControl(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        this.controller = engine.gamepad1;
+
+        this.gripperOpenConfirmationDelay = robot.tuningConfig("gripper_open_confirmation_delay").value(); // ms
+    }
+
+    @Override
+    public void exec() {
+        armManualControl();
+        wristManualControl();
+
+        automatics();
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Arm Interval", lastArmManualControlTime);
+        engine.telemetry.addData("Wrist Interval", lastWristManualControlTime);
+    }
+
+    private void armManualControl() {
+        if (robot.hardwareFault) {
+            return;
+        }
+
+        robot.status = Robot.Status.WARNING;
+
+        double stepInterval = robot.tuningConfig("arm_manual_step_interval").value();
+        int stepSize = robot.tuningConfig("arm_manual_step_size").value();
+
+        if ((controller.left_trigger > 0 || controller.right_trigger > 0) && runTime() - lastWristManualControlTime >= stepInterval) {
+            lastWristManualControlTime = runTime();
+
+            if (controller.left_trigger > 0) { // Arm DOWN
+                // robot.arm.setVelocity(5, AngleUnit.DEGREES);
+                robot.arm.setTargetPosition(robot.arm.getCurrentPosition() - stepSize);
+
+            } else if (controller.right_trigger > 0) { // Arm UP
+                robot.arm.setTargetPosition(robot.arm.getCurrentPosition() + stepSize);
+            }
+        }
+
+        // FIXME: Detect when the triggers have been released and park arm at the current position
+    }
+
+    private void wristManualControl() {
+        if (robot.hardwareFault) {
+            return;
+        }
+
+        double stepInterval = robot.tuningConfig("wrist_manual_step_interval").value();
+        double stepSize = robot.tuningConfig("wrist_manual_step_size").value();
+
+        if ((controller.dpad_left || controller.dpad_right) && runTime() - lastArmManualControlTime >= stepInterval) {
+            lastArmManualControlTime = runTime();
+
+            if (controller.dpad_left) { // Wrist Left
+                robot.wrist.setPosition(robot.wrist.getPosition() - stepSize);
+
+            } else if (controller.dpad_right) { // Wrist Right
+                robot.wrist.setPosition(robot.wrist.getPosition() + stepSize);
+            }
+        }
+    }
+
+    private void stopArm() {
+        robot.arm.setPower(0);
+    }
+
+    private void automatics() {
+        automaticWrist();
+
+        automaticHardwareMonitor();
+    }
+
+    private void automaticWrist() {
+        if (robot.wristManuallyControlled) {
+            return;
+        }
+
+        double angle = robot.tuningConfig("wrist_auto_rotate_angle").value();
+
+        if (robot.ticksToAngle(robot.arm.getCurrentPosition()) >= angle) {
+            robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value());
+        } else {
+            robot.wrist.setPosition(robot.tuningConfig("wrist_collect_position").value());
+        }
+    }
+
+    private void automaticHardwareMonitor() {
+        if (robot.hardwareFault) {
+            robot.status = Robot.Status.DANGER;
+
+            stopArm();
+        } else {
+            robot.arm.setPower(robot.tuningConfig("arm_power").value());
+        }
+    }
+
+    private void armPosition(Robot.ArmPosition position) {
+        if (robot.hardwareFault) {
+            return;
+        }
+
+        robot.status = Robot.Status.WARNING;
+
+        switch (position) {
+            case COLLECT:
+                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_collect").value()));
+                break;
+
+            case GROUND:
+                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_ground").value()));
+                break;
+
+            case LOW:
+                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_low").value()));
+                break;
+
+            case MEDIUM:
+                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_medium").value()));
+                break;
+
+            case HIGH:
+                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_high").value()));
+                break;
+
+            default:
+                throw new RuntimeException("Unexpected arm position!");
+        }
+    }
+
+    private void gripperOpen() {
+        robot.gripper.setPosition(robot.tuningConfig("gripper_open_position").value());
+    }
+
+    private void gripperClosed() {
+        robot.gripper.setPosition(robot.tuningConfig("gripper_closed_position").value());
+    }
+
+    @Override
+    public void buttonDown(Gamepad gamepad, String button) {
+        // Swap controlling gamepad
+        if (gamepad == engine.gamepad2 && button.equals("guide")) {
+            controller = controller == engine.gamepad1 ? engine.gamepad2 : engine.gamepad1;
+        }
+
+        if (gamepad != controller) {
+            return;
+        }
+
+        // Gripper Control
+        if (button.equals("left_bumper")) {
+            gripperReleaseTriggeredTime = runTime();
+        } else if (button.equals("right_bumper")) {
+            gripperClosed();
+        }
+
+        // Wrist Control
+        if (button.equals("dpad_down")) {
+            robot.wristManuallyControlled = false;
+
+            robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value());
+        } else if (button.equals("dpad_up")) {
+            robot.wristManuallyControlled = false;
+
+            robot.wrist.setPosition(robot.tuningConfig("wrist_collect_position").value());
+        }
+
+        // Automatic Arm Control
+        if (button.equals("a")) {
+            armPosition(Robot.ArmPosition.COLLECT);
+        } else if (button.equals("x")) {
+            armPosition(Robot.ArmPosition.GROUND);
+        } else if (button.equals("b")) {
+            armPosition(Robot.ArmPosition.LOW);
+        } else if (button.equals("y")) {
+            armPosition(Robot.ArmPosition.MEDIUM);
+        }
+    }
+
+    @Override
+    public void buttonUp(Gamepad gamepad, String button) {
+        if (gamepad != controller) {
+            return;
+        }
+
+        // Gripper Control - Require confirmation before opening gripper
+        if (button.equals("left_bumper") && runTime() - gripperReleaseTriggeredTime >= gripperOpenConfirmationDelay) {
+            gripperOpen();
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DriverControlState.java

@@ -1,320 +0,0 @@
-package org.timecrafters.minibots.cyberarm.chiron.states.teleop;
-
-import android.util.Log;
-
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.cyberarm.engine.V2.CyberarmState;
-import org.cyberarm.engine.V2.GamepadChecker;
-import org.timecrafters.minibots.cyberarm.chiron.Robot;
-
-public class DriverControlState extends CyberarmState {
-    private final Robot robot;
-    private final GamepadChecker controller;
-    private final double releaseConfirmationDelay;
-    private double lastArmManualControlTime = 0, lastWristManualControlTime = 0, lastLEDStatusAnimationTime = 0;
-    private boolean LEDStatusToggle = false;
-    private boolean fieldCentricControl = true;
-    private boolean invertRobotForward = false;
-    private boolean robotSlowMode = false;
-
-    public DriverControlState(Robot robot) {
-        this.robot = robot;
-        this.controller = new GamepadChecker(engine, engine.gamepad1);
-
-        this.releaseConfirmationDelay = robot.tuningConfig("cone_release_confirmation_delay").value(); // ms
-    }
-
-    @Override
-    public void exec() {
-        robot.status = Robot.Status.OKAY;
-
-        move();
-        armManualControl();
-        wristManualControl();
-
-        automatics();
-
-        controller.update();
-    }
-
-    @Override
-    public void telemetry() {
-        engine.telemetry.addData("Run Time", runTime());
-        engine.telemetry.addData("Arm Interval", lastArmManualControlTime);
-        engine.telemetry.addData("Wrist Interval", lastWristManualControlTime);
-        engine.telemetry.addData("LED Status Interval", lastLEDStatusAnimationTime);
-        engine.telemetry.addData("Field Centric Control", fieldCentricControl);
-        engine.telemetry.addData("Invert Robot Forward", invertRobotForward);
-    }
-
-    // FIXME: replace .setPower with .setVelocity
-    // REF: https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html
-    private void move() {
-        if (robot.automaticAntiTipActive || robot.hardwareFault) {
-            return;
-        }
-
-        double maxSpeed = robot.tuningConfig("drivetrain_max_power").value();
-        double slowSpeed = robot.tuningConfig("drivetrain_slow_power").value();
-        double speedLimiter = robotSlowMode ? slowSpeed : maxSpeed;
-
-        double y = (invertRobotForward ? engine.gamepad1.left_stick_y : -engine.gamepad1.left_stick_y) * speedLimiter;
-        double x = ((invertRobotForward ? engine.gamepad1.left_stick_x : -engine.gamepad1.left_stick_x) * speedLimiter) * 1.1; // Counteract imperfect strafing
-        double rx = -engine.gamepad1.right_stick_x * speedLimiter;
-        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
-
-
-        double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
-
-        if (fieldCentricControl) {
-            double heading = robot.heading();
-            double rotX = x * Math.cos(heading) - y * Math.sin(heading);
-            double rotY = x * Math.sin(heading) + y * Math.cos(heading);
-
-            frontLeftPower = (rotY + rotX + rx) / denominator;
-            backLeftPower = (rotY - rotX - rx) / denominator;
-            frontRightPower = (rotY - rotX + rx) / denominator;
-            backRightPower = (rotY + rotX - rx) / denominator;
-
-        } else {
-            frontLeftPower = (y + x + rx) / denominator;
-            backLeftPower = (y - x - rx) / denominator;
-            frontRightPower = (y - x + rx) / denominator;
-            backRightPower = (y + x - rx) / denominator;
-        }
-
-        robot.frontLeftDrive.setPower(frontLeftPower);
-        robot.frontRightDrive.setPower(frontRightPower);
-
-        robot.backLeftDrive.setPower(backLeftPower);
-        robot.backRightDrive.setPower(backRightPower);
-    }
-
-    private void stopDrive() {
-        robot.backLeftDrive.setPower(0);
-        robot.frontRightDrive.setPower(0);
-
-        robot.frontLeftDrive.setPower(0);
-        robot.backRightDrive.setPower(0);
-    }
-
-    private void armManualControl() {
-        if (robot.hardwareFault) {
-            return;
-        }
-
-        robot.status = Robot.Status.WARNING;
-
-        double stepInterval = robot.tuningConfig("arm_manual_step_interval").value();
-        int stepSize = robot.tuningConfig("arm_manual_step_size").value();
-
-        if ((engine.gamepad1.left_trigger > 0 || engine.gamepad1.right_trigger > 0) && runTime() - lastWristManualControlTime >= stepInterval) {
-            lastWristManualControlTime = runTime();
-
-            if (engine.gamepad1.left_trigger > 0) { // Arm DOWN
-                // robot.arm.setVelocity(5, AngleUnit.DEGREES);
-                robot.arm.setTargetPosition(robot.arm.getCurrentPosition() - stepSize);
-
-            } else if (engine.gamepad1.right_trigger > 0) { // Arm UP
-                robot.arm.setTargetPosition(robot.arm.getCurrentPosition() + stepSize);
-            }
-        }
-
-        // FIXME: Detect when the triggers have been released and park arm at the current position
-    }
-
-    private void wristManualControl() {
-        if (robot.hardwareFault) {
-            return;
-        }
-
-        double stepInterval = robot.tuningConfig("wrist_manual_step_interval").value();
-        double stepSize = robot.tuningConfig("wrist_manual_step_size").value();
-
-        if ((engine.gamepad1.dpad_left || engine.gamepad1.dpad_right) && runTime() - lastArmManualControlTime >= stepInterval) {
-            lastArmManualControlTime = runTime();
-
-            if (engine.gamepad1.dpad_left) { // Wrist Left
-                robot.wrist.setPosition(robot.wrist.getPosition() - stepSize);
-
-            } else if (engine.gamepad1.dpad_right) { // Wrist Right
-                robot.wrist.setPosition(robot.wrist.getPosition() + stepSize);
-            }
-        }
-    }
-
-    private void stopArm() {
-        robot.arm.setPower(0);
-    }
-
-    private void automatics() {
-        automaticWrist();
-        automaticAntiTip(); // NO OP
-        automaticHardwareMonitor();
-
-        automaticLEDStatus();
-    }
-
-    private void automaticWrist() {
-        if (robot.wristManuallyControlled) {
-            return;
-        }
-
-        if (robot.ticksToAngle(robot.arm.getCurrentPosition()) >= 50) {
-            robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value());
-        } else {
-            robot.wrist.setPosition(robot.tuningConfig("wrist_initial_position").value());
-        }
-    }
-
-    // NO-OP
-    private void automaticAntiTip() {
-        // TODO: Take over drivetrain if robot starts to tip past a preconfigured point
-        //       return control if past point of no-return or tipping is no longer a concern
-
-        // TODO: Calculate motion inverse to the normal of current direction
-    }
-
-    private void automaticHardwareMonitor() {
-        // Check that encoders are updating as expect, etc.
-
-        // NOTE: Robot should prevent/halt all movement in the event of a fault
-        // robot.hardwareFault = true;
-
-        if (robot.hardwareFault) {
-            robot.status = Robot.Status.DANGER;
-
-            stopDrive();
-            stopArm();
-        }
-    }
-
-    private void automaticLEDStatus() {
-        switch (robot.status) {
-            case OKAY:
-                robot.indicatorA.enableLed(false);
-                robot.indicatorB.enableLed(false);
-                break;
-
-            case MONITORING:
-                robot.indicatorA.enableLed(true);
-                robot.indicatorB.enableLed(true);
-                break;
-
-            case WARNING:
-                if (runTime() - lastLEDStatusAnimationTime >= 500){
-                    lastLEDStatusAnimationTime = runTime();
-                    LEDStatusToggle = !LEDStatusToggle;
-
-                    robot.indicatorA.enableLed(LEDStatusToggle);
-                    robot.indicatorA.enableLed(!LEDStatusToggle);
-                }
-                break;
-
-            case DANGER:
-                if (runTime() - lastLEDStatusAnimationTime >= 200){
-                    lastLEDStatusAnimationTime = runTime();
-                    LEDStatusToggle = !LEDStatusToggle;
-
-                    robot.indicatorA.enableLed(LEDStatusToggle);
-                    robot.indicatorA.enableLed(LEDStatusToggle);
-                }
-                break;
-        }
-    }
-
-    private void armPosition(Robot.ArmPosition position) {
-        if (robot.hardwareFault) {
-            return;
-        }
-
-        robot.status = Robot.Status.WARNING;
-
-        switch (position) {
-            case COLLECT:
-                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_collect").value()));
-                break;
-
-            case GROUND:
-                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_ground").value()));
-                break;
-
-            case LOW:
-                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_low").value()));
-                break;
-
-            case MEDIUM:
-                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_medium").value()));
-                break;
-
-            case HIGH:
-                robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_high").value()));
-                break;
-
-            default:
-                throw new RuntimeException("Unexpected arm position!");
-        }
-    }
-
-    private void gripperOpen() {
-        robot.gripper.setPosition(robot.tuningConfig("gripper_open_position").value());
-    }
-
-    private void gripperClosed() {
-        robot.gripper.setPosition(robot.tuningConfig("gripper_closed_position").value());
-    }
-
-    @Override
-    public void buttonDown(Gamepad gamepad, String button) {
-        if (gamepad != engine.gamepad1) {
-            return;
-        }
-
-        // Gripper Control
-        if (button.equals("left_bumper")) {
-            gripperOpen();
-        } else if (button.equals("right_bumper")) {
-            gripperClosed();
-        }
-
-        // Wrist Control
-        if (button.equals("dpad_down")) {
-            robot.wristManuallyControlled = false;
-
-            robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value());
-        } else if (button.equals("dpad_up")) {
-            robot.wristManuallyControlled = false;
-
-            robot.wrist.setPosition(robot.tuningConfig("wrist_initial_position").value());
-        }
-
-        // Automatic Arm Control
-        if (button.equals("a")) {
-            armPosition(Robot.ArmPosition.COLLECT);
-        } else if (button.equals("x")) {
-            armPosition(Robot.ArmPosition.GROUND);
-        } else if (button.equals("b")) {
-            armPosition(Robot.ArmPosition.LOW);
-        } else if (button.equals("y")) {
-            armPosition(Robot.ArmPosition.MEDIUM);
-        }
-
-        if (button.equals("pause")) {
-            invertRobotForward = !invertRobotForward;
-        }
-        if (button.equals("guide")) {
-            robot.hardwareFault = !robot.hardwareFault;
-        }
-        if (button.equals("start")) {
-            fieldCentricControl = !fieldCentricControl;
-        }
-    }
-
-    @Override
-    public void buttonUp(Gamepad gamepad, String button) {
-        if (gamepad != engine.gamepad1) {
-            return;
-        }
-    }
-}

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java

@@ -0,0 +1,151 @@
+package org.timecrafters.minibots.cyberarm.chiron.states.teleop;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.minibots.cyberarm.chiron.Robot;
+
+public class DrivetrainDriverControl extends CyberarmState {
+    private final Robot robot;
+    private final String groupName, actionName;
+
+    private Gamepad controller;
+
+    private boolean fieldCentricControl = true;
+    private boolean invertRobotForward = false;
+    private boolean robotSlowMode = false;
+
+    public DrivetrainDriverControl(Robot robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.groupName = groupName;
+        this.actionName = actionName;
+
+        this.controller = engine.gamepad1;
+    }
+
+    @Override
+    public void exec() {
+        robot.update();
+
+        move();
+
+        automatics();
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Run Time", runTime());
+//        engine.telemetry.addData("LED Status Interval", lastLEDStatusAnimationTime);
+        engine.telemetry.addData("Field Centric Control", fieldCentricControl);
+        engine.telemetry.addData("Invert Robot Forward", invertRobotForward);
+        engine.telemetry.addData("Robot Slow Mode", robotSlowMode);
+    }
+
+    // FIXME: replace .setPower with .setVelocity
+    // REF: https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html
+    private void move() {
+        if (robot.automaticAntiTipActive || robot.hardwareFault) {
+            return;
+        }
+
+        double maxSpeed = robot.tuningConfig("drivetrain_max_power").value();
+        double slowSpeed = robot.tuningConfig("drivetrain_slow_power").value();
+        double speedLimiter = robotSlowMode ? slowSpeed : maxSpeed;
+
+        double y = (invertRobotForward ? controller.left_stick_y : -controller.left_stick_y) * speedLimiter;
+        double x = ((invertRobotForward ? controller.left_stick_x : -controller.left_stick_x) * speedLimiter) * 1.1; // Counteract imperfect strafing
+        double rx = -controller.right_stick_x * speedLimiter;
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+
+
+        double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
+
+        if (fieldCentricControl) {
+            double heading = robot.heading();
+            double rotX = x * Math.cos(heading) - y * Math.sin(heading);
+            double rotY = x * Math.sin(heading) + y * Math.cos(heading);
+
+            frontLeftPower = (rotY + rotX + rx) / denominator;
+            backLeftPower = (rotY - rotX - rx) / denominator;
+            frontRightPower = (rotY - rotX + rx) / denominator;
+            backRightPower = (rotY + rotX - rx) / denominator;
+
+        } else {
+            frontLeftPower = (y + x + rx) / denominator;
+            backLeftPower = (y - x - rx) / denominator;
+            frontRightPower = (y - x + rx) / denominator;
+            backRightPower = (y + x - rx) / denominator;
+        }
+
+        robot.frontLeftDrive.setPower(frontLeftPower);
+        robot.frontRightDrive.setPower(frontRightPower);
+
+        robot.backLeftDrive.setPower(backLeftPower);
+        robot.backRightDrive.setPower(backRightPower);
+    }
+
+    private void stopDrive() {
+        robot.backLeftDrive.setPower(0);
+        robot.frontRightDrive.setPower(0);
+
+        robot.frontLeftDrive.setPower(0);
+        robot.backRightDrive.setPower(0);
+    }
+
+    private void automatics() {
+        automaticAntiTip(); // NO OP
+        automaticHardwareMonitor();
+    }
+
+    // NO-OP
+    private void automaticAntiTip() {
+        // TODO: Take over drivetrain if robot starts to tip past a preconfigured point
+        //       return control if past point of no-return or tipping is no longer a concern
+
+        // TODO: Calculate motion inverse to the normal of current direction
+    }
+
+    private void automaticHardwareMonitor() {
+        // Check that encoders are updating as expect, etc.
+
+        // NOTE: Robot should prevent/halt all movement in the event of a fault
+        // robot.hardwareFault = true;
+
+        if (robot.hardwareFault) {
+            robot.status = Robot.Status.DANGER;
+
+            stopDrive();
+        }
+    }
+
+    @Override
+    public void buttonDown(Gamepad gamepad, String button) {
+        if (gamepad != controller) {
+            return;
+        }
+
+        // DEBUG: Toggle hardware fault
+        if (button.equals("guide")) {
+            robot.hardwareFault = !robot.hardwareFault;
+        }
+
+        if (button.equals("pause")) {
+            invertRobotForward = !invertRobotForward;
+        }
+
+        if (button.equals("start")) {
+            fieldCentricControl = !fieldCentricControl;
+        }
+
+        if (button.equals("right_stick_button")) {
+            robotSlowMode = !robotSlowMode;
+        }
+    }
+
+    @Override
+    public void buttonUp(Gamepad gamepad, String button) {
+        if (gamepad != controller) {
+            return;
+        }
+    }
+}