Angle interpolation fixed

- Angles of shoulder --> Elbow are now working
- Need to make a thread that does movement, single thread causes major
problems