mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-15 20:12:35 +00:00
Added diagnostics engine for CHIRON, renamed liftDrive to simply arm, misc. tweaks and fixes.
This commit is contained in:
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user