Hello. Our team is beginning to use the trajectory following tools within WPILib. The robot code appears to work, somewhat, but we expected a bit less of a violent turn. We’ve run out of ideas about what to adjust in the coding and are hopeful that there will be some guidance here.
A link to our code and a brief video of the robot should be below.
The path you are running is a pretty tight turning path in that it needs to turn almost 180 degree over 1m of travel. You say S curve but the path you have specified is more like a sawtooth signal.
If I was you I would go into PathWeaver and create a path there and then use this code to import that trajectory. That will help you visualize what the robot will do and make sure that expectations match reality.