QC6B Updates

Added all sensory functions, rewrote interrupt routines, added a switch
function, added certain basic checking services, and cleaned up a few
variables. Still needs PID function and complimentary checking.