/**
* 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
}
int main(void) {
// Initializations
Board_init();
Board_configure(USE_SERIAL | USE_LCD | USE_TIMER);
I2C_init(I2C_ID, I2C_CLOCK_FREQ);
Magnetometer_init();
Interface_init();
DELAY(5);
if (Accelerometer_init() != SUCCESS) {
Interface_errorLightOn();
dbprint("Failed accel. init.\n");
return FAILURE;
}
dbprint("Ready to calibrate.\n");
Interface_readyLightOn();
DELAY(STARTUP_DELAY);
LCD_setPosition(0,0);
dbprint("Please level scope.\n");
Interface_pitchLightsOn();
state = CALIBRATE_PITCH;
Timer_new(TIMER_TEST2, PRINT_DELAY);
bool holdingCalibrate = FALSE;
while (1) {
switch (state) {
case CALIBRATE_PITCH:
if (Accelerometer_isLevel() && !holdingCalibrate) {
holdingCalibrate = TRUE;
Timer_new(TIMER_TEST, CALIBRATE_HOLD_DELAY);
Interface_readyLightOff();
Interface_waitLightOn();
LCD_setPosition(1,0);
dbprint("Hold level...\n");
}
if (Accelerometer_isLevel() && holdingCalibrate && Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(1,0);
dbprint("Pitch done!\n");
Interface_pitchLightsOff();
Interface_readyLightOn();
Interface_waitLightOff();
holdingCalibrate = FALSE;
DELAY(STARTUP_DELAY);
state = CALIBRATE_YAW;
LCD_setPosition(0,0);
dbprint("Turn scope north.\n \n");
Interface_yawLightsOn();
}
else if (!Accelerometer_isLevel() && holdingCalibrate) {
holdingCalibrate = FALSE;
Timer_stop(TIMER_TEST);
LCD_setPosition(1,0);
dbprint(" \n");
Interface_readyLightOn();
Interface_waitLightOff();
}
break;
case CALIBRATE_YAW:
if (Magnetometer_isNorth() && !holdingCalibrate) {
holdingCalibrate = TRUE;
Timer_new(TIMER_TEST, CALIBRATE_HOLD_DELAY);
Interface_readyLightOff();
Interface_waitLightOn();
LCD_setPosition(1,0);
dbprint("Hold north + level..\n");
}
if (Magnetometer_isNorth() && holdingCalibrate && Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(1,0);
dbprint("Yaw done!\n");
Interface_yawLightsOff();
Interface_readyLightOn();
Interface_waitLightOff();
holdingCalibrate = FALSE;
DELAY(STARTUP_DELAY);
return SUCCESS;
}
else if (!Magnetometer_isNorth() && holdingCalibrate) {
holdingCalibrate = FALSE;
Timer_stop(TIMER_TEST);
LCD_setPosition(1,0);
dbprint(" \n");
Interface_readyLightOn();
Interface_waitLightOff();
}
#ifdef DEBUG_MAGNETOMETER
if (Timer_isExpired(TIMER_TEST2)) {
LCD_setPosition(3,0);
dbprint("Mag: %.2f\n",Magnetometer_getHeading());
Timer_new(TIMER_TEST2, PRINT_DELAY);
}
#endif
break;
}
Magnetometer_runSM();
Accelerometer_runSM();
Interface_runSM();
}
return (SUCCESS);
}