From 9d8f4a981e41649a9e365406042fa874640a4e25 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sun, 29 Jan 2023 10:34:57 -0600 Subject: [PATCH] Fixed incorrect implementation of Robot#angleDiff, Rotate state now works, added untested ease-in-out ramping to Move state --- .idea/deploymentTargetDropDown.xml | 17 -------- .../minibots/cyberarm/chiron/Robot.java | 7 ++- .../chiron/states/autonomous/Move.java | 20 ++++++++- .../chiron/states/autonomous/Rotate.java | 43 ++++++++++++++++--- 4 files changed, 58 insertions(+), 29 deletions(-) delete mode 100644 .idea/deploymentTargetDropDown.xml diff --git a/.idea/deploymentTargetDropDown.xml b/.idea/deploymentTargetDropDown.xml deleted file mode 100644 index a4439ce..0000000 --- a/.idea/deploymentTargetDropDown.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - \ No newline at end of file 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 6451d1a..5171e4d 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 @@ -556,12 +556,11 @@ public class Robot { // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38 public double angleDiff(double from, double to) { - double value = (to - from + 180) - 180; + double value = (to - from + 180); - int fmod = (int) Math.floor(value - 0.0 / 360.0 - 0.0); - double result = (value - 0.0) - fmod * (360.0 - 0.0); + double fmod = (value - 0.0) % (360.0 - 0.0); - return result < 0 ? result + 360.0 : result + 0.0; + return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180; } public Status getStatus() { return status; } 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 3b463f2..010424f 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 @@ -8,7 +8,7 @@ public class Move extends CyberarmState { private final Robot robot; private final String groupName, actionName; - private final double targetDistance, tolerance, facing, targetVelocity, timeInMS; + private final double targetDistance, tolerance, facing, targetVelocity, minimumVelocity, timeInMS, easeInDistance, easeOutDistance; private final double maxVelocity; private double speed; @@ -25,9 +25,13 @@ public class Move extends CyberarmState { tolerance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value()); facing = robot.getConfiguration().variable(groupName, actionName, "facing").value(); targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocityInInches").value()); + minimumVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "minimumVelocityInInches").value()); timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; + easeInDistance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "easeInDistanceInInches").value()); + easeOutDistance = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "easeOutDistanceInInches").value()); + maxVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_max_velocity_in_inches").value()); speed = 0.0; } @@ -68,8 +72,20 @@ public class Move extends CyberarmState { } private void moveDirectional(double travelledDistance) { - // TODO: Ease-in-out velocity double velocity = targetVelocity; + double easeVelocity; + double ratio = 1.0; + + if (Math.abs(travelledDistance) < easeInDistance) { + ratio = travelledDistance / easeInDistance; + } else if (Math.abs(travelledDistance) > targetDistance - easeOutDistance) { + ratio = 1.0 - ((targetDistance - Math.abs(travelledDistance)) / easeOutDistance); + } + + easeVelocity = targetVelocity * ratio; + + if (easeVelocity < minimumVelocity) { easeVelocity = minimumVelocity; } + if (easeVelocity < targetVelocity) { velocity = easeVelocity; } if (targetDistance > travelledDistance) { robot.frontRightDrive.setVelocity(-velocity); diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java index b48bb85..4bfd70a 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java @@ -8,9 +8,11 @@ public class Rotate extends CyberarmState { private final Robot robot; private final String groupName, actionName; - private final double timeInMS, facing, targetFacing, targetVelocity, toleranceInDegrees; + private final double timeInMS, facing, targetFacing, targetVelocity, targetSlowVelocity, slowAngleCloseness, toleranceInDegrees; private final boolean stateDisabled, useShortestRotation, rotateRight; + private double velocity; + public Rotate(Robot robot, String groupName, String actionName) { this.robot = robot; this.groupName = groupName; @@ -24,10 +26,34 @@ public class Rotate extends CyberarmState { toleranceInDegrees = robot.getConfiguration().variable(groupName, actionName, "toleranceInDegrees").value(); targetVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.getConfiguration().variable(groupName, actionName, "targetVelocityInInches").value()); + targetSlowVelocity = robot.unitToTicks(DistanceUnit.INCH, + robot.getConfiguration().variable(groupName, actionName, "targetSlowVelocityInInches").value()); + slowAngleCloseness = robot.getConfiguration().variable(groupName, actionName, "slowAngleCloseness").value(); stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; facing = (robot.facing() + targetFacing + 360.0) % 360.0; + + velocity = targetVelocity; + } + + @Override + public void start() { + engine.telemetry.speak("Rotate"); + } + + @Override + public void telemetry() { + engine.telemetry.addLine("Rotate"); + engine.telemetry.addData("Angle Diff", robot.angleDiff(robot.facing(), facing)); + engine.telemetry.addData("Target Facing/Angle", facing); + engine.telemetry.addData("Provided Target Facing/Angle", targetFacing); + engine.telemetry.addData("Use Shortest Rotation", useShortestRotation); + engine.telemetry.addData("Rotate Right", rotateRight); + engine.telemetry.addData("Tolerance In Degrees", toleranceInDegrees); + engine.telemetry.addData("Target Velocity", targetVelocity); + engine.telemetry.addData("Target Slow Velocity", targetSlowVelocity); + engine.telemetry.addData("Slow Angle Closeness", slowAngleCloseness); } @Override @@ -49,7 +75,8 @@ public class Rotate extends CyberarmState { } double currentDegrees = robot.facing(); - double diff = robot.angleDiff(facing, currentDegrees); + double diff = robot.angleDiff(currentDegrees, facing); + velocity = targetVelocity; if (Math.abs(diff) <= toleranceInDegrees) { stop(); @@ -59,6 +86,10 @@ public class Rotate extends CyberarmState { return; } + if (Math.abs(diff) <= slowAngleCloseness) { + velocity = targetSlowVelocity; + } + if (useShortestRotation) { if (diff < 0) { rotateLeft(); @@ -83,11 +114,11 @@ public class Rotate extends CyberarmState { } private void rotate(double multiplier) { - robot.frontRightDrive.setVelocity(targetVelocity * multiplier); - robot.backLeftDrive.setVelocity(targetVelocity * multiplier); + robot.frontRightDrive.setVelocity(-velocity * multiplier); + robot.backLeftDrive.setVelocity(velocity * multiplier); - robot.backRightDrive.setVelocity(targetVelocity * multiplier); - robot.frontLeftDrive.setVelocity(targetVelocity * multiplier); + robot.backRightDrive.setVelocity(velocity * multiplier); + robot.frontLeftDrive.setVelocity(-velocity * multiplier); } @Override