V1.0 Firmware update

- Bug removed for motors randomly stopping
- Code cleaned up
- Status flags and timeout's tested
Merge pull request #32 from karan259/master

V1.0 Firmware update