Update RC_SUMO.ino

Fixed motors, added a few motor test functions.