Added code for motors

There's a problem with the right one; probably hardware.