Main TwoWheelMotorControl algorithm implemented