Large update of repo

Deletes unnecessary libraries, adds correct DMP code for bug free IMU usage, ports all previous motion control code to be used with the Mechatronic Arm.