Fixed stupid bug in my stupid drive forward routine. Stupid. Not tested yet...