diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index 02c21c3..28a3a3a 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java +++ b/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; @@ -40,7 +42,7 @@ public class Robot { HIGH } - + public enum Status { OKAY, MONITORING, @@ -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; } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java index aaf999f..6a8e32f 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java +++ b/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(); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java index f12d579..9654821 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java +++ b/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 diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/ArmDriverControl.java new file mode 100644 index 0000000..02481e2 --- /dev/null +++ b/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(); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DriverControlState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DriverControlState.java deleted file mode 100644 index 5ea2d2e..0000000 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DriverControlState.java +++ /dev/null @@ -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; - } - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java new file mode 100644 index 0000000..f5c6626 --- /dev/null +++ b/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; + } + } +}