Driver.py: goal recalcuation and overshoot handeling improvment needed

Right now the driver code only handles overshooting with turns.
Forward/backward drives I've seen it overshoot by .2m would be nice to have this corrected within some reasonable distances/tolerances 