From 3286b1ec57b175534e3995b26e86737c1cedab46 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sun, 22 Jan 2023 17:35:30 -0600 Subject: [PATCH] Tweaks to CHIRON --- .../minibots/cyberarm/chiron/Robot.java | 26 ++++++++++++--- .../engines/AutonomousRedLeftSideEngine.java | 2 ++ .../chiron/states/autonomous/Move.java | 32 +++++++++++-------- .../teleop/DrivetrainDriverControl.java | 12 +++---- .../cyberarm/chiron/tasks/FieldLocalizer.java | 4 +-- 5 files changed, 49 insertions(+), 27 deletions(-) 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 0e4b98a..8e0f72c 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 @@ -55,7 +55,8 @@ public class Robot { private final FieldLocalizer fieldLocalizer; private final double radius, diameter; - private final double wheelRadius, gearRatio, ticksPerRevolution; + private final double wheelRadius, gearRatio; + private final int ticksPerRevolution; private boolean LEDStatusToggle = false; private double lastLEDStatusAnimationTime = 0; @@ -65,9 +66,6 @@ public class Robot { this.configuration = configuration; this.fieldLocalizer = fieldLocalizer; - this.fieldLocalizer.setRobot(this); - this.fieldLocalizer.standardSetup(); - radius = tuningConfig("field_localizer_robot_radius").value(); diameter = radius * 2; @@ -153,6 +151,10 @@ public class Robot { RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD)); imu.initialize(parameters); + + // INITIALIZE AFTER EVERYTHING ELSE to prevent use before set crashes + this.fieldLocalizer.setRobot(this); + this.fieldLocalizer.standardSetup(); } public void standardTelemetry() { @@ -231,6 +233,18 @@ public class Robot { engine.telemetry.addData(" Arm", arm.getTargetPosition()); + // Motor Velocity + engine.telemetry.addLine("Motor Velocity"); + engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getVelocity()); + engine.telemetry.addData(" Front Right Drive", frontRightDrive.getVelocity()); + + engine.telemetry.addData(" Back Left Drive", backLeftDrive.getVelocity()); + engine.telemetry.addData(" Back Right Drive", backRightDrive.getVelocity()); + + engine.telemetry.addLine(); + + engine.telemetry.addData(" Arm", arm.getVelocity()); + engine.telemetry.addLine(); // Servos @@ -318,9 +332,11 @@ public class Robot { // For: Drive Wheels public int unitToTicks(DistanceUnit unit, double distance) { + double fI = (gearRatio * ticksPerRevolution) / (wheelRadius * 2 * Math.PI * (gearRatio * ticksPerRevolution) / (gearRatio * ticksPerRevolution)); + double inches = unit.toInches(unit.fromUnit(unit, distance)); // NOTE: UNTESTED - double ticks = (wheelRadius * 2 * Math.PI) / inches * (gearRatio * ticksPerRevolution); + double ticks = fI * inches; return (int)ticks; // NOTE: UNTESTED } 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 6a8e32f..3b3f658 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 @@ -28,6 +28,8 @@ public class AutonomousRedLeftSideEngine extends CyberarmEngine { fieldLocalizer ); + robot.imu.resetYaw(); + addTask(fieldLocalizer); setupFromConfig( diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java index 937b10b..8dceebd 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java @@ -10,7 +10,7 @@ public class Move extends CyberarmState { private final double positionXInInches, positionYInInches, toleranceInInches, facing, targetVelocity, timeInMS; - private final double maxSpeed; + private final double maxVelocity; private double speed; private int distanceAlreadyTravelled; @@ -25,11 +25,11 @@ public class Move extends CyberarmState { positionYInInches = robot.getConfiguration().variable(groupName, actionName, "positionYInInches").value(); toleranceInInches = robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value(); facing = robot.getConfiguration().variable(groupName, actionName, "facing").value(); - targetVelocity = robot.getConfiguration().variable(groupName, actionName, "targetVelocity").value(); + targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocity").value()); timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; - maxSpeed = robot.tuningConfig("drivetrain_max_power").value(); + maxVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_max_velocity_in_inches").value()); speed = 0.0; } @@ -72,11 +72,13 @@ public class Move extends CyberarmState { } private void move() { - if (Math.abs(speed) > maxSpeed) { - speed = speed < 0 ? -maxSpeed : maxSpeed; + if (Math.abs(speed) > maxVelocity) { + speed = speed < 0 ? -maxVelocity : maxVelocity; } - double forwardSpeed = 1.0; //distanceInInches < 0 ? -1.0 : 1.0; + speed = 1.0; + + double forwardSpeed = -1.0; //distanceInInches < 0 ? -1.0 : 1.0; double rightSpeed = 0.0; double rotateRightSpeed = 0.0; @@ -88,6 +90,8 @@ public class Move extends CyberarmState { } } + rotateRightSpeed = 0; + // FIXME: targetSpeed * speed[Limiter] will cause infinitesimal power issues double y = -forwardSpeed * speed; double x = (-rightSpeed * speed) * 1.1; // Counteract imperfect strafing. TODO: make tunable @@ -106,19 +110,19 @@ public class Move extends CyberarmState { frontRightPower = (rotY - rotX + rx) / denominator; backRightPower = (rotY + rotX - rx) / denominator; - robot.frontLeftDrive.setPower(frontLeftPower); - robot.frontRightDrive.setPower(frontRightPower); + robot.frontLeftDrive.setVelocity(frontLeftPower * targetVelocity); + robot.frontRightDrive.setVelocity(frontRightPower * targetVelocity); - robot.backLeftDrive.setPower(backLeftPower); - robot.backRightDrive.setPower(backRightPower); + robot.backLeftDrive.setVelocity(backLeftPower * targetVelocity); + robot.backRightDrive.setVelocity(backRightPower * targetVelocity); } @Override public void stop() { - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); + robot.backLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0); + robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0); - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); + robot.frontLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0); + robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0); } } 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 index f14262e..6e55fc7 100644 --- 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 @@ -49,7 +49,7 @@ public class DrivetrainDriverControl extends CyberarmState { } double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y; - double x = (invertRobotForward ? controller.left_stick_x : -controller.left_stick_x) * 1.1; // Counteract imperfect strafing + double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x) * 1.1; // Counteract imperfect strafing double rx = -controller.right_stick_x; double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); @@ -86,11 +86,11 @@ public class DrivetrainDriverControl extends CyberarmState { } private void stopDrive() { - robot.backLeftDrive.setVelocity(0); - robot.frontRightDrive.setVelocity(0); + robot.backLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0); + robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0); - robot.frontLeftDrive.setVelocity(0); - robot.backRightDrive.setVelocity(0); + robot.frontLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0); } private void automatics() { @@ -130,7 +130,7 @@ public class DrivetrainDriverControl extends CyberarmState { robot.hardwareFault = !robot.hardwareFault; } - if (button.equals("pause")) { + if (button.equals("back")) { invertRobotForward = !invertRobotForward; } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java index a8405ec..a6c688d 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java @@ -132,8 +132,8 @@ public class FieldLocalizer extends CyberarmState { deltaX = deltaXRotation * cosineTheta - deltaYRotation * sinTheta; deltaY = deltaYRotation * cosineTheta + deltaXRotation * sinTheta; - posX = lastX + deltaX; - posY = lastY + deltaY; + posX += lastX + deltaX; + posY += lastY + deltaY; lastFrontLeft = robot.frontLeftDrive.getCurrentPosition(); lastFrontRight = robot.frontRightDrive.getCurrentPosition();