Working
Working simulation, fixed kr to use deg/s not rad/s