 void setup() {
     setupQuad();
     horizon.standby();
+    setState(DISARMED);
+}
+
+void setState(State s){
+    state = s;
+    comms.sendString(stateString[s]);
 }
 
 void loop() {

const uint8_t CHANNEL_MIN = 32;
const uint8_t CHANNEL_MAX = 148;
float yawTarget = 0.0;
enum {
    DISARMED,
    ARM,
    CALIBRATE_MOTORS,
    OFF,
    ENABLE,
    CALIBRATE,
    FLYING
} state = OFF;
enum RadioChannel{
    RADIO_PITCH    = 0,
    RADIO_ROLL     = 1,


    //flight mode state machine
    switch(state){
        /*#DISARMED Quadcopter is in a safe state. hold Down and to the right on
         * the Throttle stick for two seconds to begin the arming process
        **/
        case DISARMED:
            if(timeState<&radio_downRight>() > 2000) {
                calStartTime = millis();
                setState(CALIBRATE);
            }
            break;
        /*#CALIBRATE Calibrating the gyroscope
         * This lasts for two seconds. Keep the quadcopter as still as possible.
        **/
        case CALIBRATE:
            orientation.calibrate(true);
            if(calStartTime + 2000 < millis()) {
                orientation.calibrate(false);
                yawTarget = orientation.getYaw();
                output.enable();
                setState(FLYING);
            }
            break;
        /*#FLYING Quadcopter is like
         * Hold down and to the left on the throttle stick to disarm
        **/
        case FLYING:
            fly();
            if(timeState<&radio_downLeft>() > 750) {
                output.disable();
                setState(DISARMED);
            }
            break;
    }

