TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-13 06:22:33 +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 double speed;
private int distanceAlreadyTravelled;
private int distanceAlreadyTravelled, travelledDistance;
private final boolean stateDisabled;
private double velocity;
private double ratio = 1.0;
public Move(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
@@ -41,7 +44,7 @@ public class Move extends CyberarmState {
@Override
public void start() {
// TODO: Use a dead wheel for this
distanceAlreadyTravelled = robot.frontRightDrive.getCurrentPosition();
distanceAlreadyTravelled = -robot.frontRightDrive.getCurrentPosition();
}
@Override
@@ -60,7 +63,7 @@ public class Move extends CyberarmState {
return;
}
int travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled;
travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled;
if (robot.isBetween(travelledDistance, targetDistance - tolerance, targetDistance + tolerance)) {
stop();
@@ -73,12 +76,21 @@ public class Move extends CyberarmState {
moveDirectional(travelledDistance);
}
private void moveDirectional(double travelledDistance) {
double velocity;
double ratio = 1.0;
@Override
public void telemetry() {
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) {
ratio = travelledDistance / easeInDistance;
ratio = Math.abs(travelledDistance) / easeInDistance;
} else if (Math.abs(travelledDistance) > Math.abs(targetDistance) - easeOutDistance) {
ratio = (Math.abs(targetDistance) - Math.abs(travelledDistance)) / easeOutDistance;
}