Added debug LED's and code to the Sensor. Cleaned up and re-structured Controller code. Added debug LED's and code to the Controller.