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