TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-13 08:32:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed incorrect implementation of Robot#angleDiff, Rotate state now works, added untested ease-in-out ramping to Move state

Browse Source
This commit is contained in:
Cyberarm
2023-01-29 10:34:57 -06:00
parent 4cf664843d
commit 9d8f4a981e
4 changed files with 58 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
.idea/deploymentTargetDropDown.xml generated
View File

@@ -1,17 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="deploymentTargetDropDown">
<runningDeviceTargetSelectedWithDropDown>
<Target>
<type value="RUNNING_DEVICE_TARGET" />
<deviceKey>
<Key>
<type value="SERIAL_NUMBER" />
<value value="192.168.43.1:5555" />
</Key>
</deviceKey>
</Target>
</runningDeviceTargetSelectedWithDropDown>
<timeTargetWasSelectedWithDropDown value="2023-01-26T22:23:44.435674Z" />
</component>
</project>

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

@@ -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; }

20
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java
View File

@@ -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);

43
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java
View File

@@ -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