Template Modified
-Deceleration Modified
-Struct Mission is used instead of positionQueue
Bugs:
-Speed may turn to be 0 during deceleration
-Sometimes has drift issue