Improve the slow down effect and add support for speed change in mission mode. Seperate code related to turning setpoints into motor commands into its own folder and refactor code.
