Differences

This shows you the differences between two versions of the page.

--- autonomous_routine_path_planning [2022/08/15 00:23] – rfhuang
+++ autonomous_routine_path_planning [2022/09/13 21:51] (current) – rfhuang
@@ Line 21: / Line 21: @@
 Given the modularity of the Command Based architecture, we were able to simply add the necessary commands as we saw fit with respect to the path. This was done by adding intaking, alignment, and shooting in either sequence or parallel to the trajectories. Although our autonomous routines proved to be very consistent, we endeavored to improve the accuracy and speed further. This was primarily done using the computer vision and shooter alignment previously mentioned. With this additional improvement, we were able to compensate for any errors from the path follower and achieve extremely reliable results.
+----
+===== Architecture (Sequential) =====
+. Using PathWeaver with path + splines as inputs, the desired path is turned into JSON which is machine readable to become a RamseteCommand.
+{{ ::screen_shot_2022-09-13_at_9.38.44_pm.png?400 |}}
+. TrajectorySequence.java takes in those generated path files, combines it with our drivebase PID constants into a completed RamseteCommand.
+<code java> public TrajectorySequence(Drivebase drivetrain, String ... paths) {
+    drivebase = drivetrain;
+    for (String pathLocation : paths) {
+      Command path = generatePath(pathLocation);
+      if (path == null) return;
+    }
+    for (String pathLocation : paths) {
+      Command path = generatePath(pathLocation);
+      addCommands(path);
+    }
+  }
+  private Command generatePath(String pathName) {
+    try {
+      Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(pathName);
+      Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
+      RamseteCommand ramseteCommand = new RamseteCommand(
+        trajectory,
+        drivebase::getPose,
+        new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
+        new SimpleMotorFeedforward(
+          DrivebaseConstants.ksVolts,
+          DrivebaseConstants.kvVoltSecondsPerMeter,
+          DrivebaseConstants.kaVoltSecondsSquaredPerMeter),
+        DrivebaseConstants.kDriveKinematics,
+        drivebase::getWheelSpeeds,
+        new PIDController(DrivebaseConstants.kPDriveVel, 0, 0),
+        new PIDController(DrivebaseConstants.kPDriveVel, 0, 0),
+        // RamseteCommand passes volts to the callback
+        drivebase::tankDriveVolts,
+        drivebase);
+      // Reset odometry to the starting pose of the trajectory.
+      drivebase.resetOdometry(trajectory.getInitialPose());
+      // Set up a sequence of commands
+      // First, we want to reset the drivetrain odometry
+      return new InstantCommand(() -> drivebase.resetOdometry(trajectory.getInitialPose()), drivebase)
+        .andThen(new InstantCommand(() -> drivebase.enableBrakeMode(), drivebase))
+        // next, we run the actual ramsete command
+        .andThen(ramseteCommand)
+        // Finally, we make sure that the robot stops
+        .andThen(new InstantCommand(() -> drivebase.tankDriveVolts(0, 0), drivebase));
+    } catch (IOException ex) {
+      DriverStation.reportError("Unable to open trajectory: " + pathName, ex.getStackTrace());
+    }
+    return null;
+  }</code>
+. Using a pre-generated GUI in SmartDashbaord the desired path is chosen, this happens in AutoSelector.java. The selected path is able to be read by our code when the match begins.
+<code java>public AutoSelector(Drivebase drivebase, Superstructure superstructure, Vision vision) {
+    chooser.setDefaultOption("5 Ball", new SequentialCommandGroup(
+      new InstantCommand(superstructure.intake::deploy),
+      new ParallelRaceGroup(
+        new BallManager(superstructure, drivebase),
+        new TrajectorySequence(drivebase, "paths/output/Five_1.wpilib.json", "paths/output/Five_2.wpilib.json", "paths/output/Five_3.wpilib.json")
+      ),
+      new ParallelCommandGroup(
+        new Shoot(drivebase, superstructure, vision, false).withTimeout(2.5),
+        new SequentialCommandGroup(
+          new WaitCommand(0.75),
+          new InstantCommand(superstructure.intake::deploy)
+        )
+      ),
+      new ParallelRaceGroup(
+        new BallManager(superstructure, drivebase),
+        new SequentialCommandGroup(
+          new TrajectorySequence(drivebase, "paths/output/Five_4.wpilib.json"),
+          new WaitCommand(0.75),
+          new TrajectorySequence(drivebase, "paths/output/Five_5.wpilib.json")
+        )
+      ),
+      new Shoot(drivebase, superstructure, vision).withTimeout(2)
+    ));
+    chooser.addOption("4 Ball", new SequentialCommandGroup(
+      new InstantCommand(superstructure.intake::deploy),
+      new ParallelCommandGroup(
+        new BallManager(superstructure).withInterrupt(() -> {
+          return superstructure.indexer.hasLowerBall() && superstructure.indexer.hasUpperBall();
+        }).withTimeout(4),
+        new TrajectorySequence(drivebase, "paths/output/Terminal_1.wpilib.json")
+      ),
+      new Shoot(drivebase, superstructure, vision, false).withTimeout(2.5),
+      new InstantCommand(superstructure.intake::deploy),
+      new ParallelRaceGroup(
+        new BallManager(superstructure),
+        new SequentialCommandGroup(
+          new TrajectorySequence(drivebase, "paths/output/Terminal_2.wpilib.json"),
+          new WaitCommand(1),
+          new TrajectorySequence(drivebase, "paths/output/Terminal_3.wpilib.json")
+        )
+      ),
+      new Shoot(drivebase, superstructure, vision).withTimeout(2.5)
+    ));
+    SmartDashboard.putData(chooser);
+  }
+  public Command getSelected() {
+    return chooser.getSelected();
+  }</code>
+. The selected path is taken from the GUI at game start. After that, the RamseteCommand is generated and executed in RobotContainer.java, where it will receive inputs from the drivebase controller and gyroscope to confirm that each point has been reached. If the points have not been hit, the code will adjust accordingly.