int main(void) { // Initialize the UART,Timers, and I2C1v Board_init(); Board_configure(USE_SERIAL | USE_LCD | USE_TIMER); dbprint("Starting encoders...\n"); I2C_init(I2C_ID, I2C_CLOCK_FREQ); Encoder_init(); Interface_init(); Timer_new(TIMER_TEST, PRINT_DELAY ); while (!Timer_isExpired(TIMER_TEST)) { Encoder_runSM(); } // Not working? //Encoder_setZeroPitch(); //Encoder_setZeroYaw(); dbprint("Encoders initialized.\n"); DELAY(STARTUP_DELAY) Timer_new(TIMER_TEST, PRINT_DELAY ); bool clearedCalibrateMessage = TRUE; Timer_new(TIMER_TEST2,CALIBRATE_HOLD_DELAY); //Interface_waitLightOnTimer(CALIBRATE_HOLD_DELAY); Interface_waitLightOnTimer(CALIBRATE_HOLD_DELAY); LCD_setPosition(0,0); dbprint("Encoders:\n"); while(1) { if (Timer_isExpired(TIMER_TEST)) { LCD_setPosition(1,0); dbprint(" P=%.1f,\n Y=%.1f\n",Encoder_getPitch(), Encoder_getYaw()); Timer_new(TIMER_TEST, PRINT_DELAY ); } if (Interface_isOkPressed() && Timer_isExpired(TIMER_TEST2)) { Encoder_setZeroPitch(); Encoder_setZeroYaw(); LCD_setPosition(3,0); dbprint("Zeroed encoders.\n"); clearedCalibrateMessage = FALSE; Interface_waitLightOnTimer(CALIBRATE_HOLD_DELAY); Interface_readyLightOff(); Timer_new(TIMER_TEST2,CALIBRATE_HOLD_DELAY); } if (Timer_isExpired(TIMER_TEST2) && !clearedCalibrateMessage) { clearedCalibrateMessage = TRUE; LCD_setPosition(3,0); dbprint(" "); Interface_readyLightOn(); } Encoder_runSM(); Interface_runSM(); } return (SUCCESS); }
int main(void) { Board_init(); Serial_init(); LCD_init(); Timer_init(); printf("Testing the LCD display.\nYou should see: Hello World!\n"); LCD_writeString("Hello World!\nLine2\nLine3\nLine4\n"); Timer_new(TIMER_TEST,WRITE_DELAY); while (!Timer_isExpired(TIMER_TEST)) { asm("nop"); } printf("Overwriting line 1\n"); LCD_writeString("Testing overtop line1.\n"); Timer_new(TIMER_TEST,WRITE_DELAY); while (!Timer_isExpired(TIMER_TEST)) { asm("nop"); } printf("Clearing Display\n"); LCD_clearDisplay(); Timer_new(TIMER_TEST,WRITE_DELAY); while (!Timer_isExpired(TIMER_TEST)) { asm("nop"); } LCD_writeString("Here.we.go\n"); LCD_writeString("CLEARED!\n"); LCD_writeString("CLEARED2!\n"); LCD_writeString("CLEARED3!\n"); Timer_new(TIMER_TEST,WRITE_DELAY); while (!Timer_isExpired(TIMER_TEST)) { asm("nop"); } LCD_clearDisplay(); LCD_writeString("N: E: \n"); char tmp[10]; LCD_setPosition(0,3); sprintf(tmp,"%.2f",12.5f); LCD_writeString(tmp); LCD_setPosition(0,13); sprintf(tmp,"%.2f",-0.32f); LCD_writeString(tmp); return SUCCESS; }
int main(){ //Initializations Board_init(); Serial_init(); Timer_init(); if (Drive_init() != SUCCESS) { printf("Failed to initialize drive module.\n"); } I2C_init(TILT_COMPASS_I2C_ID, I2C_CLOCK_FREQ); if (TiltCompass_init() != SUCCESS) { printf("Failed to initialize tilt compass module.\n"); } ENABLE_OUT_TRIS = OUTPUT; while (1) { printf("Driving north at full speed.\n"); Timer_new(TIMER_TEST, COMMAND_DELAY); Drive_forwardHeading(100, 0); while(!Timer_isExpired(TIMER_TEST)) { //wait for finish DELAY(1); Drive_runSM(); TiltCompass_runSM(); } Drive_stop(); Drive_runSM(); TiltCompass_runSM(); printf("Driving south at full speed.\n"); Timer_new(TIMER_TEST, COMMAND_DELAY); Drive_forwardHeading(100, 180); while(!Timer_isExpired(TIMER_TEST)) { //wait for finish DELAY(1); Drive_runSM(); TiltCompass_runSM(); } Drive_stop(); Drive_runSM(); TiltCompass_runSM(); delayMillisecond(COMMAND_DELAY); printf("Waiting for retry...\n"); } return SUCCESS; }
void Accelerometer_runSM() { if (Timer_isExpired(TIMER_ACCELEROMETER)) { updateReadings(); Timer_new(TIMER_ACCELEROMETER, UPDATE_DELAY); } }
int main(void) { //initializations Board_init(); Board_configure(USE_SERIAL | USE_LCD | USE_TIMER); Interface_init(); //I2C_init(I2C_ID, I2C_CLOCK_FREQ); dbprint("Interface online.\n"); DELAY(STARTUP_DELAY); Timer_new(TIMER_TEST, DEBUG_PRINT_DELAY); //cycle and check if buttons are pressed, if so, turn light on for 3 seconds while(1) { #ifdef DO_STUFF //check to see which button is pressed if(Timer_isExpired(TIMER_TEST)) { LCD_setPosition(0,0); dbprint("Ok=%d, C=%d, R=%d, S=%x,\n Reset=%x, SS=%x\n", Interface_isOkPressed(), Interface_isCancelPressed(), Interface_isRescuePressed(), Interface_isStopPressed(), Interface_isResetPressed(), Interface_isSetStationPressed()); Timer_new(TIMER_TEST, DEBUG_PRINT_DELAY); } Interface_runSM(); #endif } return SUCCESS; }
/********************************************************************** * Function: doStationKeepSM * @return None * @remark Steps into the station keeping state machine. **********************************************************************/ static void doStationKeepSM() { switch (subState) { case STATE_STATIONKEEP_RETURN: // Driving to the station if (event.flags.navigationDone) { subState = STATE_STATIONKEEP_IDLE; Timer_new(TIMER_STATIONKEEP,STATION_KEEP_DELAY); DBPRINT("Arrived at station.\n"); } // TODO obstacle detection break; case STATE_STATIONKEEP_IDLE: // Wait to float away from the station if (Timer_isExpired(TIMER_STATIONKEEP)) { // Check if we floated too far away from the station if (Navigation_getLocalDistance(&nedStation) > STATION_TOLERANCE_MAX) { startStationKeepSM(); // return to station return; } Timer_new(TIMER_STATIONKEEP, STATION_KEEP_DELAY); } break; } if (event.flags.navigationError) { setError((error_t)Navigation_getError()); } }
/********************************************************************** * Function: doSetOriginSM * @return None * @remark Steps into the set origin state machine. **********************************************************************/ static void doSetOriginSM() { // Waiting for origin message from command center if (event.flags.haveSetOriginMessage) { GeocentricCoordinate ecefOrigin; ecefOrigin.x = Mavlink_newMessage.gpsGeocentricData.x; ecefOrigin.y = Mavlink_newMessage.gpsGeocentricData.y; ecefOrigin.z = Mavlink_newMessage.gpsGeocentricData.z; Navigation_setOrigin(&ecefOrigin); handleAcknowledgement(); haveOrigin = TRUE; event.flags.setOriginDone = TRUE; DBPRINT("Set new origin: X=%.1f, Y=%.1f, Z=%.1f\n", ecefOrigin.x, ecefOrigin.y, ecefOrigin.z); } else { // Resend request if timer expires if (Timer_isExpired(TIMER_SETORIGIN)) { // Resend request origin if timed out if (resendMessageCount >= RESEND_MESSAGE_LIMIT) { // Sent too many times setError(ERROR_NO_ORIGIN); return; } else { DBPRINTF("Resending origin request.\n"); Mavlink_sendRequestOrigin(NO_ACK); // just want message Timer_new(TIMER_SETORIGIN, RESEND_MESSAGE_DELAY); resendMessageCount++; } } // timer expired } }
int main(){ Board_init(); Board_configure(USE_TIMER); DELAY(10); DBPRINT("Starting XBee...\n"); if (Xbee_init(XBEE_UART_ID) != SUCCESS) { DBPRINT("Failed XBee init.\n"); return FAILURE; } DBPRINT("XBee initialized.\n"); DELAY(STARTUP_DELAY); Timer_new(TIMER_TEST, PRINT_DELAY); unsigned long int sent = 0; unsigned long int got = 0; while(1){ Xbee_runSM(); if (Timer_isExpired(TIMER_TEST)) { //dbprint("XBee: Sent=%ld, Got=%ld\n", ++sent, got); Mavlink_sendRequestOrigin(); Timer_new(TIMER_TEST, PRINT_DELAY); } if (Mavlink_hasNewMessage()) { ++got; } } return SUCCESS; }
/********************************************************************** * Function: doHeartbeatMessage * @return None. * @remark Occasionally sends a heartbeat message to the ComPAS. * @author David Goodman * @date 2013.05.04 **********************************************************************/ static void doHeartbeatMessage() { if (Timer_isExpired(TIMER_HEARTBEAT)) { Mavlink_sendHeartbeat(); Timer_new(TIMER_HEARTBEAT, HEARTBEAT_SEND_DELAY); } }
int main(void) { // Initialize the UART,Timers, and I2C1v Board_init(); Board_configure(USE_SERIAL | USE_LCD | USE_TIMER); dbprint("Starting encoders...\n"); I2C_init(ENCODER_I2C_ID, I2C_CLOCK_FREQ); if (Encoder_init() != SUCCESS) { dbprint("Failed init. encoder\n"); return FAILURE; } Timer_new(TIMER_TEST, PRINT_DELAY ); while (!Timer_isExpired(TIMER_TEST)) { Encoder_runSM(); } // Not working? //Encoder_setZeroPitch(); //Encoder_setZeroYaw(); dbprint("Encoders initialized.\n"); DELAY(STARTUP_DELAY); Timer_new(TIMER_TEST, PRINT_DELAY ); LCD_setPosition(0,0); dbprint("Encoders:\n"); while(1){ if (Timer_isExpired(TIMER_TEST)) { LCD_setPosition(1,0); dbprint(" P=%.1f,\n Y=%.1f\n",Encoder_getPitch(), Encoder_getYaw()); /*dbprint("Encoders:\n P=%d,\n Y=%d\n", readDevice(SLAVE_PITCH_READ_ADDRESS,SLAVE_PITCH_WRITE_ADDRESS), readDevice(SLAVE_YAW_READ_ADDRESS,SLAVE_YAW_WRITE_ADDRESS));*/ /*dbprint("Encoders:\n P=%.1f,\n Y=%.1f\n", readDevice(SLAVE_PITCH_READ_ADDRESS,SLAVE_PITCH_WRITE_ADDRESS) * DEGREE_PER_NUMBER, readDevice(SLAVE_YAW_READ_ADDRESS,SLAVE_YAW_WRITE_ADDRESS) * DEGREE_PER_NUMBER);*/ Timer_new(TIMER_TEST, PRINT_DELAY ); } Encoder_runSM(); } return (SUCCESS); }
/********************************************************************** * Function: doBarometerUpdate * @return None. * @remark Sends barometer data such as temperature and altitude if * the timer expired. * @author David Goodman * @date 2013.05.04 **********************************************************************/ static void doBarometerUpdate() { if (Timer_isExpired(TIMER_BAROMETER_SEND)) { Mavlink_sendBarometerData(Barometer_getTemperature(), Barometer_getAltitude()); Timer_new(TIMER_BAROMETER_SEND, BAROMETER_SEND_DELAY); } }
/********************************************************************** * Function: Drive_runSM * @return None. * @remark Executes a cycle of the drive system state machine. * @author David Goodman * @date 2013.03.27 **********************************************************************/ void Drive_runSM() { switch (state) { case STATE_IDLE: // Do nothing break; case STATE_DRIVE: // Just driving, do nothing break; case STATE_TRACK: if (Timer_isExpired(TIMER_DRIVE)) { updateRudder(); Timer_new(TIMER_DRIVE, TRACK_UPDATE_DELAY); } break; } // switch }
int main() { ENABLE_OUT_TRIS = OUTPUT; //Set Enable output pin to be an output, fed to the AND gates ENABLE_OUT_LAT = MICRO; //Initialize control to that of Microcontroller Board_init(); Serial_init(); Timer_init(); Drive_init(); I2C_init(I2C_BUS_ID, I2C_CLOCK_FREQ); TiltCompass_init(); printf("Boat initialized.\n"); Drive_stop(); DELAY(5); #ifdef MOTOR_TEST Drive_pivot(0); #endif #ifdef RUDDER_TEST Drive_forwardHeading(0.0, desiredHeading); #endif int i = 0; int velocity[] = {1600, 1600, 1600, 1600, 1600}; velocityPulse = velocity[i]; Timer_new(TIMER_TEST,FINISH_DELAY); while (1) { if (Timer_isExpired(TIMER_TEST)) { i++; if (i == 5){ i = 0; } velocityPulse = velocity[i]; printf("CURRENT VELOCITY: %d\n\n",velocityPulse); Timer_new(TIMER_TEST,FINISH_DELAY); } Drive_runSM(); TiltCompass_runSM(); } Drive_stop(); return SUCCESS; }
/********************************************************************** * Function: doGpsCorrectionUpdate * @return None. * @remark Receives GPS correction data and applies it to the navigation * module, or turns it off if no new message were received. * @author David Goodman * @date 2013.05.05 **********************************************************************/ static void gpsCorrectionUpdate() { if (event.flags.haveGeocentricErrorMessage) { GeocentricCoordinate ecefError; ecefError.x = Mavlink_newMessage.gpsGeocentricData.x; ecefError.y = Mavlink_newMessage.gpsGeocentricData.y; ecefError.z = Mavlink_newMessage.gpsGeocentricData.z; Navigation_setGeocentricError(&ecefError); if (!Navigation_isUsingErrorCorrection()) DBPRINT("Error corrections enabled.\n"); Navigation_enableErrorCorrection(); Timer_new(TIMER_GPS_CORRECTION_LOST, GPS_CORRECTION_LOST_DELAY); } else if (Timer_isExpired(TIMER_GPS_CORRECTION_LOST)) { // Disable error corrections Navigation_disableErrorCorrection(); DBPRINT("Error corrections disabled due to telemetry timeout.\n"); } }
void Thermal_runSM() { if (Timer_isExpired(TIMER_THERMAL)) { #ifdef DEBUG printf("Reading sensor...\n"); #endif if(count == 0){ readChipTemp(); calculateChipTemp(); } count++; if(count >= 16){ count = 0; } readPixelValue(); readCPixelValue(); calculateIRTemp(); Timer_new(TIMER_THERMAL,READ_DELAY); } }
int main(void) { //initializations Board_init(); Serial_init(); Timer_init(); LCD_init(); Interface_init(); printf("INITIALIZATIONS COMPLETE\n"); LCD_writeString("Interface online.\n"); Timer_new(TIMER_TEST, 100); while (1) { if (Timer_isExpired(TIMER_TEST)) { printf("%X -- C:%X F:%X\n",Interface_isOkPressed(), buttonCount.okay, buttonPressed.flag.okay); Timer_new(TIMER_TEST, 100); } Interface_runSM(); } }
int main(void) { // Initialize the UART,Timers, and I2C1 //int row = 0; Board_init(); Timer_init(); Thermal_init(); Serial_init(); /*Timer_new(TIMER_TEST,100); while(!Timer_isExpired(TIMER_TEST);*/ #ifdef DEBUG_TEST2 printf("Initialized Thermal module.\n"); #endif uint8_t row = 0, col = 0; Timer_new(TIMER_TEST,PRINT_DELAY); while(1) { if (Timer_isExpired(TIMER_TEST)) { #ifdef DEBUG_TEST2 printf("Sending thermal data...\n"); #endif sendSerial32(0xFFFF1234); for(col = 0; col < TOTAL_PIXEL_COLS; col++) { for(row = 0; row < TOTAL_PIXEL_ROWS; row++){ uint8_t pixel = row + (col * TOTAL_PIXEL_ROWS); sendSerialFloat(finalPixelTempF[pixel]); } sendSerial32(END_COL_SEQUENCE); } #ifdef DEBUG_TEST2 printf("\nFinished sending thermal data.\n"); #endif Timer_new(TIMER_TEST,PRINT_DELAY); } // Timer_isExpired Thermal_runSM(); } // while }
BOOL isZeroPressed() { // Start lock press timer if pressed if (!readZeroButton()) { if (zeroTimerStarted) zeroTimerStarted = FALSE; //printf("Zero released.\n"); zeroPressed = FALSE; } else if (!zeroTimerStarted && readZeroButton()) { Timer_new(TIMER_BUTTONS, BUTTON_DELAY); zeroTimerStarted = TRUE; zeroPressed = FALSE; //printf("Zero timer started.\n"); } else if (Timer_isExpired(TIMER_BUTTONS)) { zeroPressed = TRUE; //printf("Zero on.\n"); } return zeroPressed; }
int main(){ Board_init(); Board_configure(USE_SERIAL | USE_TIMER); //Board_configure(USE_LCD | USE_TIMER); DELAY(10); dbprint("Starting XBee...\n"); if (Xbee_init(XBEE_UART_ID) != SUCCESS) { dbprint("Failed XBee init.\n"); return FAILURE; } dbprint("XBee initialized.\n"); DELAY(STARTUP_DELAY); LCD_setPosition(0,0); Timer_new(TIMER_TEST, PRINT_DELAY); unsigned long int sent = 0; unsigned long int got = 0; LCD_clearDisplay(); LCD_setPosition(2,0); dbprint("Received: %ld", got); while(1){ Xbee_runSM(); if (Timer_isExpired(TIMER_TEST)) { LCD_setPosition(0,0); dbprint("Sent XBee msg.\nSent: %ld\n", ++sent); Mavlink_sendSaveStation(WANT_ACK); Timer_new(TIMER_TEST, PRINT_DELAY); } if (Mavlink_hasNewMessage()) { LCD_setPosition(2,0); dbprint("Received: %ld", ++got); } } return SUCCESS; }
BOOL isLockPressed() { // Start lock press timer if pressed if (!readLockButton()) { if (lockTimerStarted) lockTimerStarted = FALSE; //printf("Lock released.\n"); lockPressed = FALSE; } else if (!lockTimerStarted && readLockButton()) { Timer_new(TIMER_BUTTONS, BUTTON_DELAY); lockTimerStarted = TRUE; lockPressed = FALSE; //printf("Lock timer started.\n"); } else if (Timer_isExpired(TIMER_BUTTONS)) { lockPressed = TRUE; //printf("Lock on.\n"); } return lockPressed; }
int main(void) { //initializations Board_init(); Board_configure(USE_SERIAL | USE_LCD | USE_TIMER); Interface_init(); DELAY(5); dbprint("Interface online.\n"); Timer_new(TIMER_TEST, LIGHT_ON_DELAY); state = 1; //cycle and check if buttons are pressed, if so, turn light on for 3 seconds while(1) { switch (state) { case READY_LIGHT: if (Timer_isExpired(TIMER_TEST)) { Interface_clearAll(); Interface_readyLightOn(); LCD_setPosition(1,0); dbprint("Ready light. \n"); Timer_new(TIMER_TEST, LIGHT_ON_DELAY); state++; } break; case READY_LIGHT_TIMER: if (Timer_isExpired(TIMER_TEST)) { Interface_clearAll(); Interface_readyLightOnTimer(LIGHT_ON_TIMER_DELAY); LCD_setPosition(1,0); dbprint("Ready light timer.\n"); Timer_new(TIMER_TEST, LIGHT_ON_DELAY); state++; } break; case WAIT_LIGHT: if (Timer_isExpired(TIMER_TEST)) { Interface_clearAll(); Interface_waitLightOn(); LCD_setPosition(0,0); dbprint("Wait light. \n"); Timer_new(TIMER_TEST, LIGHT_ON_DELAY); state++; } break; case WAIT_LIGHT_TIMER: if (Timer_isExpired(TIMER_TEST)) { Interface_clearAll(); Interface_waitLightOnTimer(LIGHT_ON_TIMER_DELAY); LCD_setPosition(0,0); dbprint("Wait light timer. \n"); Timer_new(TIMER_TEST, LIGHT_ON_DELAY); state++; } break; case ERROR_LIGHT: if (Timer_isExpired(TIMER_TEST)) { Interface_clearAll(); Interface_errorLightOn(); LCD_setPosition(0,0); dbprint("Error light. \n"); Timer_new(TIMER_TEST, LIGHT_ON_DELAY); state++; } break; case ERROR_LIGHT_TIMER: if (Timer_isExpired(TIMER_TEST)) { Interface_clearAll(); Interface_errorLightOnTimer(LIGHT_ON_TIMER_DELAY); LCD_setPosition(0,0); dbprint("Error light timer. \n"); Timer_new(TIMER_TEST, LIGHT_ON_DELAY); state++; } break; case FRONT_CALIB_LIGHT: if (Timer_isExpired(TIMER_TEST)) { Interface_clearAll(); CALIBRATE_FRONT_LED = ON; LCD_setPosition(0,0); dbprint("Front calib. light. \n"); Timer_new(TIMER_TEST, LIGHT_ON_DELAY); state++; } break; case BACK_CALIB_LIGHT: if (Timer_isExpired(TIMER_TEST)) { Interface_clearAll(); CALIBRATE_BACK_LED = ON; LCD_setPosition(0,0); dbprint("Back calib. light. \n"); Timer_new(TIMER_TEST, LIGHT_ON_DELAY); state = 1; } break; } // switch Interface_runSM(); } return SUCCESS; }
/********************************************************************** * Function: Interface_showMessage * @param message_t you want to display * @return None. * @remark Message will be printed out to the screen **********************************************************************/ void Interface_showMessage(message_t msgCode){ if ( !Timer_isExpired(TIMER_LCD_HOLD) && !Timer_isActive(TIMER_LCD_HOLD)) showMessage(msgCode); else nextMsgCode = msgCode; }
void delayMillisecond(int ms) { Timer_new(TIMER_DELAY, ms); while (!Timer_isExpired(TIMER_DELAY)) asm("nop"); }
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); }
/********************************************************************** * Function: Interface_runSM * @param None. * @return None. * @remark Must be called every cycle of the Command Center, checks the * timers and reacts. **********************************************************************/ void Interface_runSM(){ if (Timer_isExpired(TIMER_LIGHT_HOLD)) { timerLightOffFunction(); Timer_clear(TIMER_LIGHT_HOLD); } if (Timer_isExpired(TIMER_LCD_HOLD)) { Interface_clearDisplay(); if (nextMsgCode != NO_MESSAGE) { showMessage(nextMsgCode); nextMsgCode = NO_MESSAGE; } Timer_clear(TIMER_LCD_HOLD); } //printf("HERE! "); if(Timer_isExpired(TIMER_INTERFACE)) { #if NUMBER_OF_TIMES_TO_CHECK > 1 if(OKAY_BUTTON == PRESSED) buttonCount.okay += 1; if(CANCEL_BUTTON == PRESSED) buttonCount.cancel += 1; if(STOP_BUTTON == PRESSED) buttonCount.stop += 1; if(RESCUE_BUTTON == PRESSED) buttonCount.rescue += 1; if(SETSTATION_BUTTON == PRESSED) buttonCount.setStation += 1; if (RESET_BUTTON == PRESSED) buttonCount.reset += 1; buttonReadCount++; if(buttonReadCount >= NUMBER_OF_TIMES_TO_CHECK){ buttonReadCount = 0; // Clear button flag buttonPressed.bytes[0] = false; if(buttonCount.okay >= MINIMUM_POSITIVES) buttonPressed.flag.okay = true; if(buttonCount.cancel >= MINIMUM_POSITIVES) buttonPressed.flag.cancel = true; if(buttonCount.stop >= MINIMUM_POSITIVES) buttonPressed.flag.stop = true; if(buttonCount.rescue >= MINIMUM_POSITIVES) buttonPressed.flag.rescue = true; if(buttonCount.setStation >= MINIMUM_POSITIVES) buttonPressed.flag.setStation = true; if (buttonCount.reset >= MINIMUM_POSITIVES) buttonPressed.flag.reset = TRUE; // Reset button counts buttonCount.okay = 0; buttonCount.cancel = 0; buttonCount.stop = 0; buttonCount.rescue = 0; buttonCount.setStation = 0; buttonCount.reset = 0; } #else buttonPressed.flag.okay = (OKAY_BUTTON == PRESSED); buttonPressed.flag.cancel = (CANCEL_BUTTON == PRESSED); buttonPressed.flag.stop = (STOP_BUTTON == PRESSED); buttonPressed.flag.rescue = (RESCUE_BUTTON == PRESSED); buttonPressed.flag.setStation = (SETSTATION_BUTTON == PRESSED); buttonPressed.flag.reset = (RESET_BUTTON == PRESSED); #endif #ifdef DEBUG_BUTTON_CHECK Interface_waitLightOnTimer(1000); printf("Ok=%d, C=%d, S=%d, R=%d, SS=%d, Reset=%d\n", buttonPressed.flag.okay, buttonPressed.flag.cancel, buttonPressed.flag.stop, buttonPressed.flag.rescue, buttonPressed.flag.setStation, buttonPressed.flag.reset); #endif Timer_new(TIMER_INTERFACE, WAIT_BETWEEN_CHECKS); } // Calibration lights if (usingPitchLights) { READY_LED = OFF; if (Accelerometer_isLevel()) { CALIBRATE_FRONT_LED = ON; CALIBRATE_BACK_LED = ON; READY_LED = ON; } else if (Accelerometer_getX() > Accelerometer_getY()) { CALIBRATE_FRONT_LED = OFF; CALIBRATE_BACK_LED = ON; } else if (Accelerometer_getY() > Accelerometer_getX()) { CALIBRATE_FRONT_LED = ON; CALIBRATE_BACK_LED = OFF; } } else if (usingYawLights) { READY_LED = OFF; #ifdef USE_MAGNETOMETER if (Magnetometer_isNorth() && Accelerometer_isLevel()) { #else if (Accelerometer_isLevel()) { #endif CALIBRATE_FRONT_LED = ON; CALIBRATE_BACK_LED = ON; READY_LED = ON; } else { CALIBRATE_FRONT_LED = OFF; CALIBRATE_BACK_LED = OFF; } } if (Timer_isExpired(TIMER_INTERFACE) || !Timer_isActive(TIMER_INTERFACE)) Timer_new(TIMER_INTERFACE, WAIT_BETWEEN_CHECKS); } /********************************************************************** * Function: Interface_isCancelPressed * @param None. * @return TRUE if the cancel button was pressed. * @remark **********************************************************************/ bool Interface_isCancelPressed(){ return buttonPressed.flag.cancel; } /********************************************************************** * Function: Interface_isOkPressed * @param None. * @return TRUE if the ok button was pressed. * @remark **********************************************************************/ bool Interface_isOkPressed(){ return buttonPressed.flag.okay; } /********************************************************************** * Function: Interface_isStopPressed * @param None. * @return TRUE if the stop button was pressed. * @remark **********************************************************************/ bool Interface_isStopPressed(){ return buttonPressed.flag.stop; } /********************************************************************** * Function: Interface_isRescuePressed * @param None. * @return TRUE if the rescue button was pressed. * @remark **********************************************************************/ bool Interface_isRescuePressed(){ return buttonPressed.flag.rescue; } /********************************************************************** * Function: Interface_isSetStationPressed * @param None. * @return TRUE if the set station button was pressed. * @remark **********************************************************************/ bool Interface_isSetStationPressed(){ return buttonPressed.flag.setStation; } /********************************************************************** * Function: Interface_isResetPressed * @param None. * @return TRUE if the reset button was pressed. * @remark **********************************************************************/ bool Interface_isResetPressed(){ return buttonPressed.flag.reset; } /********************************************************************** * Function: Interface_readyLightOn * @param None. * @return None. * @remark Turns the LED On by **********************************************************************/ void Interface_readyLightOn(){ READY_LED = ON; } /********************************************************************** * Function: Interface_readyLightOff * @param None. * @return None. * @remark Turns the LED off by **********************************************************************/ void Interface_readyLightOff(){ READY_LED = OFF; } /********************************************************************** * Function: Interface_waitLightOn * @param None. * @return None. * @remark Turns the LED On by **********************************************************************/ void Interface_waitLightOn(){ WAIT_LED = ON; } /********************************************************************** * Function: Interface_waitLightOff * @param None. * @return None. * @remark Turns the LED off by **********************************************************************/ void Interface_waitLightOff(){ WAIT_LED = OFF; } /********************************************************************** * Function: Interface_errorLightOn * @param None. * @return None. * @remark Turns the LED On by **********************************************************************/ void Interface_errorLightOn(){ ERROR_LED = ON; } /********************************************************************** * Function: Interface_errorLightOff * @param None. * @return None. * @remark Turns the LED off by **********************************************************************/ void Interface_errorLightOff(){ ERROR_LED = OFF; } /********************************************************************** * Function: Interface_readyLightOnTimer * @param amount of time in ms that you want the light to remain on * @return None. * @remark Turns the LED on for a certain amount of time **********************************************************************/ void Interface_readyLightOnTimer(uint16_t ms){ Timer_new(TIMER_LIGHT_HOLD, ms); timerLightOffFunction = &Interface_readyLightOff; Interface_readyLightOn(); }
int main(){ //Initializations Board_init(); Serial_init(); Timer_init(); Drive_init(); int spd = 10; ENABLE_OUT_TRIS = OUTPUT; //Set Enable output pin to be an output, fed to the AND gates ENABLE_OUT_LAT = MICRO; //Initialize control to that of Microcontroller enum{ MICRO_FORWARD = 0x01, //State where Microcontroller is driving forward MICRO_STOP = 0x02, //State where Micro is driving backwards MICRO_LIMBO = 0x03, RECIEVER_STATE = 0x04, //Reciever has taken over state }test_state; printf("Boat is Initialized\n"); //Initialize the state to begin at forward test_state = MICRO_FORWARD; Drive_stop(); Timer_new(TIMER_TEST,RECIEVER_DELAY); int state_flag = 0; Override_init(); while(1){ switch(test_state){ case MICRO_FORWARD: printf("STATE: MICRO_FORWARD\n\n\n"); Drive_forward(spd); //The input param is a pwm duty cycle percentage that gets translated to a RC Servo time pulse Timer_new(TIMER_TEST2,RECIEVER_DELAY); test_state = MICRO_STOP; break; case MICRO_STOP: if(Timer_isExpired(TIMER_TEST2)){ printf("STATE: MICRO_STOP\n\n\n:"); Drive_stop(); Timer_new(TIMER_TEST2,RECIEVER_DELAY); test_state = MICRO_LIMBO; } break; case MICRO_LIMBO: if(Timer_isExpired(TIMER_TEST2)){ printf("STATE: MICRO_LIMBO\n\n\n"); test_state = MICRO_FORWARD; } Drive_stop(); break; case RECIEVER_STATE: ; break; } //If we got a pulse, control to reciever. if (OVERRIDE_TRIGGERED == TRUE){ printf("Reciever Control\n\n"); Timer_new(TIMER_TEST, 1000); //Set timer that is greater than the pulsewidth of the CH3 signal(54Hz) OVERRIDE_TRIGGERED = FALSE; //Re-init to zero so that we know when our pulse is triggered again. test_state = RECIEVER_STATE; //Set state equal to reciever where we do nothing autonomous ENABLE_OUT_LAT = RECIEVER; //Give control over to Reciever using the enable line INTEnable(INT_CN,1); } if (Timer_isExpired(TIMER_TEST)){ //Reciever gave up control printf("Micro Has Control\n\n"); Timer_clear(TIMER_TEST); //Clear timer so that it doesn't keep registering an expired signal test_state = MICRO_LIMBO; //Set state equal to forward for regular function OVERRIDE_TRIGGERED = FALSE; //Set Override to false to be sure that we don't trigger falsely ENABLE_OUT_LAT = MICRO; //Give Control back to microcontroller using enable line Timer_new(TIMER_TEST2, RECIEVER_DELAY); } } }
int main(){ //Initializations Board_init(); Serial_init(); Timer_init(); Drive_init(); int spd = 10; ENABLE_TRIS = INPUT; //Set the direction of the Enable pin to be an input, a switch will be used for now ENABLE_OUT_TRIS = OUTPUT; //Set Enable output pin to be an output, fed to the AND gates ENABLE_OUT_LAT = MICRO; //Initialize control to that of Microcontroller enum{ MICRO_FORWARD = 0x01, //State where Microcontroller is driving forward MICRO_STOP = 0x02, //State where Micro is driving backwards MICRO_LIMBO = 0x03, RECIEVER_STATE = 0x04, //Reciever has taken over state }test_state; printf("Boat is Initialized\n"); //Initialize the state to begin at forward test_state = MICRO_FORWARD; Drive_stop(); Timer_new(TIMER_TEST,RECIEVER_DELAY); int state_flag = 0; while(1){ if(Timer_isExpired(TIMER_TEST)){ if ((ENABLE_BIT == 1)){ test_state = RECIEVER_STATE; printf("YOU HAVE TRIGGERED THE RECIEVER FOR CONTROL\n\n"); ENABLE_OUT_LAT = RECIEVER; //Set the Enable_out pin that will be routed to the AND gates state_flag = 1; }else if (ENABLE_BIT == 0){ printf("MICRO HAS CONTROL\n\n"); ENABLE_OUT_LAT = MICRO; //setting Enable_out that will be routed to AND gates if(state_flag == 1){ Timer_new(TIMER_TEST2,RECIEVER_DELAY); test_state = MICRO_FORWARD; state_flag = 0; } } Timer_new(TIMER_TEST,RECIEVER_DELAY); } switch(test_state){ case MICRO_FORWARD: printf("STATE: MICRO_FORWARD\n\n\n"); Drive_forward(spd); //The input param is a pwm duty cycle percentage that gets translated to a RC Servo time pulse Timer_new(TIMER_TEST2,RECIEVER_DELAY); test_state = MICRO_STOP; break; case MICRO_STOP: if(Timer_isExpired(TIMER_TEST2)){ printf("STATE: MICRO_STOP\n\n\n:"); Drive_stop(); Timer_new(TIMER_TEST2,RECIEVER_DELAY); test_state = MICRO_LIMBO; } break; case MICRO_LIMBO: if(Timer_isExpired(TIMER_TEST2)){ printf("STATE: MICRO_LIMBO\n\n\n"); test_state = MICRO_FORWARD; } Drive_stop(); break; case RECIEVER_STATE: ; break; } } }
/********************************************************************** * Function: Override_isTriggered * @return TRUE or FALSE if the receiver has come online. * @remark The receiver will only be considered offline (FALSE), if * it is turned off for longer and 2 seconds. * @author David Goodman * @date 2013.04.01 **********************************************************************/ bool Override_isTriggered() { return !Timer_isExpired(TIMER_OVERRIDE); }
int main() { enum{ forward = 0x01, backward = 0x02, idle = 0x03, }drive_state; Board_init(); Serial_init(); Timer_init(); Drive_init(); int spd = 20; //Magnetometer_init(); printf("Boat initialized.\n"); Drive_forward(spd); Timer_new(TIMER_TEST,TEST_DELAY); drive_state = forward; int flag = 0; while (1) { switch(drive_state){ case forward: Drive_forward(spd); if(Timer_isExpired(TIMER_TEST)){ drive_state = idle; Drive_stop(); Timer_new(TIMER_TEST,TEST_DELAY); flag = 0; } break; case idle: Drive_stop(); if(Timer_isExpired(TIMER_TEST)){ if (flag == 0){ drive_state = backward; Drive_backward(spd); }else if (flag == 1){ drive_state = forward; Drive_forward(spd); } Timer_new(TIMER_TEST,TEST_DELAY); } break; case backward: Drive_backward(spd); if(Timer_isExpired(TIMER_TEST)){ drive_state = idle; Drive_stop(); Timer_new(TIMER_TEST,TEST_DELAY); flag = 1; } } Drive_runSM(); } Drive_stop(); Drive_runSM(); return SUCCESS; }