Changed the feedback linearization to fix the motors