Eastbots Wiki

Encoder Based Odometric Path Planning

Github
TrajectorySequence.java AutoSelector.java

One of the goals we set during the offseason was to upgrade our current autonomous pathing through the use of motion profiling. We had tried this in 2020, but had great difficulty in getting off the ground. This year, we did more research and we were able to get ahead of the curve before we started development. We used the WPILib toolsuite for designing and running autonomous paths with trajectories using Ramsete controllers, odometry, and the Pathweaver drawing app. We tested the pipeline of characterization (to determine necessary controllers and constants), generating and compiling a drawn path, and executing the path on Romis. This allowed us to gain experience with the process in creating and tuning robust autonomous paths. We were able to put this to the test on our robot, giving us the ability to create many different paths and adapt to unpredictable situations.

Fig 4.6: One of our three-ball autonomous routines. We designed this path with high velocities and high angle turns which was made possible by our highly refined PID loops. This was to make

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.

1. Using PathWeaver with path + splines as inputs, the desired path is turned into JSON which is machine readable to become a RamseteCommand. 2. TrajectorySequence.java takes in those generated path files, combines it with our drivebase PID constants into a completed RamseteCommand.

 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;
  }

3. 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.

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();
  }

4. 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.