The simplified code revealed that the motors were switched.
Merge pull request #7 from antoneri/master

The simplified code revealed that the motors were switched.