pathfinder experimentation thoughts

[virtuald]

If you're going to go down this path, here's what I recommend:

Pathfinder takes max velocity and max acceleration as parameters. I didn't understand at first what their relation to reality was. However, to get a smooth trajectory, it's really important to understand how they relate to the generated trajectory and the motor output values.

• Read this paper: https://www.chiefdelphi.com/media/papers/3402
• Use the methodology in there to compute the theoretical max velocity and acceleration of your robot
  • Protip: this computation is done in https://github.com/robotpy/pyfrc/pull/129 and logged to the stdout...
• Optional: use the paper to find actual kv/ka values for your robot ... though, you probably don't have time for that

Pathfinder 'features' to be aware of:

• You can give it feet instead of meters and it'll work fine, just be consistent
• It outputs encoder values and output motor values, but they don't take the actual robot heading into account. You have to do this on your own
  • The documentation mentions this and suggests a P controller (p*gyro_error)... but we've had more success with a PD controller
  • p*gyro_error + d*(gyro_error - last_gyro_error) ... specifying the correct D value is critical to making it work right on carpet
  • Make sure you're able to tune turning separately
• It doesn't allow trajectories that have > 90 degree turns. But I think you can work around that by using multiple waypoints that are less sharp
• It always thinks a robot is going forwards, so if you want to chain multiple trajectories together (to do multiple cubes) you have to do more work. We did it, it's not so bad.
• Sometimes it outputs motor values higher than 1. You'll have to compensate for that.

Generating your trajectories on the robot is a bad idea, but here's other performance thoughts:

• I recommend generating all of your trajectories as part of the deploy process, that way you're guaranteed to always have the latest trajectory data on your robot. See http://robotpy-pathfinder.readthedocs.io/en/stable/usage.html#generating-a-trajectory
• At robot startup (not during autonomous mode), precompute all of your EncoderFollower objects and TankModifiers. They take a few ms to compute, and you don't want to do that during your 15s of autonomous mode

Once you get it working, you need to know how well it's actually working (or not working).

• Set up telemetry logging -- when testing out pathfinder, you're going to want to log data from the robot and compare the actual numbers versus what it's actually doing.
  • See https://github.com/robotpy/pynetworktables/tree/master/samples/json_logger for something I created this year to do exactly this. You'll want to send an array of data each robot loop to it.
  • The data you'll find useful to plot are wheel error (actual encoder value - what pathfinder wanted the value to be), gyro error (actual heading - pathfinder's computed heading), and finally the motor output value. By tuning the various input values, the goal is to get the error to be low along the entire path. I'd say shoot for < 5 degrees and < 1ft.
  • These plots are even useful in simulation. Just because it's following 'a path' that looks similar to what you want does not mean it's following the path you want it to follow. Getting that tuning right is important.

Finally, I'd recommend writing it as a magicbot component. Don't use a separate thread as the Java docs recommend -- threads are bad in python. You can use the LoopTimer to measure how stable your loops are... I've had issues this season but in theory we should be able to obtain stable 20ms loops (+/- 2ms or so).

[virtuald]

I updated the realistic physics PR. It feels a lot realistic than it did previously. I also updated an example program to show how to use it: https://github.com/robotpy/examples/tree/tankmodel/physics-4wheel/src

[ErikBoesen]

Now that the season is over and we have more free time to experiment with these things, we're actually going to work on getting MP working. Sorry we didn't get to this sooner. We'll let you know if we have any questions.