Today was my team’s last meeting for this season and it is to my great satisfaction that we were able to get our pathfinding working today. While it is working, it is far from perfect and still requires a lot of tuning. Unfortunately, I will have to leave that with the rest of my team for next year as this was my senior year of robotics.
The idea of following trajectories with our mecanum bot while utilizing the holonomic drivetrain started off as a small idea midway through last year’s build season and really took shape at the end of last year’s season. Since we didn’t plan on building an entirely new robot this year, we were able to divert more time and resources towards getting it working.
Since WPILib has little documentation on the mecanum trajectory following, this post serves as a place to help other teams who may be looking to do the same as we did.
To start: Instead of using wpilib’s trajectory generator, we opted to use @mjansen4857 path planner which we found on their GitHub. Linked below. I noticed they had added an “experimental” holonomic drivetrain version of their path generator so I gave it a shot and it actually worked very well! So @mjansen4857 if you are still wondering whether your holonomic generator works… it does!
Path Planner Github: https://github.com/mjansen4857/PathPlanner
Path Planner CD Post: https://www.chiefdelphi.com/t/motion-profile-generator/338871
The nicest part about having a visual generator is the ease of use and time it saves from having to map out waypoints. For anyone who doesn’t have a visual generator, I highly recommend giving this one a shot.
Here is an example video of a path being previewed on the path planner and then followed by the robot:
Preview Then Play | Slalom Path (No Markers)
Here is another example of a path being run:
Forward and Spin 360
Here is our GitHub repository for the trajectory following:
We use WPILibs trajectory class because we had our code setup for that before we started using the visual path planner. We ended up just creating an array of trajectories because we needed more data than a typical Trajectory.State could provide (See next fact). If I had more time I probably would have created my own trajectory class that has everything we needed to follow the trajectories.
We have the path planner output Time, Velocity, Acceleration, X Pose, Y Pose, Heading at each timestep, Curvature, and Absolute Heading. Since the trajectory.state can only take in one heading we had to send the absolute heading in a parallel trajectory. The absolute heading is used to calculate velocity feed forwards for chassis speeds. Whereas heading at each timestep is how the robot should actually be orientated.
We use a modified MecanumControllerCommand that both follows the trajectories and also follows the headings as shown in the path planner. This allows us to basically know exactly how the robot will drive based on the preview in the planner.
Bugs we fixed:
Since some of these fixes were not merged yet so we pulled the related classes out of wpilib and implemented them ourselves.
- WPILib’s PIDController and ProfiledPIDController had issues with continuous inputs. We implemented this fix: https://github.com/wpilibsuite/allwpilib/issues/3168
- WPILib’s Mecanum Kinematics were slightly off. We implemented this fix: https://github.com/wpilibsuite/allwpilib/pull/3266
If anyone else has any questions about anything here let me know I am more than happy to help! If anyone is having troubles following trajectories with their mecanum drivetrain I am also more than happy to help! I know our code is a little messy and I apologize for that. We were in a rush to get this working before we had to pack our season up so I haven’t had a chance to clean up the code much. I just hope that any teams who have been struggling with mecanum pathfinding can find some helpfulness in our code. We surely had a struggle getting it working but the satisfaction when it finally did was like nothing else. Good luck to all the teams this year and in your future years of FRC. FIRST has impacted me so positively and I am so grateful to have been a part of it for the past three years.
The face of a happy Testy Botty Bot