Changed from angle rate to raw angle
working basic motions
Merge branch 'raw'