Slip correction via accelerometer data

[ghost]

Use acceleration data to decide if the wheels are slipping. If a wheel has slipped, get encoder positions back on track and recalculate the motion profile.