Fixing stuff

Added extra movements
Removed runMovement and just use using System.Thread
Fixed knee angle calculation for replant foot