Here's a new initialize routine to put into auton

You could also just write it yourself if you prefer that, your choice.