TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-16 05:22:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

Refactored motor names to front/back left/right, implemented mecanum drive, experimental arm to angle

Browse Source
This commit is contained in:
Cyberarm
2023-01-20 16:43:35 -06:00
parent c49736d59c
commit 5d2c7b82fc
3 changed files with 101 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,7 +18,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Act
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
public class Robot { public class Robot {
public final DcMotorEx leftDrive, rightDrive, frontDrive, backDrive, arm; public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
public final ServoImplEx gripper, wrist; public final ServoImplEx gripper, wrist;
public final IMU imu; public final IMU imu;
public final ColorSensor indicatorA, indicatorB; public final ColorSensor indicatorA, indicatorB;
@@ -54,12 +54,13 @@ public class Robot {
this.configuration = configuration; this.configuration = configuration;
imuAngleOffset = hardwareConfig("imu_angle_offset").value(); imuAngleOffset = hardwareConfig("imu_angle_offset").value();
// FIXME: Rename motors in configuration
// Define hardware // Define hardware
leftDrive = engine.hardwareMap.get(DcMotorEx.class, "left_drive"); // MOTOR PORT: ? backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "left_drive"); // MOTOR PORT: ?
rightDrive = engine.hardwareMap.get(DcMotorEx.class, "right_drive"); // MOTOR PORT: ? frontRightDrive = engine.hardwareMap.get(DcMotorEx.class, "right_drive"); // MOTOR PORT: ?
frontDrive = engine.hardwareMap.get(DcMotorEx.class, "front_drive"); // MOTOR PORT: ? frontLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "front_drive"); // MOTOR PORT: ?
backDrive = engine.hardwareMap.get(DcMotorEx.class, "back_drive"); // MOTOR PORT: ? backRightDrive = engine.hardwareMap.get(DcMotorEx.class, "back_drive"); // MOTOR PORT: ?
// FIXME: Rename lift_drive to arm in hardware config // FIXME: Rename lift_drive to arm in hardware config
arm = engine.hardwareMap.get(DcMotorEx.class, "lift_drive"); // MOTOR PORT: ? arm = engine.hardwareMap.get(DcMotorEx.class, "lift_drive"); // MOTOR PORT: ?
@@ -76,30 +77,30 @@ public class Robot {
// Configure hardware // Configure hardware
// MOTORS // MOTORS
// DIRECTION // DIRECTION
leftDrive.setDirection(hardwareConfig("left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); frontLeftDrive.setDirection(hardwareConfig("front_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
rightDrive.setDirection(hardwareConfig("right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); frontRightDrive.setDirection(hardwareConfig("front_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
frontDrive.setDirection(hardwareConfig("front_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); backLeftDrive.setDirection(hardwareConfig("back_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
backDrive.setDirection(hardwareConfig("back_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); backRightDrive.setDirection(hardwareConfig("back_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
arm.setDirection(hardwareConfig("arm_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); arm.setDirection(hardwareConfig("arm_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
// RUNMODE // RUNMODE
leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
arm.setTargetPosition(0); arm.setTargetPosition(0);
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// ZERO POWER BEHAVIOR // ZERO POWER BEHAVIOR
leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -124,7 +125,7 @@ public class Robot {
IMU.Parameters parameters = new IMU.Parameters( IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot( new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)); RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
imu.initialize(parameters); imu.initialize(parameters);
} }
@@ -139,53 +140,53 @@ public class Robot {
// Motor Powers // Motor Powers
engine.telemetry.addLine("Motor Powers"); engine.telemetry.addLine("Motor Powers");
engine.telemetry.addData(" Left Drive", leftDrive.getPower()); engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getPower());
engine.telemetry.addData(" Right Drive", rightDrive.getPower()); engine.telemetry.addData(" Front Right Drive", frontRightDrive.getPower());
engine.telemetry.addData(" Front Drive", frontDrive.getPower()); engine.telemetry.addData(" Back Left Drive", backLeftDrive.getPower());
engine.telemetry.addData(" Back Drive", backDrive.getPower()); engine.telemetry.addData(" Back Right Drive", backRightDrive.getPower());
engine.telemetry.addLine(); engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getPower()); engine.telemetry.addData(" Arm", arm.getPower());
engine.telemetry.addLine(); engine.telemetry.addLine();
// Motor Positions // Motor Positions
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(" Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getCurrentPosition()));
engine.telemetry.addData(" Right Drive", "%d (%2f in)", rightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, rightDrive.getCurrentPosition())); engine.telemetry.addData(" Front Right Drive", "%d (%8.2f in)", frontRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getCurrentPosition()));
engine.telemetry.addData(" Front Drive", "%d (%2f in)", frontDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontDrive.getCurrentPosition())); engine.telemetry.addData(" Back Left Drive", "%d (%8.2f in)", backLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getCurrentPosition()));
engine.telemetry.addData(" Back Drive", "%d (%2f in)", backDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backDrive.getCurrentPosition())); engine.telemetry.addData(" Back Right Drive", "%d (%8.2f in)", backRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getCurrentPosition()));
engine.telemetry.addLine(); engine.telemetry.addLine();
engine.telemetry.addData(" Arm", "%d (%2f degrees)", arm.getCurrentPosition(), ticksToAngle(arm.getCurrentPosition())); engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getCurrentPosition(), ticksToAngle(arm.getCurrentPosition()));
engine.telemetry.addLine(); engine.telemetry.addLine();
// Motor Currents // Motor Currents
engine.telemetry.addLine("Motor Currents (AMPS)"); engine.telemetry.addLine("Motor Currents (AMPS)");
engine.telemetry.addData(" Left Drive", leftDrive.getCurrent(CurrentUnit.AMPS)); engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Right Drive", rightDrive.getCurrent(CurrentUnit.AMPS)); engine.telemetry.addData(" Front Right Drive", frontRightDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Front Drive", frontDrive.getCurrent(CurrentUnit.AMPS)); engine.telemetry.addData(" Back Left Drive", backLeftDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Back Drive", backDrive.getCurrent(CurrentUnit.AMPS)); engine.telemetry.addData(" Back Right Drive", backRightDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addLine(); engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getCurrent(CurrentUnit.AMPS)); engine.telemetry.addData(" Arm", arm.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addLine(); engine.telemetry.addLine();
// Motor Directions // Motor Directions
engine.telemetry.addLine("Motor Directions"); engine.telemetry.addLine("Motor Directions");
engine.telemetry.addData(" Left Drive", leftDrive.getDirection()); engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getDirection());
engine.telemetry.addData(" Right Drive", rightDrive.getDirection()); engine.telemetry.addData(" Front Right Drive", frontRightDrive.getDirection());
engine.telemetry.addData(" Front Drive", frontDrive.getDirection()); engine.telemetry.addData(" Back Left Drive", backLeftDrive.getDirection());
engine.telemetry.addData(" Back Drive", backDrive.getDirection()); engine.telemetry.addData(" Back Right Drive", backRightDrive.getDirection());
engine.telemetry.addLine(); engine.telemetry.addLine();
@@ -195,11 +196,11 @@ public class Robot {
// Motor Target Positions // Motor Target Positions
engine.telemetry.addLine("Motor Target Positions"); engine.telemetry.addLine("Motor Target Positions");
engine.telemetry.addData(" Left Drive", leftDrive.getTargetPosition()); engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getTargetPosition());
engine.telemetry.addData(" Right Drive", rightDrive.getTargetPosition()); engine.telemetry.addData(" Front Right Drive", frontRightDrive.getTargetPosition());
engine.telemetry.addData(" Front Drive", frontDrive.getTargetPosition()); engine.telemetry.addData(" Back Left Drive", backLeftDrive.getTargetPosition());
engine.telemetry.addData(" Back Drive", backDrive.getTargetPosition()); engine.telemetry.addData(" Back Right Drive", backRightDrive.getTargetPosition());
engine.telemetry.addLine(); engine.telemetry.addLine();
@@ -248,12 +249,21 @@ public class Robot {
// For: Arm // For: Arm
public int angleToTicks(double angle) { public int angleToTicks(double angle) {
return 0; int ticksPerRevolution = tuningConfig("arm_ticks_per_revolution").value();
double d = ticksPerRevolution / 360.0;
// Casting to float so that the int version of Math.round is used.
return Math.round((float)d * (float)angle);
} }
// For: Arm // For: Arm
public double ticksToAngle(int ticks) { public double ticksToAngle(int ticks) {
return 0; int ticksPerRevolution = tuningConfig("arm_ticks_per_revolution").value();
double oneDegree = 360.0 / ticksPerRevolution;
return oneDegree / ticks;
} }
public Variable hardwareConfig(String variableName) { public Variable hardwareConfig(String variableName) {
@@ -286,6 +296,10 @@ public class Robot {
return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
} }
public double heading() {
return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
}
public double turnRate() { public double turnRate() {
return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
} }

8
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java
View File

@@ -26,11 +26,11 @@ public class ConfigureAndTestHardwareEngine extends CyberarmEngine {
public void setup() { public void setup() {
robot = new Robot(this, new TimeCraftersConfiguration("CHIRON")); robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"));
addState(new MotorSetupState("Left Drive", robot.leftDrive, "left_drive_direction_forward", robot)); addState(new MotorSetupState("Left Drive", robot.backLeftDrive, "left_drive_direction_forward", robot));
addState(new MotorSetupState("Right Drive", robot.rightDrive, "right_drive_direction_forward", robot)); addState(new MotorSetupState("Right Drive", robot.frontRightDrive, "right_drive_direction_forward", robot));
addState(new MotorSetupState("Front Drive", robot.frontDrive, "front_drive_direction_forward", robot)); addState(new MotorSetupState("Front Drive", robot.frontLeftDrive, "front_drive_direction_forward", robot));
addState(new MotorSetupState("Back Drive", robot.backDrive, "back_drive_direction_forward", robot)); addState(new MotorSetupState("Back Drive", robot.backRightDrive, "back_drive_direction_forward", robot));
addState(new MotorSetupState("Lift Drive", robot.arm, "lift_drive_direction_forward", robot)); addState(new MotorSetupState("Lift Drive", robot.arm, "lift_drive_direction_forward", robot));
} }

68
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java
View File

@@ -12,6 +12,7 @@ public class DriverControlState extends CyberarmState {
private final double releaseConfirmationDelay; private final double releaseConfirmationDelay;
private double lastArmManualControlTime = 0, lastWristManualControlTime = 0, lastLEDStatusAnimationTime = 0; private double lastArmManualControlTime = 0, lastWristManualControlTime = 0, lastLEDStatusAnimationTime = 0;
private boolean LEDStatusToggle = false; private boolean LEDStatusToggle = false;
private boolean fieldCentricControl = true;
public DriverControlState(Robot robot) { public DriverControlState(Robot robot) {
this.robot = robot; this.robot = robot;
@@ -22,13 +23,9 @@ public class DriverControlState extends CyberarmState {
@Override @Override
public void exec() { public void exec() {
double forwardSpeed = engine.gamepad1.left_stick_y * -1;
double rightSpeed = engine.gamepad1.right_stick_x;
double forwardAngle = robot.facing();
robot.status = Robot.Status.OKAY; robot.status = Robot.Status.OKAY;
move(forwardAngle, forwardSpeed, rightSpeed); move();
armManualControl(); armManualControl();
wristManualControl(); wristManualControl();
@@ -43,42 +40,53 @@ public class DriverControlState extends CyberarmState {
engine.telemetry.addData("Arm Interval", lastArmManualControlTime); engine.telemetry.addData("Arm Interval", lastArmManualControlTime);
engine.telemetry.addData("Wrist Interval", lastWristManualControlTime); engine.telemetry.addData("Wrist Interval", lastWristManualControlTime);
engine.telemetry.addData("LED Status Interval", lastLEDStatusAnimationTime); engine.telemetry.addData("LED Status Interval", lastLEDStatusAnimationTime);
engine.telemetry.addData("Field Centric Control", fieldCentricControl);
} }
// FIXME: replace .setPower with .setVelocity // FIXME: replace .setPower with .setVelocity
private void move(double forwardAngle, double forwardSpeed, double rightSpeed) { // REF: https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html
private void move() {
if (robot.automaticAntiTipActive || robot.hardwareFault) { if (robot.automaticAntiTipActive || robot.hardwareFault) {
return; return;
} }
if (rightSpeed == 0 && forwardSpeed != 0) { // DRIVE STRAIGHT double y = -engine.gamepad1.left_stick_y;
robot.leftDrive.setPower(forwardSpeed); double x = engine.gamepad1.left_stick_x * 1.1;
robot.rightDrive.setPower(forwardSpeed); double rx = engine.gamepad1.right_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
robot.frontDrive.setPower(0); double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
robot.backDrive.setPower(0);
} else if (rightSpeed != 0 && forwardSpeed == 0) { // TURN IN PLACE if (fieldCentricControl) {
robot.leftDrive.setPower(rightSpeed); double heading = -robot.heading();
robot.rightDrive.setPower(-rightSpeed); double rotX = x * Math.cos(heading) - y * Math.sin(heading);
double rotY = x * Math.sin(heading) + y * Math.cos(heading);
robot.frontDrive.setPower(rightSpeed); frontLeftPower = (rotY + rotX + rx) / denominator;
robot.backDrive.setPower(rightSpeed); backLeftPower = (rotY - rotX + rx) / denominator;
frontRightPower = (rotY - rotX - rx) / denominator;
backRightPower = (rotY + rotX - rx) / denominator;
} else if (rightSpeed != 0 && forwardSpeed != 0) { // ANGLE DRIVE
// TODO
stopDrive();
} else { } else {
stopDrive(); 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() { private void stopDrive() {
robot.leftDrive.setPower(0); robot.backLeftDrive.setPower(0);
robot.rightDrive.setPower(0); robot.frontRightDrive.setPower(0);
robot.frontDrive.setPower(0); robot.frontLeftDrive.setPower(0);
robot.backDrive.setPower(0); robot.backRightDrive.setPower(0);
} }
private void armManualControl() { private void armManualControl() {
@@ -215,23 +223,23 @@ public class DriverControlState extends CyberarmState {
switch (position) { switch (position) {
case COLLECT: case COLLECT:
robot.arm.setTargetPosition(robot.angleToTicks(120)); robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_collect").value()));
break; break;
case GROUND: case GROUND:
robot.arm.setTargetPosition(robot.angleToTicks(100)); robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_ground").value()));
break; break;
case LOW: case LOW:
robot.arm.setTargetPosition(robot.angleToTicks(80)); robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_low").value()));
break; break;
case MEDIUM: case MEDIUM:
robot.arm.setTargetPosition(robot.angleToTicks(35)); robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_medium").value()));
break; break;
case HIGH: case HIGH:
robot.arm.setTargetPosition(robot.angleToTicks(15)); robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_high").value()));
break; break;
default: default:
@@ -291,6 +299,8 @@ public class DriverControlState extends CyberarmState {
if (button.equals("guide")) { if (button.equals("guide")) {
robot.hardwareFault = !robot.hardwareFault; robot.hardwareFault = !robot.hardwareFault;
} else if (button.equals("start")) {
fieldCentricControl = !fieldCentricControl;
} }
} }
} }