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 f7af352..3e5f1c1 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, arm; + public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm; public final ServoImplEx gripper, wrist; public final IMU imu; public final ColorSensor indicatorA, indicatorB; @@ -54,12 +54,13 @@ public class Robot { this.configuration = configuration; imuAngleOffset = hardwareConfig("imu_angle_offset").value(); + // FIXME: Rename motors in configuration // Define hardware - leftDrive = engine.hardwareMap.get(DcMotorEx.class, "left_drive"); // MOTOR PORT: ? - rightDrive = engine.hardwareMap.get(DcMotorEx.class, "right_drive"); // MOTOR PORT: ? + backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "left_drive"); // MOTOR PORT: ? + frontRightDrive = engine.hardwareMap.get(DcMotorEx.class, "right_drive"); // MOTOR PORT: ? - frontDrive = engine.hardwareMap.get(DcMotorEx.class, "front_drive"); // MOTOR PORT: ? - backDrive = engine.hardwareMap.get(DcMotorEx.class, "back_drive"); // MOTOR PORT: ? + frontLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "front_drive"); // MOTOR PORT: ? + backRightDrive = engine.hardwareMap.get(DcMotorEx.class, "back_drive"); // MOTOR PORT: ? // FIXME: Rename lift_drive to arm in hardware config arm = engine.hardwareMap.get(DcMotorEx.class, "lift_drive"); // MOTOR PORT: ? @@ -76,30 +77,30 @@ public class Robot { // Configure hardware // MOTORS // DIRECTION - leftDrive.setDirection(hardwareConfig("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); + frontLeftDrive.setDirection(hardwareConfig("front_left_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); - backDrive.setDirection(hardwareConfig("back_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); + 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 - leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeftDrive.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); - backDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftDrive.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.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); // ZERO POWER BEHAVIOR - leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - frontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - backDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -124,7 +125,7 @@ public class Robot { IMU.Parameters parameters = new IMU.Parameters( new RevHubOrientationOnRobot( RevHubOrientationOnRobot.LogoFacingDirection.UP, - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)); + RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD)); imu.initialize(parameters); } @@ -139,53 +140,53 @@ public class Robot { // Motor Powers engine.telemetry.addLine("Motor Powers"); - engine.telemetry.addData(" Left Drive", leftDrive.getPower()); - engine.telemetry.addData(" Right Drive", rightDrive.getPower()); + engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getPower()); + engine.telemetry.addData(" Front Right Drive", frontRightDrive.getPower()); - engine.telemetry.addData(" Front Drive", frontDrive.getPower()); - engine.telemetry.addData(" Back Drive", backDrive.getPower()); + engine.telemetry.addData(" Back Left Drive", backLeftDrive.getPower()); + engine.telemetry.addData(" Back Right Drive", backRightDrive.getPower()); engine.telemetry.addLine(); - engine.telemetry.addData(" Arm", arm.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(" Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.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 Drive", "%d (%2f in)", backDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backDrive.getCurrentPosition())); + engine.telemetry.addData(" Back Left Drive", "%d (%8.2f in)", backLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getCurrentPosition())); + engine.telemetry.addData(" Back Right Drive", "%d (%8.2f in)", backRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getCurrentPosition())); 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(); // Motor Currents engine.telemetry.addLine("Motor Currents (AMPS)"); - engine.telemetry.addData(" Left Drive", leftDrive.getCurrent(CurrentUnit.AMPS)); - engine.telemetry.addData(" Right Drive", rightDrive.getCurrent(CurrentUnit.AMPS)); + engine.telemetry.addData(" Front Left Drive", frontLeftDrive.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 Drive", backDrive.getCurrent(CurrentUnit.AMPS)); + engine.telemetry.addData(" Back Left Drive", backLeftDrive.getCurrent(CurrentUnit.AMPS)); + engine.telemetry.addData(" Back Right Drive", backRightDrive.getCurrent(CurrentUnit.AMPS)); engine.telemetry.addLine(); - engine.telemetry.addData(" Arm", arm.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(" Front Left Drive", frontLeftDrive.getDirection()); + engine.telemetry.addData(" Front Right Drive", frontRightDrive.getDirection()); - engine.telemetry.addData(" Front Drive", frontDrive.getDirection()); - engine.telemetry.addData(" Back Drive", backDrive.getDirection()); + engine.telemetry.addData(" Back Left Drive", backLeftDrive.getDirection()); + engine.telemetry.addData(" Back Right Drive", backRightDrive.getDirection()); engine.telemetry.addLine(); @@ -195,11 +196,11 @@ public class Robot { // 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 Left Drive", frontLeftDrive.getTargetPosition()); + engine.telemetry.addData(" Front Right Drive", frontRightDrive.getTargetPosition()); - engine.telemetry.addData(" Front Drive", frontDrive.getTargetPosition()); - engine.telemetry.addData(" Back Drive", backDrive.getTargetPosition()); + engine.telemetry.addData(" Back Left Drive", backLeftDrive.getTargetPosition()); + engine.telemetry.addData(" Back Right Drive", backRightDrive.getTargetPosition()); engine.telemetry.addLine(); @@ -248,12 +249,21 @@ public class Robot { // For: Arm 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 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) { @@ -286,6 +296,10 @@ public class Robot { return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); } + public double heading() { + return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + } + public double turnRate() { return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED } 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 index 0671045..c6dec44 100644 --- 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 @@ -26,11 +26,11 @@ public class ConfigureAndTestHardwareEngine extends CyberarmEngine { 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("Left Drive", robot.backLeftDrive, "left_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("Back Drive", robot.backDrive, "back_drive_direction_forward", robot)); + addState(new MotorSetupState("Front Drive", robot.frontLeftDrive, "front_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)); } 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 ac4c8fa..859b3d6 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 @@ -12,6 +12,7 @@ public class DriverControlState extends CyberarmState { private final double releaseConfirmationDelay; private double lastArmManualControlTime = 0, lastWristManualControlTime = 0, lastLEDStatusAnimationTime = 0; private boolean LEDStatusToggle = false; + private boolean fieldCentricControl = true; public DriverControlState(Robot robot) { this.robot = robot; @@ -22,13 +23,9 @@ public class DriverControlState extends CyberarmState { @Override 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; - move(forwardAngle, forwardSpeed, rightSpeed); + move(); armManualControl(); wristManualControl(); @@ -43,42 +40,53 @@ public class DriverControlState extends CyberarmState { 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); } // 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) { return; } - if (rightSpeed == 0 && forwardSpeed != 0) { // DRIVE STRAIGHT - robot.leftDrive.setPower(forwardSpeed); - robot.rightDrive.setPower(forwardSpeed); + double y = -engine.gamepad1.left_stick_y; + double x = engine.gamepad1.left_stick_x * 1.1; + 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); - robot.backDrive.setPower(0); + double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0; - } else if (rightSpeed != 0 && forwardSpeed == 0) { // TURN IN PLACE - robot.leftDrive.setPower(rightSpeed); - robot.rightDrive.setPower(-rightSpeed); + 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); - robot.frontDrive.setPower(rightSpeed); - robot.backDrive.setPower(rightSpeed); + frontLeftPower = (rotY + rotX + rx) / denominator; + 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 { - 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() { - robot.leftDrive.setPower(0); - robot.rightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + robot.frontRightDrive.setPower(0); - robot.frontDrive.setPower(0); - robot.backDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); } private void armManualControl() { @@ -215,23 +223,23 @@ public class DriverControlState extends CyberarmState { switch (position) { case COLLECT: - robot.arm.setTargetPosition(robot.angleToTicks(120)); + robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_collect").value())); break; case GROUND: - robot.arm.setTargetPosition(robot.angleToTicks(100)); + robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_ground").value())); break; case LOW: - robot.arm.setTargetPosition(robot.angleToTicks(80)); + robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_low").value())); break; case MEDIUM: - robot.arm.setTargetPosition(robot.angleToTicks(35)); + robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_medium").value())); break; case HIGH: - robot.arm.setTargetPosition(robot.angleToTicks(15)); + robot.arm.setTargetPosition(robot.angleToTicks(robot.tuningConfig("arm_position_angle_high").value())); break; default: @@ -291,6 +299,8 @@ public class DriverControlState extends CyberarmState { if (button.equals("guide")) { robot.hardwareFault = !robot.hardwareFault; + } else if (button.equals("start")) { + fieldCentricControl = !fieldCentricControl; } } }