Not totally working
WIP
single motions working
Blocked interrupts and stuff
Merge branch 'feature/cleanup' into develop
Added Trajectory programming
Merge conflicts