Motor spinning fix, code cleanup
Merge pull request #2 from piotrek91666/fix_motor

Motor spinning fix, code cleanup