From 4f519af193f72075c1c325b62af38db6dd88fdbe Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Thu, 19 Jan 2023 23:02:47 -0600 Subject: [PATCH] Added diagnostics engine for CHIRON, renamed liftDrive to simply arm, misc. tweaks and fixes. --- .../minibots/cyberarm/chiron/Robot.java | 86 +++++---- .../ConfigureAndTestHardwareEngine.java | 164 ++++++++++++++++++ .../cyberarm/chiron/engines/TeleOpEngine.java | 3 + .../chiron/states/DriverControlState.java | 84 +++++---- 4 files changed, 271 insertions(+), 66 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java 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 1033337..f7af352 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 @@ -18,7 +18,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Act import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; public class Robot { - public final DcMotorEx leftDrive, rightDrive, frontDrive, backDrive, liftDrive; + public final DcMotorEx leftDrive, rightDrive, frontDrive, backDrive, arm; public final ServoImplEx gripper, wrist; public final IMU imu; public final ColorSensor indicatorA, indicatorB; @@ -30,7 +30,7 @@ public class Robot { public Status status = Status.OKAY; - public enum LiftPosition { + public enum ArmPosition { COLLECT, GROUND, LOW, @@ -61,7 +61,8 @@ public class Robot { frontDrive = engine.hardwareMap.get(DcMotorEx.class, "front_drive"); // MOTOR PORT: ? backDrive = engine.hardwareMap.get(DcMotorEx.class, "back_drive"); // MOTOR PORT: ? - liftDrive = engine.hardwareMap.get(DcMotorEx.class, "lift_drive"); // MOTOR PORT: ? + // FIXME: Rename lift_drive to arm in hardware config + arm = engine.hardwareMap.get(DcMotorEx.class, "lift_drive"); // MOTOR PORT: ? gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper"); // SERVO PORT: ? wrist = engine.hardwareMap.get(ServoImplEx.class, "wrist"); // SERVO PORT: ? @@ -81,7 +82,7 @@ public class Robot { frontDrive.setDirection(hardwareConfig("front_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); backDrive.setDirection(hardwareConfig("back_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); - liftDrive.setDirection(hardwareConfig("lift_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); + arm.setDirection(hardwareConfig("arm_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); // RUNMODE leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -90,7 +91,8 @@ public class Robot { frontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - liftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); liftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + arm.setTargetPosition(0); + arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); // ZERO POWER BEHAVIOR leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -99,16 +101,19 @@ public class Robot { frontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - liftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // MOTOR POWER + arm.setPower(0.35); // SERVOS (POSITIONAL) // Gripper gripper.setDirection(hardwareConfig("gripper_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE); - gripper.setPosition(hardwareConfig("gripper_initial_position").value()); + gripper.setPosition(tuningConfig("gripper_initial_position").value()); // Wrist wrist.setDirection(hardwareConfig("wrist_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE); - wrist.setPosition(hardwareConfig("wrist_initial_position").value()); + wrist.setPosition(tuningConfig("wrist_initial_position").value()); // SENSORS // COLOR SENSORS @@ -125,6 +130,13 @@ public class Robot { } public void standardTelemetry() { + engine.telemetry.addLine(); + + // STATUS + engine.telemetry.addLine("DATA"); + engine.telemetry.addData(" Hardware Fault", hardwareFault); + engine.telemetry.addLine(); + // Motor Powers engine.telemetry.addLine("Motor Powers"); engine.telemetry.addData(" Left Drive", leftDrive.getPower()); @@ -135,21 +147,21 @@ public class Robot { engine.telemetry.addLine(); - engine.telemetry.addData(" Lift Drive", backDrive.getPower()); + engine.telemetry.addData(" Arm", arm.getPower()); engine.telemetry.addLine(); // Motor Positions engine.telemetry.addLine("Motor Positions"); - engine.telemetry.addData(" Left Drive", "%d (%.2f in)", leftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, leftDrive.getCurrentPosition())); - engine.telemetry.addData(" Right Drive", "%d (%.2f in)", rightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, rightDrive.getCurrentPosition())); + engine.telemetry.addData(" Left Drive", "%d (%2f in)", leftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, leftDrive.getCurrentPosition())); + engine.telemetry.addData(" Right Drive", "%d (%2f in)", rightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, rightDrive.getCurrentPosition())); - engine.telemetry.addData(" Front Drive", "%d (%.2f in)", frontDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontDrive.getCurrentPosition())); - engine.telemetry.addData(" Back Drive", "%d (%.2f in)", backDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backDrive.getCurrentPosition())); + engine.telemetry.addData(" Front Drive", "%d (%2f in)", frontDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontDrive.getCurrentPosition())); + engine.telemetry.addData(" Back Drive", "%d (%2f in)", backDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backDrive.getCurrentPosition())); engine.telemetry.addLine(); - engine.telemetry.addData(" Lift Drive", "%d (%.2d degrees)", leftDrive.getCurrentPosition(), ticksToAngle(liftDrive.getCurrentPosition())); + engine.telemetry.addData(" Arm", "%d (%2f degrees)", arm.getCurrentPosition(), ticksToAngle(arm.getCurrentPosition())); engine.telemetry.addLine(); @@ -163,40 +175,54 @@ public class Robot { engine.telemetry.addLine(); - engine.telemetry.addData(" Lift Drive", backDrive.getCurrent(CurrentUnit.AMPS)); + engine.telemetry.addData(" Arm", arm.getCurrent(CurrentUnit.AMPS)); engine.telemetry.addLine(); // Motor Directions engine.telemetry.addLine("Motor Directions"); - engine.telemetry.addData(" Left Drive", leftDrive.getDirection()); - engine.telemetry.addData(" Right Drive", rightDrive.getDirection()); + engine.telemetry.addData(" Left Drive", leftDrive.getDirection()); + engine.telemetry.addData(" Right Drive", rightDrive.getDirection()); - engine.telemetry.addData(" Front Drive", frontDrive.getDirection()); - engine.telemetry.addData(" Back Drive", backDrive.getDirection()); + engine.telemetry.addData(" Front Drive", frontDrive.getDirection()); + engine.telemetry.addData(" Back Drive", backDrive.getDirection()); engine.telemetry.addLine(); - engine.telemetry.addData(" Lift Drive", backDrive.getDirection()); + engine.telemetry.addData(" Arm", arm.getDirection()); + + engine.telemetry.addLine(); + + // Motor Target Positions + engine.telemetry.addLine("Motor Target Positions"); + engine.telemetry.addData(" Left Drive", leftDrive.getTargetPosition()); + engine.telemetry.addData(" Right Drive", rightDrive.getTargetPosition()); + + engine.telemetry.addData(" Front Drive", frontDrive.getTargetPosition()); + engine.telemetry.addData(" Back Drive", backDrive.getTargetPosition()); + + engine.telemetry.addLine(); + + engine.telemetry.addData(" Arm", arm.getTargetPosition()); engine.telemetry.addLine(); // Servos engine.telemetry.addLine("Servos"); - engine.telemetry.addData(" Gripper Direction", gripper.getDirection()); - engine.telemetry.addData(" Gripper Position", gripper.getPosition()); - engine.telemetry.addData(" Gripper Enabled", gripper.isPwmEnabled()); + engine.telemetry.addData(" Gripper Direction", gripper.getDirection()); + engine.telemetry.addData(" Gripper Position", gripper.getPosition()); + engine.telemetry.addData(" Gripper Enabled", gripper.isPwmEnabled()); engine.telemetry.addLine(); - engine.telemetry.addData(" Wrist Direction", wrist.getDirection()); - engine.telemetry.addData(" Wrist Position", wrist.getPosition()); - engine.telemetry.addData(" Wrist Enabled", wrist.isPwmEnabled()); + engine.telemetry.addData(" Wrist Direction", wrist.getDirection()); + engine.telemetry.addData(" Wrist Position", wrist.getPosition()); + engine.telemetry.addData(" Wrist Enabled", wrist.isPwmEnabled()); engine.telemetry.addLine(); // Sensors / IMU engine.telemetry.addLine("IMU"); - engine.telemetry.addData(" Facing", facing()); - engine.telemetry.addData(" Turn Rate", turnRate()); + engine.telemetry.addData(" Facing", facing()); + engine.telemetry.addData(" Turn Rate", turnRate()); } public TimeCraftersConfiguration getConfiguration() { @@ -220,12 +246,12 @@ public class Robot { return 0; } - // For: Lift Arm + // For: Arm public int angleToTicks(double angle) { return 0; } - // For: Lift Arm + // For: Arm public double ticksToAngle(int ticks) { return 0; } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java new file mode 100644 index 0000000..0671045 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java @@ -0,0 +1,164 @@ +package org.timecrafters.minibots.cyberarm.chiron.engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.cyberarm.engine.V2.CyberarmState; +import org.cyberarm.engine.V2.GamepadChecker; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; +import org.timecrafters.minibots.cyberarm.chiron.Robot; + +@TeleOp(name = "CHIRON | Diagnostics", group = "CHIRON") +public class ConfigureAndTestHardwareEngine extends CyberarmEngine { + private enum Stage { + ACKNOWLEDGE, + ENCODER, + DIRECTION, + COMPLETE + } + + private Robot robot; + + @Override + public void setup() { + robot = new Robot(this, new TimeCraftersConfiguration("CHIRON")); + + addState(new MotorSetupState("Left Drive", robot.leftDrive, "left_drive_direction_forward", robot)); + addState(new MotorSetupState("Right Drive", robot.rightDrive, "right_drive_direction_forward", robot)); + + addState(new MotorSetupState("Front Drive", robot.frontDrive, "front_drive_direction_forward", robot)); + addState(new MotorSetupState("Back Drive", robot.backDrive, "back_drive_direction_forward", robot)); + + addState(new MotorSetupState("Lift Drive", robot.arm, "lift_drive_direction_forward", robot)); + } + + @Override + public void loop() { + super.loop(); + + robot.standardTelemetry(); + } + + private class MotorSetupState extends CyberarmState { + private final DcMotorEx motor; + private final String name, direction_key; + + private Stage stage = Stage.ACKNOWLEDGE; + + private final GamepadChecker controller; + private String message = "???"; + + public MotorSetupState(String name, DcMotorEx motor, String direction_key, Robot robot) { + this.name = name; + this.motor = motor; + this.direction_key = direction_key; + + controller = new GamepadChecker(engine, engine.gamepad1); + + this.motor.setPower(0.0); + this.motor.setDirection(robot.hardwareConfig(direction_key).value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); + } + + @Override + public void exec() { + switch (stage) { + case ACKNOWLEDGE: + handleAcknowledgement(); + break; + case ENCODER: + handleEncoder(); + break; + case DIRECTION: + handleDirection(); + break; + default: + setHasFinished(true); + } + + controller.update(); + } + + @Override + public void telemetry() { + engine.telemetry.addLine(); + engine.telemetry.addLine("Configuring " + name); + engine.telemetry.addLine(message); + engine.telemetry.addLine(); + engine.telemetry.addData("DIRECTION KEY", direction_key); + engine.telemetry.addData("STAGE", stage); + } + + private void handleAcknowledgement() { + message = "Press 'A' if it is safe to drive: " + name; + } + + private void handleEncoder() { + if (motor.getPower() == 0) { + message = "Press 'A' if motor has an encoder.\nPress 'X' if there is no encoder."; + } else { + message = "Press 'A' if encoder position is changing."; + } + } + + private void handleDirection() { + message = "Press 'A' to complete. Press 'X' to toggle direction."; + + motor.setPower(0.25); + } + + // INPUT + private void handleAcknowledgementInput(String button) { + if (button.equals("a")) { + stage = Stage.ENCODER; + } + } + + private void handleEncoderInput(String button) { + if (button.equals("a")) { + if (motor.getPower() == 0) { + motor.setPower(0.25); + } else { + motor.setPower(0); + stage = Stage.DIRECTION; + } + } + + if (button.equals("x")) { + stage = Stage.DIRECTION; + } + } + + private void handleDirectionInput(String button) { + if (button.equals("a")) { + motor.setPower(0.0); + stage = Stage.COMPLETE; + } + + if (button.equals("x")) { + if (motor.getDirection() == DcMotorSimple.Direction.FORWARD) { + motor.setDirection(DcMotorSimple.Direction.REVERSE); + } else { + motor.setDirection(DcMotorSimple.Direction.FORWARD); + } + } + } + + @Override + public void buttonDown(Gamepad gamepad, String button) { + switch (stage) { + case ACKNOWLEDGE: + handleAcknowledgementInput(button); + break; + case ENCODER: + handleEncoderInput(button); + break; + case DIRECTION: + handleDirectionInput(button); + break; + } + } + } +} 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 dcf9c01..38b970b 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 @@ -1,10 +1,13 @@ package org.timecrafters.minibots.cyberarm.chiron.engines; +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.DriverControlState; +@TeleOp(name = "CHIRON | TeleOp", group = "CHIRON") public class TeleOpEngine extends CyberarmEngine { private Robot robot; @Override diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java index ecc9926..ac4c8fa 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java @@ -10,7 +10,7 @@ public class DriverControlState extends CyberarmState { private final Robot robot; private final GamepadChecker controller; private final double releaseConfirmationDelay; - private double lastLiftManualControlTime = 0, lastWristManualControlTime = 0, lastLEDStatusAnimationTime = 0; + private double lastArmManualControlTime = 0, lastWristManualControlTime = 0, lastLEDStatusAnimationTime = 0; private boolean LEDStatusToggle = false; public DriverControlState(Robot robot) { @@ -29,7 +29,7 @@ public class DriverControlState extends CyberarmState { robot.status = Robot.Status.OKAY; move(forwardAngle, forwardSpeed, rightSpeed); - liftManualControl(); + armManualControl(); wristManualControl(); automatics(); @@ -37,6 +37,14 @@ public class DriverControlState extends CyberarmState { 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); + } + // FIXME: replace .setPower with .setVelocity private void move(double forwardAngle, double forwardSpeed, double rightSpeed) { if (robot.automaticAntiTipActive || robot.hardwareFault) { @@ -55,7 +63,7 @@ public class DriverControlState extends CyberarmState { robot.rightDrive.setPower(-rightSpeed); robot.frontDrive.setPower(rightSpeed); - robot.backDrive.setPower(-rightSpeed); + robot.backDrive.setPower(rightSpeed); } else if (rightSpeed != 0 && forwardSpeed != 0) { // ANGLE DRIVE // TODO @@ -73,29 +81,29 @@ public class DriverControlState extends CyberarmState { robot.backDrive.setPower(0); } - private void liftManualControl() { + private void armManualControl() { if (robot.hardwareFault) { return; } robot.status = Robot.Status.WARNING; - double stepInterval = robot.tuningConfig("lift_manual_step_interval").value(); - int stepSize = robot.tuningConfig("lift_manual_step_size").value(); + 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) { // Lift DOWN - // robot.liftDrive.setVelocity(5, AngleUnit.DEGREES); - robot.liftDrive.setTargetPosition(robot.leftDrive.getTargetPosition() - stepSize); + 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) { // Lift UP - robot.liftDrive.setTargetPosition(robot.leftDrive.getTargetPosition() + 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 lift arm at the current position + // FIXME: Detect when the triggers have been released and park arm at the current position } private void wristManualControl() { @@ -106,8 +114,8 @@ public class DriverControlState extends CyberarmState { 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() - lastLiftManualControlTime >= stepInterval) { - lastLiftManualControlTime = runTime(); + 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); @@ -118,8 +126,8 @@ public class DriverControlState extends CyberarmState { } } - private void stopLift() { - robot.liftDrive.setPower(0); + private void stopArm() { + robot.arm.setPower(0); } private void automatics() { @@ -135,10 +143,10 @@ public class DriverControlState extends CyberarmState { return; } - if (robot.ticksToAngle(robot.liftDrive.getCurrentPosition()) >= 50) { - robot.wrist.setPosition(robot.hardwareConfig("wrist_deposit_position").value()); + if (robot.ticksToAngle(robot.arm.getCurrentPosition()) >= 50) { + robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value()); } else { - robot.wrist.setPosition(robot.hardwareConfig("wrist_initial_position").value()); + robot.wrist.setPosition(robot.tuningConfig("wrist_initial_position").value()); } } @@ -160,7 +168,7 @@ public class DriverControlState extends CyberarmState { robot.status = Robot.Status.DANGER; stopDrive(); - stopLift(); + stopArm(); } } @@ -198,7 +206,7 @@ public class DriverControlState extends CyberarmState { } } - private void liftPosition(Robot.LiftPosition position) { + private void armPosition(Robot.ArmPosition position) { if (robot.hardwareFault) { return; } @@ -207,36 +215,36 @@ public class DriverControlState extends CyberarmState { switch (position) { case COLLECT: - robot.liftDrive.setTargetPosition(robot.angleToTicks(120)); + robot.arm.setTargetPosition(robot.angleToTicks(120)); break; case GROUND: - robot.liftDrive.setTargetPosition(robot.angleToTicks(100)); + robot.arm.setTargetPosition(robot.angleToTicks(100)); break; case LOW: - robot.liftDrive.setTargetPosition(robot.angleToTicks(80)); + robot.arm.setTargetPosition(robot.angleToTicks(80)); break; case MEDIUM: - robot.liftDrive.setTargetPosition(robot.angleToTicks(35)); + robot.arm.setTargetPosition(robot.angleToTicks(35)); break; case HIGH: - robot.liftDrive.setTargetPosition(robot.angleToTicks(15)); + robot.arm.setTargetPosition(robot.angleToTicks(15)); break; default: - throw new RuntimeException("Unexpected lift position!"); + throw new RuntimeException("Unexpected arm position!"); } } private void gripperOpen() { - robot.gripper.setPosition(robot.hardwareConfig("gripper_open_position").value()); + robot.gripper.setPosition(robot.tuningConfig("gripper_open_position").value()); } private void gripperClosed() { - robot.gripper.setPosition(robot.hardwareConfig("gripper_closed_position").value()); + robot.gripper.setPosition(robot.tuningConfig("gripper_closed_position").value()); } @Override @@ -256,22 +264,22 @@ public class DriverControlState extends CyberarmState { if (button.equals("dpad_down")) { robot.wristManuallyControlled = false; - robot.wrist.setPosition(robot.hardwareConfig("wrist_deposit_position").value()); + robot.wrist.setPosition(robot.tuningConfig("wrist_deposit_position").value()); } else if (button.equals("dpad_up")) { robot.wristManuallyControlled = false; - robot.wrist.setPosition(robot.hardwareConfig("wrist_initial_position").value()); + robot.wrist.setPosition(robot.tuningConfig("wrist_initial_position").value()); } - // Automatic Lift Control + // Automatic Arm Control if (button.equals("a")) { - liftPosition(Robot.LiftPosition.COLLECT); + armPosition(Robot.ArmPosition.COLLECT); } else if (button.equals("x")) { - liftPosition(Robot.LiftPosition.GROUND); + armPosition(Robot.ArmPosition.GROUND); } else if (button.equals("b")) { - liftPosition(Robot.LiftPosition.LOW); + armPosition(Robot.ArmPosition.LOW); } else if (button.equals("y")) { - liftPosition(Robot.LiftPosition.MEDIUM); + armPosition(Robot.ArmPosition.MEDIUM); } } @@ -280,5 +288,9 @@ public class DriverControlState extends CyberarmState { if (gamepad != engine.gamepad1) { return; } + + if (button.equals("guide")) { + robot.hardwareFault = !robot.hardwareFault; + } } }