Best Path Follower/Movement Algorithm

So in our offseason, our team was planning to use motion profiling of some sort. We were originally planning to use a Pure Pursuit algorithm to do something similar to this, but then realized, holonomic drives wouldn’t work very well for something like this. Does anyone have any ideas for any path following algorithms?

If you’re using a holonomic setup, that can be finagled into acting like a linear system, so you could use an LQR controller, which’ll theoretically give you optimal control. That’s a lot of work though. PID’s the next best thing in this situation, I suppose.

For nonholonomic, Ramsete ( theorem 8.5.2) is usually the go-to.

1 Like

If you talk for autonomous. Then we so far have employed the following strategy with mechanum and have been successful in the way of “good enough” to score every season top 3 in autonomous in our competitions.

1.) most important make sure the mechanics work and your drive is predictable.
2.) use external sensors. We in past years have used up to 3 ultrasonic range finders
3.) have a gyro and work on distance correction. that means a routine where you kick your robot off target and have it self correct direction wise
4.) establish a speed at which you travel a constant distance in a given time. So get a value like at speed .6 we travel x inches/sec when strafing and x inches when going straight
5.) now develop a path to the target like move forward 3 seconds at speed .6 keeping the right wall 10 inches away or until your front sensor is at 10 inch of the target then slow to .3 until you are 2 in awy then deploy whatever or turn right 45 deg etc. So we got good “flying blind”
6.) Have a safety routine that can also be used as a drive till you crash routine. We use talon SRX on the drive which give current feedback. So we know in normal circumstances the drive will report 2-4A per wheel so as soon as it reports 8A we have hit the target and slightly compressed the bumper so we stop and break. This way if another robot gets in the way you don’t harm yourself or an alliance member during auto. came in handy I think a year or 2 ago when going for the center switch and another alliance robot running amok. Plus you can write something like drive forward till you hit back up an inch, strafe till you hit etc. we call it feel your way by bouncing off walls

This all seems WAY more complicated than using a freaking quadrature encoder…


Pathweaver/Pathfinder is a very popular option for path following. We’re working on implementing it in our code this summer.


I recommend using Pathfinder to generate the path to follow to your target, then just setting the XY position as PID setpoints at each timestep, if you just translate and then do a PID rotation in-place to place the game piece.

Alternatively, if you can trust your robot to go forward and turn in a tank-like fashion, then you can implement Ramsete like Solomon said above, or do your standard motion profile follower. 5190’s 2018 code has good, simple code for following a path.

For either case, you can either stream the motion profile to a Talon and have the Talon execute it, or you can do all the path calculations on the Rio and just set PID+feedforward on the Talon at each timestep.

What do you mean by “holonomic drive” though? That can mean a lot of different things.