Initial test with chained paths

grt192 · Feb 18, 2022 · 7f75a9d · 7f75a9d
1 parent 07aba7c
commit 7f75a9d
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -102,7 +102,16 @@ public RobotContainer() {
                     new Translation2d(2, -1)
                 ),
                 new Pose2d(3, 0, new Rotation2d())
-            );
+            ).andThen(new FollowPathCommand(
+                tankSubsystem,
+                new Pose2d(3, 0, new Rotation2d()),
+                List.of(
+                    new Translation2d(1, 1),
+                    new Translation2d(2, -1)
+                ),
+                new Pose2d(),
+                true
+            )).andThen(new InstantCommand(() -> tankSubsystem.setTankDriveVoltages(0, 0)));
         } else {
             autonCommand = new InstantCommand();
         }

diff --git a/src/main/java/frc/robot/commands/tank/FollowPathCommand.java b/src/main/java/frc/robot/commands/tank/FollowPathCommand.java
@@ -74,20 +74,23 @@ public FollowPathCommand(TankSubsystem tankSubsystem, Trajectory trajectory) {
     }
 
     /**
-     * Creates a FollowPathCommand from a given start point, list of waypoints, and end point.
+     * Creates a FollowPathCommand from a given start point, list of waypoints, end point, and boolean representing whether
+     * the path should be reversed (if the robot should drive backwards through the trajectory).
      * 
      * @param tankSubsystem The tank subsystem.
      * @param start The start point of the trajectory as a Pose2d.
      * @param waypoints A list of waypoints the robot must pass through as a List<Translation2d>.
      * @param end The end point of the trajectory as a Pose2d.
+     * @param reversed Whether the trajectory is reversed.
      */
-    public FollowPathCommand(TankSubsystem tankSubsystem, Pose2d start, List<Translation2d> waypoints, Pose2d end) {
+    public FollowPathCommand(TankSubsystem tankSubsystem, Pose2d start, List<Translation2d> waypoints, Pose2d end, boolean reversed) {
         this(
             tankSubsystem,
             // Target trajectory
             TrajectoryGenerator.generateTrajectory(
                 start, waypoints, end, 
                 new TrajectoryConfig(MAX_VEL, MAX_ACCEL)
+                    .setReversed(reversed)
                     .setKinematics(KINEMATICS)
                     .addConstraint(new DifferentialDriveVoltageConstraint(
                         new SimpleMotorFeedforward(Ks, Kv, Ka), 
@@ -96,4 +99,16 @@ public FollowPathCommand(TankSubsystem tankSubsystem, Pose2d start, List<Transla
             )
         );
     }
+
+    /**
+     * Creates a FollowPathCommand from a given start point, list of waypoints, and end point.
+     * 
+     * @param tankSubsystem The tank subsystem.
+     * @param start The start point of the trajectory as a Pose2d.
+     * @param waypoints A list of waypoints the robot must pass through as a List<Translation2d>.
+     * @param end The end point of the trajectory as a Pose2d.
+     */
+    public FollowPathCommand(TankSubsystem tankSubsystem, Pose2d start, List<Translation2d> waypoints, Pose2d end) {
+        this(tankSubsystem, start, waypoints, end, false);
+    }
 }