Control implemented.
Need to change the move, because we don't want a desired torque, but a actual voltage.
Thread period set to 1 msec.