From Romi Path Following to FRC Robot

Recently, I purchased a Romi to use as a learning experience to dive deep into FRC Path Following. It was probably the best money I’ve ever spent toward FIRST. Just gonna outline some of my experiences here.

Start from the basics

This is sooooo incredibly critical! If you cannot write a basic java program that allows you to control the robot with joysticks, step back and focus on that for a bit. While the “Path Following” code itself looks copy/pastable, you need to understand what’s going on. Debugging is incredibly critical here.

I first programmed my Romi to use my controller, and then chased after my cat. A very amusing and fun experience.

I consider myself having a pretty moderate level of programming experience. There are many many things that can go wrong with path following. It’s incredibly easy to think one this is the issue, and break others in the process of fixing that.

Characterizing the Romi

This project by ZQ which allows for Romi trajectory tracking has been fantastic (soon available for frc-char replacement SysID). However, there were a couple of pain points.

  1. Be very very careful with your units. Meters everything, meters always. Do not convert later, you will step on a nail here.
  2. I initially forgot to enter my units per rotation (0.219915) and that took me several hours to debug later on. Do it right first.

For those interested, this is my Romi Characterization (2.8 MB).

Oh yeah, before I forget, as noted in ZQs characterization README, it’s quite important to put the velocity threshold quite low. I put (0.050) for mine.

Disclaimer: While Romis are “identical”, I got quite different characterization results than ZQs data did. As a test, I used his and mine. While his data tracked “okay”, I got much much better results using my own data, and tuning that.

Robot Code Pitfalls

Do this

  • Send your robot Pose2D to Field2D, it’ll make debugging immensely easier
  • Plot your trajectory onto the dashboard, this will eliminate any concerns that the trajectory itself is wrong.
  • Plot current wheel speeds versus expected wheel speeds, this will help debugging it not following a smooth path (I did before and after Ramsete wheel speeds)

Don’t do this

  • Be very careful with inversions. Only ever invert in one place (I recommend your inputs), to keep things consistent. Inversions can very quickly lead you into a rabbit hole.
  • Start off with a straight line trajectory, make sure it follows it reasonable well (you’re odometry is accurate)
  • Move toward a curve, verify it follows the curve reasonable well (this is where the inversions can really get tricky).

Read the docs… once… twice… thrice!!

Do not skip steps! This takes time to learn!

ZQ Characterization Project: romi-examples/romi-characterization at main · bb-frc-workshops/romi-examples · GitHub

  • Used as a baseline to debug any characterization issues

ZQ Ramsete Project: romi-examples/romi-trajectory-ramsete at main · bb-frc-workshops/romi-examples · GitHub

  • Used as a reference to determine any inversion issues or code issues.

Characterization Documentation: Robot Characterization — FIRST Robotics Competition documentation

  • Used to understand the inputs and outputs, this is very important since you can’t compare data on an actual robot.

Path Planning Documentation: Trajectory Tutorial — FIRST Robotics Competition documentation

  • Meat and bones of the tutorial

Troubleshooting Path Planning: Troubleshooting — FIRST Robotics Competition documentation

  • This article was so useful for debugging why my Romi’s tracking was doodoo.

Non-Commandbased Ramsete Example: allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller at main · wpilibsuite/allwpilib · GitHub

  • I did non-commandbased for my romi project, and used this as another reference.

My Output

Romi Code: GitHub - Daltz333/Romi_Traj_Following

Then… the same day I got the above path nice and cleaned up, I went to try it on an actual robot…

In a session of two hours, I characterized and programmed my robot to follow the barrel racing path using the knowledge and debugging steps from the Romi. This wasn’t without flaws. I prepared the robot project by using previous robot code as a baseline (already had sensor logic for velocity/distance, gyro, and motor ids). Then, at home, I threw together it in simulation to verify my robot code logic was accurated.

Then, in only a two hour timespan, I characterized my robot and plugged those in to my already created robot code. I did have some motor inversion issues to work out due to simulation inversions differing from real life. Additionally, my math for inches to meters was wrong for some dumb reason I have yet to figure out (I copied CTREs native to meters example code).

Robot Code: GitHub - Daltz333/CTRE_Trajectory_Following: Example Robot following Trajectories

Overall… just another Romi success story.

20 Likes

Thanks for this detailed write-up! It mirrors our team’s experience this season very closely. The Romi has definitely given us 1000% more hands-on experience using WPILIB than was ever possible before. We also were able to take Romi code and translate it for use on our competition robot, and vice-versa.

Excellent tip here, and thank you for sharing your code. This should be very helpful. I guess I know what I’ll be doing today…

2 Likes

Basically, if your expected wheel speeds are too much then there are a couple of solutions.

  • Your max speed is too high.
  • Your feedforward is not enough
  • Your wheel pids is not enough
3 Likes

Fantastic write-up!

Most excellent. This is the most powerful thing to me. As far as simulation can get you (and as much of a quantum leap as it was), there was historically still a gap of the skillset of “getting on hardware”. This is very clear evidence to me that the gap is getting filled in quite effectively.

It’s awesome because all the things you mentioned are my historical pain-points when going from sim to hardware. The fact you were able to do your learning on a small-ish, mostly-idealized platform and then rip through it fast on the real robot is a very powerful tool indeed.

3 Likes

Would it also be worth noting that you had to lower the max velocity error to get good P values for the romi?

1 Like

It wouldn’t hurt. It’s pretty easy to tune the PID compared to tuning the feedforward (I found tuning feedforward to cause lots of unpredictable behavior such as straight lines fine, but curves crap, and vise-versa)

1 Like

We have to do that too, but I feel we have not spent as much time as we should characterizing it.

1 Like

What do you mean by spending more time characterizing it?

We mostly either use the constants from ZQ’s characterization, or constants we got from running the characterization tool for what we feel may be too short a time. Our trajectories are not incredibly consistant. I was thinking this was a large piece of that.

I personally got Romi constants that were quite a ways different from ZQs. I characterized in a 2 meter space.

1 Like

We only had ~1 meter when we tried characterizing. I think that is the next thing we will work on.

I had ZQs data working “okay”. It followed the path, just not very well. If your robot doesn’t the follow the path at all, even remotely, I would verify other things first.

1 Like

Thank you. That is what happens with us too. It made the shape, but the specific waypoints were not consistently achieved. The not very well for us on the Frantic Fetch course though, was the difference between proficient and not proficient.

We were able to tweak the waypoints to make it work much of the time, but the thing that took so much time was that the errors were not consistent.

The other thing we did not know was too look at the projected speed v. actual speed. It is another reason we are thankful for this thread.

@Daltz3, I have to start learning programming from scratch with no knowledge. Our team uses C++ and you had mentioned that learning Java would be critical/implied. With a time crunch to learn the basics of programming before June and learn C++ before the FRC Season, do you think that learning Java would be better? And if so, do you have any resources for someone who doesn’t know Java to learn how to use Java?

I would recommend Java for rookie teams, assuming nobody on the team has any prior experience. You gain the most in terms of support, and there are generally less concepts that are required to learn. If you don’t already have a grasp on C++ yet, you can switch to Java.

Anyways, the concepts learned from C++ can near directly be applied to Java, so you can always switch down-the-line.

The Romi supports both languages, as it is run in English. But VS Code’s intellisense for C++ isn’t that good (it’s slowly getting better). My two examples in the above post are both in Java, non-commandbased.

4 Likes