V1.0 Firmware update

- Bug removed for motors randomly stopping
- Code cleaned up
- Status flags and timeout's tested