I wrote
something like this a while ago, and I've got a little bit of advice.
During a competition, any periodic functions aren't exactly constant. Their timing may change, especially on a field, as it waits for data from the Driver Station. To remedy this, use something like a 'heartbeat' thread, that triggers on a constant rate, all the time (for us, we use 50hz). This means you don't get the next data point at an inconsistent rate, which causes jerking in driving and/or any other mechanisms.
Other than that, it looks good
. Good luck in Autonomous!