Added code to correct motion using motor and AOD. Needs testing.