Added restrictions on motor speeds in simulation. Added wrap to pi for theta.