ShoeBot: Fix speed loop

Now closing a loop on the wheel speed to control the target orientation.
This makes the robot adaptive to changes in the center of mass.