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.
chfriedrich98