diff --git a/src/main/deploy/choreo/AmpLanePABCSprint.1.traj b/src/main/deploy/choreo/AmpLanePABCSprint.1.traj index 15541071..97899bee 100644 --- a/src/main/deploy/choreo/AmpLanePABCSprint.1.traj +++ b/src/main/deploy/choreo/AmpLanePABCSprint.1.traj @@ -89,6 +89,15 @@ "velocityX": 0.6104210144241932, "velocityY": -0.03874143612331081, "timestamp": 0.6651392633986909 + }, + { + "x": 2.59665584564209, + "y": 6.861835956573486, + "heading": 0.499346976196016, + "angularVelocity": 0, + "velocityX": 2.166068917875568e-34, + "velocityY": -1.0924876316624288e-35, + "timestamp": 0 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePABCSprint.2.traj b/src/main/deploy/choreo/AmpLanePABCSprint.2.traj index 2c1fa834..33d26041 100644 --- a/src/main/deploy/choreo/AmpLanePABCSprint.2.traj +++ b/src/main/deploy/choreo/AmpLanePABCSprint.2.traj @@ -98,6 +98,15 @@ "velocityX": -0.049670298773671635, "velocityY": -0.5956898266326979, "timestamp": 0.7211120494834121 + }, + { + "x": 2.489257335662842, + "y": 5.573054313659668, + "heading": 0, + "angularVelocity": -7.406891463392208e-37, + "velocityX": 0, + "velocityY": -1.9623633387097932e-38, + "timestamp": 0 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePABCSprint.3.traj b/src/main/deploy/choreo/AmpLanePABCSprint.3.traj index 760ee518..b369d31a 100644 --- a/src/main/deploy/choreo/AmpLanePABCSprint.3.traj +++ b/src/main/deploy/choreo/AmpLanePABCSprint.3.traj @@ -107,6 +107,15 @@ "velocityX": 0.030625814625206325, "velocityY": -0.5382739261723708, "timestamp": 0.7203042277615874 + }, + { + "x": 2.560856342315674, + "y": 4.302172660827637, + "heading": -0.6696388746005083, + "angularVelocity": 3.3466099182293266e-40, + "velocityX": -1.5329788094467477e-40, + "velocityY": -2.4298197889709004e-41, + "timestamp": 0 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpLanePABCSprint.4.traj b/src/main/deploy/choreo/AmpLanePABCSprint.4.traj index a98f5740..94283d2c 100644 --- a/src/main/deploy/choreo/AmpLanePABCSprint.4.traj +++ b/src/main/deploy/choreo/AmpLanePABCSprint.4.traj @@ -314,6 +314,15 @@ "velocityX": 0.6467380935432705, "velocityY": -0.1749643126016669, "timestamp": 2.289462634346943 + }, + { + "x": 7.896503925323486, + "y": 3.1254632472991943, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.197315800387914e-40, + "velocityY": 0, + "timestamp": 4.687868099872624 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGFSprint.1.traj b/src/main/deploy/choreo/SourceLanePHGFSprint.1.traj index 6477f943..c6336dfd 100644 --- a/src/main/deploy/choreo/SourceLanePHGFSprint.1.traj +++ b/src/main/deploy/choreo/SourceLanePHGFSprint.1.traj @@ -323,6 +323,15 @@ "velocityX": -0.6245486738021084, "velocityY": 0.15056155787898376, "timestamp": 3.129535870722922 + }, + { + "x": 5.800710201263428, + "y": 1.4382134675979614, + "heading": -0.6435017481374367, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -1.3678756469181992e-30, + "timestamp": 0 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGFSprint.2.traj b/src/main/deploy/choreo/SourceLanePHGFSprint.2.traj index 3f77a8e3..22b5b56c 100644 --- a/src/main/deploy/choreo/SourceLanePHGFSprint.2.traj +++ b/src/main/deploy/choreo/SourceLanePHGFSprint.2.traj @@ -296,6 +296,15 @@ "velocityX": -0.6307640701566927, "velocityY": 0.19074504838259257, "timestamp": 2.2599600442952408 + }, + { + "x": 4.941522121429443, + "y": 4.46327018737793, + "heading": -0.16514867741462683, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGFSprint.3.traj b/src/main/deploy/choreo/SourceLanePHGFSprint.3.traj index be0ceb46..5344e952 100644 --- a/src/main/deploy/choreo/SourceLanePHGFSprint.3.traj +++ b/src/main/deploy/choreo/SourceLanePHGFSprint.3.traj @@ -251,6 +251,15 @@ "velocityX": -0.7490809237244538, "velocityY": 0.09593023905468932, "timestamp": 2.3823851103441687 + }, + { + "x": 4.941522121429443, + "y": 4.46327018737793, + "heading": -0.16514867741462683, + "angularVelocity": 2.094638185256543e-30, + "velocityX": 0, + "velocityY": 4.196369620080891e-32, + "timestamp": 0 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceLanePHGFSprint.4.traj b/src/main/deploy/choreo/SourceLanePHGFSprint.4.traj index 0c371299..fd94802f 100644 --- a/src/main/deploy/choreo/SourceLanePHGFSprint.4.traj +++ b/src/main/deploy/choreo/SourceLanePHGFSprint.4.traj @@ -134,6 +134,15 @@ "velocityX": 0.6361166658859392, "velocityY": -0.38962222963131005, "timestamp": 1.2533412071928467 + }, + { + "x": 7.80548095703125, + "y": 2.7090952396392822, + "heading": 0, + "angularVelocity": -2.0882967897850613e-33, + "velocityX": 6.051909834592122e-36, + "velocityY": 0, + "timestamp": 9.363148771806673 } ] } \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/util/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/AutoCommand.java index 44772a0e..4f62364d 100644 --- a/src/main/java/org/team1540/robot2024/util/AutoCommand.java +++ b/src/main/java/org/team1540/robot2024/util/AutoCommand.java @@ -15,7 +15,7 @@ public class AutoCommand extends SequentialCommandGroup { private List paths = new ArrayList<>(); - private int index = 0; + private int pathIndex = 0; public AutoCommand(String name){ this.name = name; @@ -52,16 +52,16 @@ public List getPaths() { return paths; } - public PathHelper getPath(int index){ - return paths.get(index); + public PathHelper getPath(int pathIndex){ + return paths.get(pathIndex); } public PathHelper getNextPath(){ - return getPath(index++); + return getPath(pathIndex++); } - public int getIndex(){ - return index; + public int getPathIndex(){ + return pathIndex; } - public void setIndex(int index){ - this.index = index; + public void setPathIndex(int index){ + this.pathIndex = index; } } diff --git a/src/main/java/org/team1540/robot2024/util/PathHelper.java b/src/main/java/org/team1540/robot2024/util/PathHelper.java index 234db91c..18cf6f21 100644 --- a/src/main/java/org/team1540/robot2024/util/PathHelper.java +++ b/src/main/java/org/team1540/robot2024/util/PathHelper.java @@ -71,12 +71,12 @@ public PathPlannerPath getPath() { public Command getCommand(Drivetrain drivetrain, boolean shouldRealign) { BooleanSupplier shouldFlip = () -> DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red; - Supplier flippedInitialPose = () -> shouldFlip.getAsBoolean() ? GeometryUtil.flipFieldPose(initialPose) : initialPose; + Supplier startingPose = () -> shouldFlip.getAsBoolean() ? GeometryUtil.flipFieldPose(initialPose) : initialPose; Command command = new ConditionalCommand( AutoBuilder.pathfindThenFollowPath(path, constraints), AutoBuilder.followPath(path), - () -> drivetrain.getPose().getTranslation().getDistance((flippedInitialPose.get()).getTranslation()) > 1 && shouldRealign); //TODO tune this distance - Command resetCommand = new InstantCommand(() -> drivetrain.setPose(flippedInitialPose.get())); + () -> drivetrain.getPose().getTranslation().getDistance((startingPose.get()).getTranslation()) > 1 && shouldRealign); //TODO tune this distance + Command resetCommand = new InstantCommand(() -> drivetrain.setPose(startingPose.get())); return isResetting ? resetCommand.andThen(command) : command; }