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

Fixed Move not working correctly after first Move state

Browse Source
This commit is contained in:
Cyberarm
2023-01-31 21:38:21 -06:00
parent 3902bc3291
commit e666590d0d
1 changed files with 19 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -15,9 +15,12 @@ public class Move extends CyberarmState {
private final double maxVelocity; private final double maxVelocity;
private double speed; private double speed;
private int distanceAlreadyTravelled; private int distanceAlreadyTravelled, travelledDistance;
private final boolean stateDisabled; private final boolean stateDisabled;
private double velocity;
private double ratio = 1.0;
public Move(Robot robot, String groupName, String actionName) { public Move(Robot robot, String groupName, String actionName) {
this.robot = robot; this.robot = robot;
this.groupName = groupName; this.groupName = groupName;
@@ -41,7 +44,7 @@ public class Move extends CyberarmState {
@Override @Override
public void start() { public void start() {
// TODO: Use a dead wheel for this // TODO: Use a dead wheel for this
distanceAlreadyTravelled = robot.frontRightDrive.getCurrentPosition(); distanceAlreadyTravelled = -robot.frontRightDrive.getCurrentPosition();
} }
@Override @Override
@@ -60,7 +63,7 @@ public class Move extends CyberarmState {
return; return;
} }
int travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled; travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled;
if (robot.isBetween(travelledDistance, targetDistance - tolerance, targetDistance + tolerance)) { if (robot.isBetween(travelledDistance, targetDistance - tolerance, targetDistance + tolerance)) {
stop(); stop();
@@ -73,12 +76,21 @@ public class Move extends CyberarmState {
moveDirectional(travelledDistance); moveDirectional(travelledDistance);
} }
private void moveDirectional(double travelledDistance) { @Override
double velocity; public void telemetry() {
double ratio = 1.0; engine.telemetry.addLine("Move");
engine.telemetry.addData("Minimum Velocity", minimumVelocity);
engine.telemetry.addData("Target Velocity", targetVelocity);
engine.telemetry.addData("Velocity", velocity);
engine.telemetry.addData("Target Distance", targetDistance);
engine.telemetry.addData("Travelled Distance", travelledDistance);
engine.telemetry.addData("Distance Already Travelled", distanceAlreadyTravelled);
engine.telemetry.addData("Travel Forward", targetDistance > travelledDistance);
}
private void moveDirectional(double travelledDistance) {
if (Math.abs(travelledDistance) < easeInDistance) { if (Math.abs(travelledDistance) < easeInDistance) {
ratio = travelledDistance / easeInDistance; ratio = Math.abs(travelledDistance) / easeInDistance;
} else if (Math.abs(travelledDistance) > Math.abs(targetDistance) - easeOutDistance) { } else if (Math.abs(travelledDistance) > Math.abs(targetDistance) - easeOutDistance) {
ratio = (Math.abs(targetDistance) - Math.abs(travelledDistance)) / easeOutDistance; ratio = (Math.abs(targetDistance) - Math.abs(travelledDistance)) / easeOutDistance;
} }