int main(void) { // Initialize the UART,Timers, and I2C1v Board_init(); Serial_init(); I2C_init(MAGNETOMETER_I2C_ID, I2C_CLOCK_FREQ); Magnetometer_init(); while(1){ Magnetometer_runSM(); while(!Serial_isTransmitEmpty()); printf("Angle: %.1f\n", Magnetometer_getDegree()); } return (SUCCESS); }
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); }