Cleaned up sensor code, split up main code from RS485 code. Added debug functions instead of Searial.println