Fixed the motor id number 5, integrated curve trajectory correctly