Updated motor control

This version will let the motor rotate backwards!