Formatted and tested code.  Need to test code with actual hardware and sensors.  Consider using HIL testing to supplant real-world testing and logistics