Major refactor

- Refactored command queue parsing and processing
- Switched from WWDG to IWDG to reduce the number of spurious WDG trips
- Added timer interrupts for sending back heartbeats and for creating a
motor control loop