/********************************************************************** * Function: doMasterSM * @return None. * @remark Executes one cycle of the boat's master state machine. * @author David Goodman * @date 2013.03.28 **********************************************************************/ static void doMasterSM() { checkEvents(); #ifdef USE_TILTCOMPASS TiltCompass_runSM(); #endif #ifdef USE_GPS GPS_runSM(); #endif #ifdef USE_NAVIGATION Navigation_runSM(); #ifdef USE_ERROR_CORRECTION gpsCorrectionUpdate(); #endif #endif #ifdef USE_DRIVE Drive_runSM(); #endif #ifdef USE_XBEE Xbee_runSM(); #endif #ifdef USE_BAROMETER Barometer_runSM(); doBarometerUpdate(); // send barometer data #endif switch (state) { case STATE_SETSTATION: doSetStationSM(); if (event.flags.haveStartRescueMessage) { startRescueSM(); } else if (event.flags.setStationDone) { if (haveOrigin) startStationKeepSM(); else startOverrideSM(); } break; case STATE_SETORIGIN: doSetOriginSM(); if (event.flags.setOriginDone) startOverrideSM(); break; case STATE_STATIONKEEP: doStationKeepSM(); if (event.flags.haveStartRescueMessage) startRescueSM(); else if (!haveStation) setError(ERROR_NO_STATION); break; case STATE_OVERRIDE: if (!wantOverride) { //setError(ERROR_NO_ORIGIN); if (!haveOrigin) startSetOriginSM(); // do we ant infinite startup loop? else if (event.flags.haveStartRescueMessage) startRescueSM(); else if (event.flags.haveSetStationMessage ) startSetStationSM(); else if (haveOrigin && haveStation) startStationKeepSM(); // Use autonomous controls if (haveOrigin && (haveStation || event.flags.haveStartRescueMessage)) { Override_giveMicroControl(); DBPRINT("Micro has control.\n"); #ifdef USE_SIREN Siren_blueLightOff(); #endif } } break; case STATE_RESCUE: doRescueSM(); if (event.flags.haveStartRescueMessage) { startRescueSM(); } else if (event.flags.haveReturnStationMessage) { if (haveStation) startStationKeepSM(); else setError(ERROR_NO_STATION); } // Turn off rescue siren (red) if (event.flags.haveError || state != STATE_RESCUE) { #ifdef USE_SIREN Siren_redLightOff(); #endif } break; } // ------- Caught by most states ----------- if (state != STATE_RESCUE) { if (event.flags.haveSetStationMessage) startSetStationSM(); } if (state != STATE_OVERRIDE) { if (event.flags.haveError) { startOverrideSM(); overrideShutdown = TRUE; } if (wantOverride) startOverrideSM(); } if (event.flags.haveResetMessage) resetAtlas(); }
/** * Function: runMasterSM * @return None. * @remark Executes one cycle of the command center's state machine. * @author David Goodman * @date 2013.03.09 */ void runMasterSM() { //Magnetometer_runSM(); // Record these button presses since we don't know // if they will be pressed after runSM lockPressed = isLockPressed(); zeroPressed = isZeroPressed(); if(lockPressed || zeroPressed){ #ifdef USE_ENCODERS Encoder_runSM(); #endif if(lockPressed) { printf("Lock was pressed.\n"); #ifdef USE_NAVIGATION #ifdef USE_ENCODERS Encoder_enableZeroAngle(); Encoder_runSM(); Coordinate ned; // = Coordinate_new(ned, 0, 0 ,0); if (Navigation_getProjectedCoordinate(&ned, Encoder_getYaw(), Encoder_getPitch(), height)) { printf("Desired coordinate -- N: %.6f, E: %.6f, D: %.2f (m)\n", ned.x, ned.y, ned.z); #ifdef USE_XBEE Mavlink_send_start_rescue(XBEE_UART_ID, TRUE, 0,ned.x, ned.y); #endif } else { printf("Failed to obtain desired NED coordinate.\n"); } Encoder_disableZeroAngle(); #else printf("Navigation module is disabled.\n"); #endif #endif } else if (zeroPressed) { // Zero was pressed #ifdef USE_ENCODERS Encoder_setZeroAngle(); #endif useLevel = TRUE; #ifdef USE_MAGNETOMETER Magnetometer_runSM(); heading = Magnetometer_getDegree(); updateHeading(); #endif //printf("Zeroing...\n"); } else { } } if (!zeroPressed) { if (useLevel) printf("Done zeroing.\n"); useLevel = FALSE; } #ifdef USE_ACCELEROMETER Accelerometer_runSM(); updateAccelerometerLEDs(); #endif #ifdef USE_NAVIGATION Navigation_runSM(); #endif #ifdef USE_XBEE Xbee_runSM(); #endif }