/*************************************************************************************************************** SRX1 robot main cpu This robot implements subsumption architecture using Larry Barello's excellent AvrX pre-emptive RTOS. (http://www.barello.net/avrx/) See project web page for more details (http://robotics.no-ip.org) ToDos (in order of priority) ===== - Correct problem with Candle in immediate corner and robot crashing in wall. (enter room deeper before searching??) - Implement a timeout in extinguish so we can abort if both sensors can't agree. - Enable AVOID for extra robustness??? - Have the SCM auto-calibrate right after motion sensor triggers and base is stopped??? In case of error do what??? - Implement basic filtering of motion sensor input (average samples)???? - Implement a way to replace hard-coded distance commands in PLANNER. (ex: Look for presence/absence of walls) - For case #1 (Enter Room #1): Simply Follow right wall 'till the end. OR Follow Right until front sensor reads around 46cm. - For case #2 (To room #3): Go forward until both Left sensors are above threshold. - For case #3 (Enter Room #4): Add a "Travel for x distance" to Follow. - Have ALIGN align take over the responsibility of aligning with candle??? - Modify AVOID to better fit new navigational concepts. ex. backup on front threshold, etc. - Optimize behaviours so they don't keep sending message every time through the loop unecessarily (their status has not changed)! ToTest ====== - Minimize stack sizes. History: ======= - August 25th 2002 / Stephane Gauthier / Genesis - August 26th 2002 / Stephane Gauthier / Got basic PWM working. - August 28th 2002 / Stephane Gauthier / Added interrupts handlers for ext. int #2 & 3 (left and right wheel encoder). - September 2nd 2002 / Stephane Gauthier / modified encoder ISRs to decrease count when going in reverse. (based on current PWM command) - December 21st 2002 / Stephane Gauthier / setup OC2 PWM to replace dead OC1A PWM pin (transparently handled by PWM_L constant) - January 3rt 2003 / Stephane Gauthier / Total redesign of the behaviour communication system and arbitration system using Messages. - January 12th 2003 / Stephane Gauthier / Ported back to STK-300 board with atmega128. - January 13th 2003 / Stephane Gauthier / Implementated handling of dynamic priorities in ARBITRATOR. - January 14th 2003 / Stephane Gauthier / Optimized AVOID so it will only send message to arbitrate when there is a change (and not every cycle). - January 15th 2003 / Stephane Gauthier / Cleaned up EXEC commands a bit. Added Fan command. - January 18th 2003 / Stephane Gauthier / Implemented full quadrature decoding of encoders using a polling method (in TIMER0 ISR). - January 21st 2003 / Stephane Gauthier / Implemented a radically faster quadrature polling algorithm (Big thanks to Larry Barello!). - February 5th 2003 / Stephane Gauthier / Modified gpd_read() so it returns the average reading of a specific number of samples. - February 8th 2003 / Stephane Gauthier / Redesign basic wall following code. Added door frame detection/alignment. - February 9th 2003 / Stephane Gauthier / Implemented a way for ROTATE to pass full resolution commands to arbitrator. - February 9th 2003 / Stephane Gauthier / Implemented SEARCH. - February 9th 2003 / Stephane Gauthier / Implemented PLANNER. - February 10th 2003 / Stephane Gauthier / Implemented a way for PLANNER to enable/disable behaviours (mostly AVOID). - February 10th 2003 / Stephane Gauthier / Fixed/tweaked a few more bugs. Found AVOID to be stealing ROTATE's realtime, temporary fix: suspend AVOID task when not needed. - February 20th 2003 / Stephane Gauthier / Added a "kill" PLANNER and a FOLLOW/ARBITRATE debug outputs toggles to EXEC. - February 24th 2003 / Stephane Gauthier / Tentative Re-design of the navigational stuff to make it more robust. - February 25th 2003 / Stephane Gauthier / Implemented ALIGN: a subset of FOLLOW to staticly align with wall for added robustness. - February 25th 2003 / Stephane Gauthier / Added status reporting to behaviours so PLANNER can implement contigencies handling. - February 26th 2003 / Stephane Gauthier / Added WaitForMotionToStop(). In ROTATE: fully stop before rotating (increases precision). - February 27th 2003 / Stephane Gauthier / Re-organized PLANNER. Moved redundant code to individual functions to reduce code size and increase readability. - March 3rd 2003 / Stephane Gauthier / Implemented Parameter Table in EEPROM with associated functions. Most parameter now tweakable at runtime. - March 4th 2003 / Stephane Gauthier / Implemented EmergencyStop() to prevent PID from decellerating. - March 4th 2003 / Stephane Gauthier / Tweaked EXTINGUISH behaviour to track candle as it moves forward. - March 5th 2003 / Stephane Gauthier / Tweaked EXTINGUISH further. Added SCM auto-calibration option to EXEC. - March 7th 2003 / Stephane Gauthier / Moved SCM calibration start code to seperate SCM_calibrate() function (in HW_drivers.c). Added switch read to calibrate at powerup in PLANNER. - March 8th 2003 / Stephane Gauthier / Finished GO_AT implementation (not the best but good enough for its intended use). - March 8th 2003 / Stephane Gauthier / Refined GO_AT implementation to dynamically main bearing (in conjunction with AVOID). - March 12th 2003 / Stephane Gauthier / Implemented OdometryReset() to MotorTask.c. Added 'Go Home' and 'Reset Odometry' commands to EXEC. - March 17th 2003 / Stephane Gauthier / ALIGN now clears the door automatically when asked to align with door. - March 18th 2003 / Stephane Gauthier / Redone ALIGN to use simpler centered sensors using a global var to track last triggered line sensor. - March 28th 2003 / Stephane Gauthier / Added code to permit Line sensor calibration at startup (via PORTD switches). - April 3rd 2003 / Stephane Gauthier / Commented out the align with door code in PLANNER as it's flaky and awkward. - April 4th 2003 / Stephane Gauthier / Added a "Ignore Door" argument to Forward. Modified Forward() to not ignore when expected door. - April 4th 2003 / Stephane Gauthier / Added a call to ALIGN (w/ wall) in SEARCH to compensate for removal of "door alignment" code. - April 5th 2003 / Stephane Gauthier / Added a "Ignore Door" argument to Follow. Modified Follow() to not ignore when expected door. - April 5th 2003 / Stephane Gauthier / Test runs at Ron's place: Several minor bug fixes and tweaks. - April 7th 2003 / Stephane Gauthier / Have both SCM and motion sensor scan the room in SEARCH for increased redundancy. - April 8th 2003 / Stephane Gauthier / Added code to EXTINGUISH to make it more redundant (rotate left and right until it's out) ****************************************************************************************************************/ #define ENABLE_BIT_DEFINITIONS //#include //#include #include #include // for printf #include #include // for abs() #include #include #include #include "c:/avrx/avrx/avrx.h" #include "hardware.h" #include "I2C.h" #include "serialio.h" #include "generalio.h" #include "HW_drivers.h" #include "text_strings.h" #include "MotorTask.h" #include "scm\scm_cmds.h" // Define all AVRX task here so that all task variables (i.e. Pid, Tcb, etc..) are available in file's scope. AVRX_GCC_TASKDEF(exec, 50, EXEC_PRIORITY); AVRX_GCC_TASKDEF(arbitrate, 40, ARBITRATE_PRIORITY); AVRX_GCC_TASKDEF(follow, 30, FOLLOW_PRIORITY); AVRX_GCC_TASKDEF(goat, 40, GOAT_PRIORITY); AVRX_GCC_TASKDEF(forward, 30, FORWARD_PRIORITY); AVRX_GCC_TASKDEF(rotate, 30, ROTATE_PRIORITY); AVRX_GCC_TASKDEF(avoid, 40, AVOID_PRIORITY); AVRX_GCC_TASKDEF(search, 40, SEARCH_PRIORITY); AVRX_GCC_TASKDEF(extinguish, 30, EXTINGUISH_PRIORITY); AVRX_GCC_TASKDEF(planner, 30, PLANNER_PRIORITY); AVRX_GCC_TASKDEF(align, 30, ALIGN_PRIORITY); AVRX_EXTERNTASK(MotorTask); // // Prototypes // void InitBehaviourTable(void); unsigned char Align( unsigned char type ); unsigned char Forward( unsigned char lenght, unsigned char expected); void Rotate( signed int angle, unsigned char scan ); unsigned char Search(unsigned char side); unsigned char Follow(unsigned char side, unsigned char expected); void GoHome(void); void InitParameterTable(void); void LoadParameterTable(void); void DumpParameterTable(void); void EditParameterTable(unsigned char id); // // Globals // // Imported Odometry Variables extern volatile int iX, iY, iTheta; // Imported from MotorTask.c // Declare the control blocks needed for GLOBAL timers (LOCAL ONES DON'T SEEM TO WANT TO WORK AT ALL!) AVRX_TIMER(exec_timer); AVRX_TIMER(goat_timer); AVRX_TIMER(follow_timer); AVRX_TIMER(avoid_timer); AVRX_TIMER(extinguish_timer); AVRX_TIMER(planner_timer); AVRX_TIMER(align_timer); // Declare semaphores. //AVRX_MUTEX(rotate_mutex); //AVRX_MUTEX(forward_mutex); // EXEC Debug options volatile char velocity_print = FALSE; volatile char arbitrate_debug = FALSE; volatile char follow_debug = FALSE; // Misc Globals unsigned char Room; // Current Room number unsigned char last_line_trigger; // Last triggered line sensors // Others ConfigData E_ParameterTable EEPROM; // Non-Volatile storage copy. (defaults) ConfigData R_ParameterTable; // Runtime copy in SRAM // Subsumption variables BehaviourInfo BehaviourTable[BEHAVIOUR_COUNT]; MessageQueue RotateQueue; // The Rotate message queue MessageQueue ForwardQueue; // The Forward message queue MessageQueue FollowQueue; // The Forward message queue MessageQueue ArbitrateQueue; // The Arbritrator message queue MessageQueue GoAtQueue; // The GoAt message queue MessageQueue ExtinguishQueue; // The Extinguish message queue MessageQueue AvoidQueue; // The Avoid message queue MessageQueue SearchQueue; // The Search message queue MessageQueue AlignQueue; // The Align message queue /****************************************** Timer 0 Overflow Interrupt Handler Prototypical Interrupt handler: . Switch to kernel context . handle interrupt . Update Quadrature Encoders counts . switch back to interrupted context. ******************************************/ AVRX_SIGINT(SIG_OVERFLOW0) { IntProlog(); // Switch to kernel stack/context TCNT0 = TCNT0_INIT; AvrXTimerHandler(); // Call Time queue manager QuadratureDecoder(); // Update encoder counts // Motion Sensor debug if (bit_test(MOTION_SENSE)) bit_set(LED1); else bit_clear(LED1); Epilog(); // Return to tasks } /****************************************** Task - exec ******************************************/ NAKEDFUNC(exec) { unsigned char exit = FALSE; int temp_int; unsigned char temp_char; unsigned char temp_char2; unsigned char index; static RotateMessage RotateCmd; static ForwardMessage ForwardCmd; static FollowMessage FollowCmd; static ArbitrateMessage ArbitrateCmd; static GoAtMessage GoAtCmd; static ExtinguishMessage ExtinguishCmd; static SearchMessage SearchCmd; static AlignMessage AlignCmd; // Reduce code by initializing it once only. ArbitrateCmd.id=EXEC; ArbitrateCmd.info.type = ABSOLUTE; // assuming EXEC doesn't use relative PID commands. while (1) { // Wait for attention code while (GetChar() != EXEC_KEY) ; // Suspend tasks to prevent contention issues. (keep arbitrate task running though) //AvrXSuspend(PID(MotorTask)); //AvrXSuspend(PID(follow)); // Do we need AVOID to back us up here? Not really. // keep in mind that EXEC doesn't reset it to its initial value on exit! //BehaviourTable[AVOID].enable = FALSE; // Get Sumbsumption Priority ArbitrateCmd.info.output_l = POWER(0); ArbitrateCmd.info.output_r = POWER(0); ArbitrateCmd.info.active = TRUE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // Disable printing of velocity by MotorTask velocity_print = FALSE; // Display Welcome Message PrintString(str_exec_login); PrintString(str_exec_login_bar); while (exit == FALSE) { PutCRLF(); PutChar('>'); // Print EXEC prompt switch (_GetChar()) // Get Command (with echo) { // Print Help screen case ('?'): PrintString(str_help1); PrintString(str_help2); PrintString(str_help3); PrintString(str_help4); PrintString(str_help5); PrintString(str_help6); PrintString(str_help7); PrintString(str_help8); PrintString(str_help9); PrintString(str_help10); PrintString(str_help11); PrintString(str_help12); PrintString(str_help13); PrintString(str_help14); PrintString(str_help15); break; // Dump status case ('d'): PrintString(str_exec_login_bar); // Print current robot position. PrintString(str_cartesian_position); PutChar('X'); PutChar('='); PutDecSignedWord(iX); PutChar(' '); PutChar('Y'); PutChar('='); PutDecSignedWord(iY); PutChar(' '); PutChar('@'); PutChar('='); PutDecSignedWord(iTheta); // Display Current Active Behaviour PrintString(str_spacer); PrintString(str_behaviour_status2); PrintString(str_exec); // EXEC PutChar('\t'); PutChar('\t'); if (BehaviourTable[EXEC].enable) PutChar('1'); else PutChar('0'); PutChar('\t'); if (BehaviourTable[EXEC].active) PutChar('1'); else PutChar('0'); PutChar('\t'); PutDecByte(BehaviourTable[EXEC].priority); PutChar('\t'); PutChar('\t'); PutDecSignedWord(BehaviourTable[EXEC].output_l); PutChar(','); PutDecSignedWord(BehaviourTable[EXEC].output_r); PutChar('\t'); if (BehaviourTable[EXEC].type == ABSOLUTE) PutChar('A'); else PutChar('R'); PutCRLF(); PrintString(str_follow); // FOLLOW PutChar('\t'); PutChar('\t'); if (BehaviourTable[FOLLOW].enable) PutChar('1'); else PutChar('0'); PutChar('\t'); if (BehaviourTable[FOLLOW].active) PutChar('1'); else PutChar('0'); PutChar('\t'); PutDecByte(BehaviourTable[FOLLOW].priority); PutChar('\t'); PutChar('\t'); PutDecSignedWord(BehaviourTable[FOLLOW].output_l); PutChar(','); PutDecSignedWord(BehaviourTable[FOLLOW].output_r); PutChar('\t'); if (BehaviourTable[FOLLOW].type == ABSOLUTE) PutChar('A'); else PutChar('R'); PutCRLF(); PrintString(str_avoid); // AVOID PutChar('\t'); PutChar('\t'); if (BehaviourTable[AVOID].enable) PutChar('1'); else PutChar('0'); PutChar('\t'); if (BehaviourTable[AVOID].active) PutChar('1'); else PutChar('0'); PutChar('\t'); PutDecByte(BehaviourTable[AVOID].priority); PutChar('\t'); PutChar('\t'); PutDecSignedWord(BehaviourTable[AVOID].output_l); PutChar(','); PutDecSignedWord(BehaviourTable[AVOID].output_r); PutChar('\t'); if (BehaviourTable[AVOID].type == ABSOLUTE) PutChar('A'); else PutChar('R'); PutCRLF(); PrintString(str_goat); // GOAT PutChar('\t'); PutChar('\t'); if (BehaviourTable[GOAT].enable) PutChar('1'); else PutChar('0'); PutChar('\t'); if (BehaviourTable[GOAT].active) PutChar('1'); else PutChar('0'); PutChar('\t'); PutDecByte(BehaviourTable[GOAT].priority); PutChar('\t'); PutChar('\t'); PutDecSignedWord(BehaviourTable[GOAT].output_l); PutChar(','); PutDecSignedWord(BehaviourTable[GOAT].output_r); PutChar('\t'); if (BehaviourTable[GOAT].type == ABSOLUTE) PutChar('A'); else PutChar('R'); PutCRLF(); PrintString(str_extinguish); // EXTINGUISH PutChar('\t'); if (BehaviourTable[EXTINGUISH].enable) PutChar('1'); else PutChar('0'); PutChar('\t'); if (BehaviourTable[EXTINGUISH].active) PutChar('1'); else PutChar('0'); PutChar('\t'); PutDecByte(BehaviourTable[EXTINGUISH].priority); PutChar('\t'); PutChar('\t'); PutDecSignedWord(BehaviourTable[EXTINGUISH].output_l); PutChar(','); PutDecSignedWord(BehaviourTable[EXTINGUISH].output_r); PutChar('\t'); if (BehaviourTable[EXTINGUISH].type == ABSOLUTE) PutChar('A'); else PutChar('R'); PutCRLF(); PrintString(str_rotate); // ROTATE PutChar('\t'); PutChar('\t'); if (BehaviourTable[ROTATE].enable) PutChar('1'); else PutChar('0'); PutChar('\t'); if (BehaviourTable[ROTATE].active) PutChar('1'); else PutChar('0'); PutChar('\t'); PutDecByte(BehaviourTable[ROTATE].priority); PutChar('\t'); PutChar('\t'); PutDecSignedWord(BehaviourTable[ROTATE].output_l); PutChar(','); PutDecSignedWord(BehaviourTable[ROTATE].output_r); PutChar('\t'); if (BehaviourTable[ROTATE].type == ABSOLUTE) PutChar('A'); else PutChar('R'); PutCRLF(); PrintString(str_align); // ALIGN PutChar('\t'); PutChar('\t'); if (BehaviourTable[ALIGN].enable) PutChar('1'); else PutChar('0'); PutChar('\t'); if (BehaviourTable[ALIGN].active) PutChar('1'); else PutChar('0'); PutChar('\t'); PutDecByte(BehaviourTable[ALIGN].priority); PutChar('\t'); PutChar('\t'); PutDecSignedWord(BehaviourTable[ALIGN].output_l); PutChar(','); PutDecSignedWord(BehaviourTable[ALIGN].output_r); PutChar('\t'); if (BehaviourTable[ALIGN].type == ABSOLUTE) PutChar('A'); else PutChar('R'); PutCRLF(); PrintString(str_forward); // FORWARD PutChar('\t'); PutChar('\t'); if (BehaviourTable[FORWARD].enable) PutChar('1'); else PutChar('0'); PutChar('\t'); if (BehaviourTable[FORWARD].active) PutChar('1'); else PutChar('0'); PutChar('\t'); PutDecByte(BehaviourTable[FORWARD].priority); PutChar('\t'); PutChar('\t'); PutDecSignedWord(BehaviourTable[FORWARD].output_l); PutChar(','); PutDecSignedWord(BehaviourTable[FORWARD].output_r); PutChar('\t'); if (BehaviourTable[FORWARD].type == ABSOLUTE) PutChar('A'); else PutChar('R'); PutCRLF(); PrintString(str_search); // SEARCH PutChar('\t'); PutChar('\t'); if (BehaviourTable[SEARCH].enable) PutChar('1'); else PutChar('0'); PutChar('\t'); if (BehaviourTable[SEARCH].active) PutChar('1'); else PutChar('0'); PutChar('\t'); PutDecByte(BehaviourTable[SEARCH].priority); PutChar('\t'); PutChar('\t'); PutDecSignedWord(BehaviourTable[SEARCH].output_l); PutChar(','); PutDecSignedWord(BehaviourTable[SEARCH].output_r); PutChar('\t'); if (BehaviourTable[SEARCH].type == ABSOLUTE) PutChar('A'); else PutChar('R'); PutCRLF(); PrintString(str_roam); // ROAM PutChar('\t'); PutChar('\t'); if (BehaviourTable[ROAM].enable) PutChar('1'); else PutChar('0'); PutChar('\t'); if (BehaviourTable[ROAM].active) PutChar('1'); else PutChar('0'); PutChar('\t'); PutDecByte(BehaviourTable[ROAM].priority); PutChar('\t'); PutChar('\t'); PutDecSignedWord(BehaviourTable[ROAM].output_l); PutChar(','); PutDecSignedWord(BehaviourTable[ROAM].output_r); PutChar('\t'); if (BehaviourTable[ROAM].type == ABSOLUTE) PutChar('A'); else PutChar('R'); PrintString(str_spacer); // TEMP: Display PID coefficients PutCRLF(); PutChar('P'); PutChar('='); PutDecSignedByte(R_ParameterTable.Kp/K_O); PutChar('.'); PutDecByte(R_ParameterTable.Kp%K_O); PutChar(' '); PutChar('I'); PutChar('='); PutDecSignedByte(R_ParameterTable.Ki/K_O); PutChar('.'); PutDecByte(R_ParameterTable.Ki%K_O); PutChar(' '); PutChar('D'); PutChar('='); PutDecSignedByte(R_ParameterTable.Kd/K_O); PutChar('.'); PutDecByte(R_ParameterTable.Kd%K_O); // TEMP: Display Follow PD coefficients PutCRLF(); PutChar('p'); PutChar('='); PutDecSignedByte(R_ParameterTable.f_p/F_O); PutChar('.'); PutDecByte(R_ParameterTable.f_p%F_O); PutChar(' '); PutChar('d'); PutChar('='); PutDecSignedByte(R_ParameterTable.f_d/F_O); PutChar('.'); PutDecByte(R_ParameterTable.f_d%F_O); PrintString(str_spacer); // Print Line sensor readings PrintString(str_line_sensors); PutChar('L'); PutChar(':'); temp_char = adc_read(LINE_SENSOR_L); if (LINE_SENSOR(temp_char)) PutChar('1'); else PutChar('0'); PutChar(' '); PutChar('('); PutDecByte(temp_char); PutChar(')'); PutChar(' '); PutChar('R'); PutChar(':'); temp_char = adc_read(LINE_SENSOR_R); if (LINE_SENSOR(temp_char)) PutChar('1'); else PutChar('0'); PutChar(' '); PutChar('('); PutDecByte(temp_char); PutChar(')'); PrintString(str_spacer); // Print GP2D12 sensors readings PrintString(str_gpd_readings); PutChar('\t'); PutChar('\t'); PutDecByte(gpd_read(GPD_F)); PutCRLF(); PutChar('\t'); PutChar('\t'); PutChar('\t'); PutDecByte(gpd_read(GPD_L_F)); PutChar('\t'); PutChar('\t'); PutDecByte(gpd_read(GPD_R_F)); PutCRLF(); PutChar('\t'); PutChar('\t'); PutChar('\t'); PutDecByte(gpd_read(GPD_L_B)); PutChar('\t'); PutChar('\t'); PutDecByte(gpd_read(GPD_R_B)); // Display SCM status PrintString(str_spacer); PutCRLF(); PutChar('S'); PutChar('C'); PutChar('M'); PutChar(':'); PutChar(' '); if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS)) { PutChar('('); PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_X)); PutChar(','); PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_Y)); PutChar(')'); } else { PutChar('N'); PutChar('o'); PutChar('n'); PutChar('e'); } // Debug - line sensor last triggered //PutCRLF(); //PutDecWord(last_line_trigger); // Tidy up PrintString(str_exec_login_bar); break; // Stop motors case ('S'): ArbitrateCmd.info.output_l = POWER(0); ArbitrateCmd.info.output_r = POWER(0); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Rotate Right case ('D'): ArbitrateCmd.info.output_r = POWER(-50); ArbitrateCmd.info.output_l = POWER(50); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Rotate Left case ('A'): ArbitrateCmd.info.output_r = POWER(50); ArbitrateCmd.info.output_l = POWER(-50); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Go forward at High speed case ('3'): ArbitrateCmd.info.output_r = POWER(90); ArbitrateCmd.info.output_l = POWER(90); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Go forward at medium speed case ('2'): ArbitrateCmd.info.output_r = POWER(56); ArbitrateCmd.info.output_l = POWER(56); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Go forward at low speed case ('1'): ArbitrateCmd.info.output_r = POWER(36); ArbitrateCmd.info.output_l = POWER(36); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Go Backwards at medium speed case ('5'): ArbitrateCmd.info.output_r = POWER(-56); ArbitrateCmd.info.output_l = POWER(-56); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Go Backwards at low speed case ('4'): ArbitrateCmd.info.output_r = POWER(-35); ArbitrateCmd.info.output_l = POWER(-35); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Read an I/O port case ('r'): PrintString(str_enter_port_letter); temp_char = _GetChar(); switch(temp_char) { case ('A'): case ('a'): PrintString(str_port_read_banner); PutChar('A'); PutChar('='); PutHexByte(inp(PINA)); PrintString(str_port_read_banner2); PutBin(inp(PINA)); break; case ('B'): case ('b'): PrintString(str_port_read_banner); PutChar('B'); PutChar('='); PutHexByte(inp(PINB)); PrintString(str_port_read_banner2); PutBin(inp(PINB)); break; // Special case: This is an output port only so print current output case ('C'): case ('c'): PrintString(str_port_read_banner); PutChar('C'); PutChar('='); PutHexByte(inp(PORTC)); PrintString(str_port_read_banner2); PutBin(inp(PORTC)); break; case ('D'): case ('d'): PrintString(str_port_read_banner); PutChar('D'); PutChar('='); PutHexByte(inp(PIND)); PrintString(str_port_read_banner2); PutBin(inp(PIND)); break; case ('E'): case ('e'): PrintString(str_port_read_banner); PutChar('E'); PutChar('='); PutHexByte(inp(PINE)); PrintString(str_port_read_banner2); PutBin(inp(PINE)); break; } break; // Quit EXEC mode. case ('`'): exit = TRUE; PrintString(str_exec_quit); break; // Software Reset processor (WARM BOOT) case ('!'): PrintString(str_soft_reset); RESET(); break; // I2C Byte Read case ('#'): PrintString(str_enter_address); temp_char = GetHexByte(); PutCRLF(); PrintString(str_enter_element); temp_char2 = GetHexByte(); PutCRLF(); temp_char = i2c_readbyte(temp_char, temp_char2); PutHexByte(temp_char); break; // I2C Byte Write case ('$'): PrintString(str_enter_address); index = GetHexByte(); PutCRLF(); PrintString(str_enter_element); temp_char = GetHexByte(); PutCRLF(); PrintString(str_enter_byte); temp_char2 = GetHexByte(); i2c_writebyte(index, temp_char, temp_char2); break; // I2C Word Read case ('%'): PrintString(str_enter_address); temp_char = GetHexByte(); PutCRLF(); PrintString(str_enter_element); temp_char2 = GetHexByte(); PutCRLF(); temp_int = i2c_readword(temp_char, temp_char2); PutHexWord(temp_int); break; // I2C Word Write case ('^'): PrintString(str_enter_address); index = GetHexByte(); PutCRLF(); PrintString(str_enter_element); temp_char = GetHexByte(); PutCRLF(); PrintString(str_enter_byte); temp_int = GetHexWord(); i2c_writeword(index, temp_char, temp_int); break; // Rotate case ('R'): PutChar(':'); do temp_char = GetChar(); while (temp_char != 'A' && temp_char != 'R'); PutChar(temp_char); if (temp_char == 'R') RotateCmd.type = RELATIVE; else RotateCmd.type = ABSOLUTE; RotateCmd.scan = FALSE; PutChar(':'); temp_int = GetDecSignedWord(); RotateCmd.angle = temp_int; // Temporary disable EXEC's status ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); AvrXWaitMessageAck(&RotateCmd.mcb); // restore our status ArbitrateCmd.info.active = TRUE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Go Forward case ('F'): PutChar(':'); temp_int = GetDecSignedWord(); ForwardCmd.lenght = temp_int; ForwardCmd.ignore_door = FALSE; // Temporary disable EXEC's status ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); AvrXSendMessage(&ForwardQueue, &ForwardCmd.mcb); AvrXWaitMessageAck(&ForwardCmd.mcb); // restore our status ArbitrateCmd.info.active = TRUE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // Print reported result PutChar('='); PutDecByte(ForwardCmd.status); break; // Track/Follow Wall case ('T'): PutChar(':'); do temp_char = GetChar(); while (temp_char != 'L' && temp_char != 'R'); PutChar(temp_char); FollowCmd.ignore_door = LEFT; if (temp_char == 'L') FollowCmd.side = LEFT; else FollowCmd.side = RIGHT; // Temporary disable EXEC's status ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); AvrXSendMessage(&FollowQueue, &FollowCmd.mcb); AvrXWaitMessageAck(&FollowCmd.mcb); // restore our status ArbitrateCmd.info.active = TRUE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // Print FOLLOW's reported result PutChar('='); PutDecByte(FollowCmd.status); break; // Align with Wall/Door case ('n'): PutChar(':'); do temp_char = GetChar(); while (temp_char != 'L' && temp_char != 'R' && temp_char != 'D'); PutChar(temp_char); if (temp_char == 'L') AlignCmd.type = ALIGN_LEFT; else if (temp_char == 'D') AlignCmd.type = ALIGN_DOOR; else AlignCmd.type = ALIGN_RIGHT; // Temporary disable EXEC's status ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); AvrXSendMessage(&AlignQueue, &AlignCmd.mcb); AvrXWaitMessageAck(&AlignCmd.mcb); // restore our status ArbitrateCmd.info.active = TRUE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // Print ALIGN's reported result PutChar('='); PutDecByte(AlignCmd.status); break; // Go At X,Y location case ('G'): PutChar(' '); PutChar('X'); PutChar(':'); GoAtCmd.x = GetDecSignedWord(); PutChar(' '); PutChar('Y'); PutChar(':'); GoAtCmd.y = GetDecSignedWord(); PutChar(' '); PutChar('@'); PutChar(':'); GoAtCmd.theta = GetDecWord(); // Temporary disable EXEC's status ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); AvrXSendMessage(&GoAtQueue, &GoAtCmd.mcb); AvrXWaitMessageAck(&GoAtCmd.mcb); // restore our status ArbitrateCmd.info.active = TRUE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Search Room case ('c'): PutChar(':'); // Which side to scan on? do temp_char = GetChar(); while (temp_char != 'L' && temp_char != 'R'); PutChar(temp_char); if (temp_char == 'L') SearchCmd.side = LEFT; else SearchCmd.side = RIGHT; // Temporary disable EXEC's status ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); AvrXSendMessage(&SearchQueue, &SearchCmd.mcb); AvrXWaitMessageAck(&SearchCmd.mcb); // If candle was found, proceed to extinguish it. if (SearchCmd.status == TRUE) { AvrXSendMessage(&ExtinguishQueue, &ExtinguishCmd.mcb); AvrXWaitMessageAck(&ExtinguishCmd.mcb); } // Print result of search PutChar('='); if (SearchCmd.status == TRUE) PutChar('1'); else PutChar('0'); // restore our status ArbitrateCmd.info.active = TRUE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Fan debug case (':'): bit_toggle(FAN_CTL); break; // Toggle ARBITRATE debug output case ('a'): arbitrate_debug = arbitrate_debug ? FALSE : TRUE; break; // Toggle FOLLOW debug output case ('f'): arbitrate_debug = arbitrate_debug ? FALSE : TRUE; break; // Dump out SCM status & registers. case ('g'): for (temp_char = 0; temp_char < 14; temp_char++) { PutCRLF(); PutDecByte(temp_char); PutChar(':'); PutHexByte(i2c_readbyte(SCM_I2C_ADDR, temp_char+SCM_CMD_COUNT)); } PutCRLF(); PutChar('A'); PutChar(':'); PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_AVERAGE)); PutCRLF(); PutChar('T'); PutChar(':'); PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_THRESHOLD)); PutCRLF(); PutChar('C'); PutChar(':'); PutDecWord((unsigned int)((i2c_readbyte(SCM_I2C_ADDR, SCM_COUNT_HI)<<8) | (i2c_readbyte(SCM_I2C_ADDR, SCM_COUNT_LO)))); PutCRLF(); PutChar('H'); PutChar(':'); PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_PIXEL_MAX)); PutCRLF(); PutChar('L'); PutChar(':'); PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_PIXEL_MIN)); PutCRLF(); PutChar('S'); PutChar(':'); PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_TARGET_MIN_SIZE)); break; // Reinitialize Parameter table in EEPROM (DEFAULTS->EEPROM). case ('I'): InitParameterTable(); break; // Reload Parameter table (EEPROM->SRAM). case ('L'): LoadParameterTable(); break; // Dump Parameter Table case ('P'): DumpParameterTable(); break; // Edit Parameter Table case ('E'): PrintString(str_parameter_edit); temp_char = GetDecWord(); EditParameterTable(temp_char); break; // kill PLANNER for this session case ('k'): AvrXSuspend(PID(planner)); break; // Resurrect PLANNER case ('l'): AvrXRunTask(TCB(planner)); break; // Request SCM auto-calibration case ('x'): SCM_calibrate(TRUE); break; // Go Home! case ('h'): // Temporary disable EXEC's status PutChar(':'); temp_char = GetDecWord(); Room = temp_char; ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); GoHome(); // restore our status ArbitrateCmd.info.active = TRUE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); break; // Reset Odometry case ('o'): ResetOdometry(); break; default: break; } } // Enable printing of velocity by MotorTask velocity_print = TRUE; exit = FALSE; // We've quit, release sumbsumption flag //ArbitrateCmd.info.output = STOP; ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // Resume halted tasks //AvrXResume(PID(MotorTask)); //AvrXResume(PID(follow)); } } /****************************************** Task - PLANNER ******************************************/ NAKEDFUNC(planner) { // Check for SCM calibration request via STK300's PORTD switches. (bit 7) if (bit_test(SCM_CALIBRATE_REQ) == FALSE) { SCM_calibrate(TRUE); } // Check for Line Sensor calibration request via STK300's PORTD switches. (bit 6) if (bit_test(LINE_CALIBRATE_REQ) == FALSE) { Line_calibrate(TRUE); } // Align with Left Wall before waiting for Start signal //Align(ALIGN_LEFT); // Initial delay to give a chance to Motion sensor to stabilize AvrXDelay(&planner_timer, STARTUP_DELAY); AvrXDelay(&planner_timer, STARTUP_DELAY); AvrXDelay(&planner_timer, STARTUP_DELAY); // (this is where the sound detector board loop would go) // Reset Room number var. Room = 0; // // HOME -> ROOM #1 // // Move forward for fixed amount to clear the home circle Forward(FORWARD_CLEAR_HOME_CIRCLE, FORWARD_REPORT_SUCCESS); // follow the left wall until wall ends. Follow(LEFT, FOLLOW_REPORT_SUCCESS); // Go forward 'till obstacle in front at threshold Forward(0, FORWARD_REPORT_OBSTACLE); // Rotate Left 90 degrees Rotate(-90, FALSE); // Align with Right Wall Align(ALIGN_RIGHT); // Move forward for fixed amount Forward(46, FORWARD_REPORT_SUCCESS); // TODO: Replace with a more robust algorithm. (no hard-coded distances!) // Align with Right Wall Align(ALIGN_RIGHT); // Rotate Left 90 degrees Rotate(-90, FALSE); // Go forward 'till door detected Forward(0, FORWARD_REPORT_DOOR); // Align with Door frame //Align(ALIGN_DOOR); // We're in room #1 Room = 1; // Configure SEARCH to search current room. if (Search(RIGHT) == TRUE) GoHome(); // // ROOM #1 -> ROOM #2 // // I guess the candle was not found so align with Right Wall Align(ALIGN_RIGHT); // And go forward until door is found. Forward(0, FORWARD_REPORT_DOOR); // Align with Door frame (and clear it) //Align(ALIGN_DOOR); // Move forward to clear door frame //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); // Move forward 'till wall Forward(0, FORWARD_REPORT_OBSTACLE); // Rotate Left 90 degrees Rotate(-90, FALSE); // Align with Right Wall Align(ALIGN_RIGHT); // Go forward 'till wall detected. Forward(0, FORWARD_REPORT_OBSTACLE); // Align with Left Wall Align(ALIGN_LEFT); // Rotate Right 90 degrees Rotate(+90, FALSE); // follow the left wall until door frame. Follow(LEFT, FOLLOW_REPORT_DOOR); // Align with Door frame //Align(ALIGN_DOOR); // We're in room #2 Room = 2; // Configure SEARCH to search current room. if (Search(RIGHT) == TRUE) GoHome(); // // ROOM #2 -> ROOM #3 // // I guess the candle was not found so exit room by following Right wall. Follow(RIGHT, FOLLOW_REPORT_DOOR); // Align with Door frame (and clear it) //Align(ALIGN_DOOR); // Move forward to clear door frame //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); // Keep following right wall 'till obstacle Follow(RIGHT, FOLLOW_REPORT_OBSTACLE); // Rotate Left 90 degrees Rotate(-90, FALSE); // Align with Right wall Align(ALIGN_RIGHT); // Go forward a fixed amount so we can find left wall Forward(51, FORWARD_REPORT_SUCCESS); // TODO: Add a call to ALIGN to add robustness and give a break to FOLLOW??? // Left wall should now be in range. Follow it. Follow(LEFT, FOLLOW_REPORT_SUCCESS); // Go forward a fixed amount so we can find Right wall Forward(24, FORWARD_REPORT_SUCCESS); // Align with Right Wall Align(ALIGN_RIGHT); // Rotate Left 90 degrees Rotate(-90, FALSE); // Move forward for fixed amount Forward(49, FORWARD_REPORT_SUCCESS); // Left wall should now be in range. Follow it. Follow(LEFT, FOLLOW_REPORT_OBSTACLE); // Rotate Right 90 degrees Rotate(+90, FALSE); // Align with Left Wall Align(ALIGN_LEFT); // Follow Left wall to door Follow(LEFT, FOLLOW_REPORT_DOOR); // Align with door frame //Align(ALIGN_DOOR); // We're in room #3 Room = 3; // Configure SEARCH to search current room. if (Search(RIGHT) == TRUE) GoHome(); // // ROOM #3 -> ROOM #4 // // I guess the candle was not found so exit room by following Right wall. Follow(RIGHT, FOLLOW_REPORT_DOOR); // Align with door frame (and clear it) //Align(ALIGN_DOOR); // Move forward to clear door frame //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); // Keep following right wall 'till obstacle Follow(RIGHT, FOLLOW_REPORT_OBSTACLE); // Rotate Left 90 degrees Rotate(-90, FALSE); // Align with Right wall Align(ALIGN_RIGHT); // Follow right wall 'till the end Follow(RIGHT, FOLLOW_REPORT_SUCCESS); // Move forward 'till obstacle detected Forward(0, FORWARD_REPORT_OBSTACLE); // Rotate Left 90 degrees Rotate(-90, FALSE); // Align with Right wall Align(ALIGN_RIGHT); // Follow Right wall 'till obstacle detected. Follow(RIGHT, FOLLOW_REPORT_SUCCESS); // Move forward till obstacle Forward(0, FORWARD_REPORT_OBSTACLE); // Align with Left wall Align(ALIGN_LEFT); // Rotate Right 90 degrees Rotate(+90, FALSE); // Align with Left wall Align(ALIGN_LEFT); // TODO: Use follow instead and implement a 'distance traveled' option. (darn floor items!) // Move forward to entrance. Forward(48, FORWARD_REPORT_SUCCESS); // Align with Left wall Align(ALIGN_LEFT); // Rotate Right 90 degrees Rotate(+90, FALSE); // Move forward to entrance. Forward(0, FORWARD_REPORT_DOOR); // Align with door frame //Align(ALIGN_DOOR); // We're in room #4 Room = 4; // Configure SEARCH to search current room. if (Search(LEFT) == TRUE) GoHome(); // TODO: Add necessary calls to have robot get back to home and prepare to go over again.. //goto START; // If we get here it means we suck! AvrXSuspend(PID(planner)); } /****************************************** Task - Forward (Move forward/Backwards) ----------------------------- Inputs: lenght argument contains vectorial info (if 0, keep forward until obstacle detected). ******************************************/ NAKEDFUNC(forward) { MessageControlBlock *pForwardCmd; static ArbitrateMessage ArbitrateCmd; // static is required. int iLenght; unsigned char line_sense_l, line_sense_r; unsigned char exit; // Identify ourselves ArbitrateCmd.id = FORWARD; while (1) { pForwardCmd = AvrXWaitMessage(&ForwardQueue); // setup exit = FALSE; ArbitrateCmd.info.active = TRUE; ArbitrateCmd.info.type = ABSOLUTE; last_line_trigger = 0; // Loop Counter = distance / (DIST_TICK * Velocity) iLenght = ((ForwardMessage*)pForwardCmd)->lenght / (DIST_PER_TICK * FORWARD_SPEED); if (iLenght >= 0) // Covers continous case (ex. lenght argument = 0) { ArbitrateCmd.info.output_l = FORWARD_SPEED; ArbitrateCmd.info.output_r = FORWARD_SPEED; } else { ArbitrateCmd.info.output_l = -FORWARD_SPEED; ArbitrateCmd.info.output_r = -FORWARD_SPEED; iLenght = -iLenght; } AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // TODO: Have different speed constants for both cases? if (iLenght == 0) // continuous case { while (exit != TRUE) { SynchWithMotion(); line_sense_l = adc_read(LINE_SENSOR_L); line_sense_r = adc_read(LINE_SENSOR_R); // debug //PutCRLF(); //PutDecWord(line_sense_l); //PutChar('-'); //PutDecWord(line_sense_r); if (gpd_read(GPD_F) < FORWARD_FRONT_THRESHOLD) { ((ForwardMessage*)pForwardCmd)->status = FORWARD_REPORT_OBSTACLE; // report back. exit = TRUE; } else if ((((ForwardMessage*)pForwardCmd)->ignore_door != TRUE) && (LINE_SENSOR(line_sense_l) || LINE_SENSOR(line_sense_r))) { // Keep track which sensor fired first if (LINE_SENSOR(line_sense_l)) last_line_trigger = LINE_SENSOR_LEFT; if (LINE_SENSOR(line_sense_r)) last_line_trigger += LINE_SENSOR_RIGHT; // Special case: if both sensors triggered simultaneously ((ForwardMessage*)pForwardCmd)->status = FORWARD_REPORT_DOOR; // report back. //EmergencyStop(); // Slam the breaks hard! exit = TRUE; } } } else // Fixed case { while (iLenght--) // TODO: Add front sensor reading in here too? SynchWithMotion(); ((ForwardMessage*)pForwardCmd)->status = FORWARD_REPORT_SUCCESS; // report back. } // Stop motion and de-assert ourselves ArbitrateCmd.info.output_l = POWER(0); ArbitrateCmd.info.output_r = POWER(0); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // report back! AvrXAckMessage(pForwardCmd); } } /****************************************** Task - avoid ******************************************/ NAKEDFUNC(avoid) { //MessageControlBlock *pAvoidCmd; static ArbitrateMessage ArbitrateCmd; // static is required. unsigned char state; unsigned char laststate; unsigned char d_front, d_l_f, d_r_f; // Reduce code by initializing it once only. ArbitrateCmd.id=AVOID; ArbitrateCmd.info.type = ABSOLUTE; laststate = FALSE; while (1) { //pAvoidCmd = AvrXWaitMessage(&AvoidQueue); // Synch up with motion control //SynchWithMotion(); d_front = gpd_read(GPD_F); d_l_f = gpd_read(GPD_L_F); d_r_f = gpd_read(GPD_R_F); if ( d_front < AVOID_FRONT_THRESHOLD || d_l_f < AVOID_SIDE_THRESHOLD || d_r_f < AVOID_SIDE_THRESHOLD) { state = TRUE; // To prevent redundant messages ArbitrateCmd.info.active = TRUE; if (d_l_f > d_r_f) { ArbitrateCmd.info.output_r = AVOID_TURN_SPEED; ArbitrateCmd.info.output_l = -AVOID_TURN_SPEED; } else { ArbitrateCmd.info.output_r = -AVOID_TURN_SPEED; ArbitrateCmd.info.output_l = AVOID_TURN_SPEED; } } else { state = FALSE; // To prevent redundant messages ArbitrateCmd.info.active = FALSE; ArbitrateCmd.info.output_l = POWER(0); ArbitrateCmd.info.output_r = POWER(0); } // Update our arbitration status (only if there was any changes since last update) if (state != laststate) { laststate = state; //Update for next loop. AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); } // report back! //AvrXAckMessage(pAvoidCmd); AvrXDelay(&avoid_timer, AVOID_LOOP_DELAY); } } /****************************************** Task - rotate ******************************************/ NAKEDFUNC(rotate) { MessageControlBlock *pRotateCmd; static ArbitrateMessage ArbitrateCmd; // static is required. int iTurn; int sign; // Reduce code by initializing it once only. ArbitrateCmd.id=ROTATE; ArbitrateCmd.info.type = ABSOLUTE; ArbitrateCmd.info.precision = TRUE; // request right to pass non-normalized values to keep precision. while (1) { pRotateCmd = AvrXWaitMessage(&RotateQueue); // Take over ArbitrateCmd.info.active = TRUE; ((RotateMessage*)pRotateCmd)->status = FALSE; // Init just in case. // Make sure we're totally stopped before starting to keep precision high ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = 0; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); WaitForMotionToStop(); // Was requested rotation absolute or relative? if (((RotateMessage*)pRotateCmd)->type == ABSOLUTE) { // Normalize input (force within valid 0-359 range) iTurn = ((RotateMessage*)pRotateCmd)->angle - iTheta; sign = (iTurn > 0) ? 1 : -1; iTurn = abs(iTurn) % 360; // pick shortest route around the circle if (iTurn > 180) { iTurn = 360 - iTurn; sign = -sign; } iTurn = iTurn * sign; } else // RELATIVE iTurn = ((RotateMessage*)pRotateCmd)->angle; #define ANGLE_ADJUST -1 // TODO: Implement a lookup table of pre-calculated angles to gain precision. if (iTurn > 0) // Start new motion { ArbitrateCmd.info.output_l = (ONE_DEG-ANGLE_ADJUST)<<8; ArbitrateCmd.info.output_r = (-ONE_DEG+ANGLE_ADJUST)<<8; } else { ArbitrateCmd.info.output_l = (-ONE_DEG+ANGLE_ADJUST)<<8; ArbitrateCmd.info.output_r = (ONE_DEG-ANGLE_ADJUST)<<8; iTurn = -iTurn; } AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); while (iTurn--) { // Synch up with motion control SynchWithMotion(); // If requested to, check for candle as we rotate if (((RotateMessage*)pRotateCmd)->scan == TRUE) { if (bit_test(MOTION_SENSE) || (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == TRUE)) { ((RotateMessage*)pRotateCmd)->status = TRUE; EmergencyStop(); // Slam the breaks hard! break; // exit loop prematurely. } } } // Make sure we're totally stopped before starting to keep turn precision high ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = 0; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); WaitForMotionToStop(); // We're done here. release. ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // To prevent errors. //WaitForMotionToStop(); // report back! AvrXAckMessage(pRotateCmd); } } /****************************************** Task - follow (wall following) ******************************************/ NAKEDFUNC(follow) { MessageControlBlock *pFollowCmd; static ArbitrateMessage ArbitrateCmd; // static is required. unsigned char d_side_front; unsigned char d_side_back; unsigned char d_front; signed int delta_speed; signed int error; signed int error_last; unsigned char side; unsigned char exit; unsigned char line_sense_r, line_sense_l; // Reduce code by initializing it once only. ArbitrateCmd.id = FOLLOW; ArbitrateCmd.info.type = ABSOLUTE; while (1) { pFollowCmd = AvrXWaitMessage(&FollowQueue); side = ((FollowMessage*)pFollowCmd)->side; last_line_trigger = 0; // Setup error = 0; error_last = 0; delta_speed = 0; exit = FALSE; ArbitrateCmd.info.active = TRUE; while (exit == FALSE) { // Synch up with motion control SynchWithMotion(); // Select which side to work on if (side == LEFT) { d_side_front = gpd_read(GPD_L_F); d_side_back = gpd_read(GPD_L_B); } else { d_side_front = gpd_read(GPD_R_F); d_side_back = gpd_read(GPD_R_B); } d_front = gpd_read(GPD_F); // Debug information if (follow_debug) { PutCRLF(); PutDecByte(d_front); PutChar('-'); PutDecByte(d_side_front); PutChar('-'); PutDecByte(d_side_back); } line_sense_l = adc_read(LINE_SENSOR_L); line_sense_r = adc_read(LINE_SENSOR_R); if (d_front < FOLLOW_FRONT_THRESHOLD) // Obstacle in front? { ((FollowMessage*)pFollowCmd)->status = FOLLOW_REPORT_OBSTACLE; //status report. exit = TRUE; } else // Did we pass a door frame? If yes, report back. if ((((FollowMessage*)pFollowCmd)->ignore_door != TRUE) && ((LINE_SENSOR(line_sense_l) || LINE_SENSOR(line_sense_r)))) { // Keep track which sensor fired first if (LINE_SENSOR(line_sense_l)) last_line_trigger = LINE_SENSOR_LEFT; if (LINE_SENSOR(line_sense_r)) last_line_trigger += LINE_SENSOR_RIGHT; // Special case: if both sensors triggered simultaneously exit = TRUE; ((FollowMessage*)pFollowCmd)->status = FOLLOW_REPORT_DOOR; //status report. } else // Otherwise, follow wall. if ( d_side_front < FOLLOW_SIDE_THRESHOLD && d_side_back < FOLLOW_SIDE_THRESHOLD) { error_last = error; error = (FOLLOW_DISTANCE_FACTOR*(FOLLOW_DISTANCE_SETPOINT - ((d_side_front + d_side_back)/2))) + (FOLLOW_STEERING_FACTOR*(d_side_back-d_side_front)); //delta_speed = ((error * F_P) + ((error-error_last) * F_D))/F_O; delta_speed = ((error * R_ParameterTable.f_p) + ((error-error_last) * R_ParameterTable.f_d))/F_O; #ifdef wall_follow_debug2 PutChar('='); PutDecSignedWord((FOLLOW_DISTANCE_SETPOINT - ((d_side_front + d_side_back)/2))); PutChar('+'); PutDecSignedWord(FOLLOW_STEERING_FACTOR*(d_side_back-d_side_front)); PutChar('='); PutDecSignedWord(error); #endif // Debug Information if (follow_debug) { PutChar(':'); PutDecSignedWord(delta_speed); } if (side == LEFT) { ArbitrateCmd.info.output_l = FOLLOW_FORWARD_SPEED + delta_speed; ArbitrateCmd.info.output_r = FOLLOW_FORWARD_SPEED + (-delta_speed); } else { ArbitrateCmd.info.output_l = FOLLOW_FORWARD_SPEED + (-delta_speed); ArbitrateCmd.info.output_r = FOLLOW_FORWARD_SPEED + delta_speed; } } else // wall as ended so report back. if ( d_side_front >= FOLLOW_SIDE_THRESHOLD && d_side_back < FOLLOW_SIDE_THRESHOLD) { exit = TRUE; ((FollowMessage*)pFollowCmd)->status = FOLLOW_REPORT_SUCCESS; //status report. } else // Unexpected error. report back. { exit = TRUE; ((FollowMessage*)pFollowCmd)->status = FOLLOW_REPORT_FAILURE; //status report. } // Clean up if before exiting. if (exit == TRUE) { ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = 0; ArbitrateCmd.info.active = FALSE; } // Apply our decision! AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); if (exit != TRUE) AvrXDelay(&follow_timer, FOLLOW_LOOP_DELAY); // Give a chance to lower priority tasks to run now. } // We're done for now. Report back to requestor and await next command. AvrXAckMessage(pFollowCmd); } } /****************************************** Task - align (Align with door frame or wall) ******************************************/ NAKEDFUNC(align) { MessageControlBlock *pAlignCmd; static ArbitrateMessage ArbitrateCmd; // static is required. static ForwardMessage ForwardCmd; unsigned char d_side_front; unsigned char d_side_back; signed int delta_speed; signed int error; signed int error_last; unsigned char type; unsigned char exit; unsigned char line_sense_l, line_sense_r; // Reduce code by initializing it once only. ArbitrateCmd.id = ALIGN; ArbitrateCmd.info.type = ABSOLUTE; while (1) { pAlignCmd = AvrXWaitMessage(&AlignQueue); type = ((AlignMessage*)pAlignCmd)->type; error = 0; error_last = 0; delta_speed = 0; exit = FALSE; ArbitrateCmd.info.active = TRUE; ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = 0; ((AlignMessage*)pAlignCmd)->status = ALIGN_REPORT_SUCCESS; if (type == ALIGN_DOOR) // Align with door frame { // Update sensor readings line_sense_l = adc_read(LINE_SENSOR_L); line_sense_r = adc_read(LINE_SENSOR_R); // // Left - Figure out where we're at and act accordingly // // We triggered but went too far if ((last_line_trigger == LINE_SENSOR_LEFT || last_line_trigger == LINE_SENSOR_BOTH) && !LINE_SENSOR(line_sense_l)) { // Backup until sensor in view. ArbitrateCmd.info.output_l = -ALIGN_DOOR_SPEED_LO; ArbitrateCmd.info.output_r = 0; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); } else // Didn't trigger at all. if ((last_line_trigger != LINE_SENSOR_LEFT) && !LINE_SENSOR(line_sense_l)) { // Stopped Short. Move forward till in view. ArbitrateCmd.info.output_l = ALIGN_DOOR_SPEED_LO; ArbitrateCmd.info.output_r = 0; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); } // Correct it! do { // Synch up with motion control SynchWithMotion(); line_sense_l = adc_read(LINE_SENSOR_L); } while (!LINE_SENSOR(line_sense_l)); // We're done with Left side. ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = 0; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // // Right - Figure out where we're at and act accordingly // // We triggered but went too far if ((last_line_trigger == LINE_SENSOR_RIGHT || last_line_trigger == LINE_SENSOR_BOTH) && !LINE_SENSOR(line_sense_r)) { // Backup until sensor in view. ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = -ALIGN_DOOR_SPEED_LO; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); } else // Didn't trigger at all. if ((last_line_trigger != LINE_SENSOR_RIGHT) && !LINE_SENSOR(line_sense_r)) { // Stopped Short. Move forward till in view. ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = ALIGN_DOOR_SPEED_LO; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); } // Correct it! do { // Synch up with motion control SynchWithMotion(); line_sense_r = adc_read(LINE_SENSOR_R); } while (!LINE_SENSOR(line_sense_r)); // We're done with Right side. ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = 0; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // Clear line WaitForMotionToStop(); ForwardCmd.lenght = ALIGN_CLEAR_DISTANCE; AvrXSendMessage(&ForwardQueue, &ForwardCmd.mcb); AvrXWaitMessageAck(&ForwardCmd.mcb); // We're done here. ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = 0; ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); } else // Align with wall { while (exit == FALSE) { // Synch up with motion control SynchWithMotion(); // Select which side to work on if (type == ALIGN_LEFT) { d_side_front = gpd_read(GPD_L_F); d_side_back = gpd_read(GPD_L_B); } else { d_side_front = gpd_read(GPD_R_F); d_side_back = gpd_read(GPD_R_B); } if (d_side_back == d_side_front) // We're aligned { exit = TRUE; ((AlignMessage*)pAlignCmd)->status = ALIGN_REPORT_SUCCESS; //ALIGN_REPORT_SUCCESS } else if ((d_side_front >= ALIGN_SIDE_THRESHOLD) || (d_side_back >= ALIGN_SIDE_THRESHOLD) ) { if (d_side_front >= ALIGN_SIDE_THRESHOLD) // Front sensor blind, move forward. { ArbitrateCmd.info.output_l = -ALIGN_CORRECTION_SPEED; ArbitrateCmd.info.output_r = -ALIGN_CORRECTION_SPEED; } else // Rear sensor blind, move backwards { ArbitrateCmd.info.output_l = ALIGN_CORRECTION_SPEED; ArbitrateCmd.info.output_r = ALIGN_CORRECTION_SPEED; } } else // Align ourselves if ((d_side_front < ALIGN_SIDE_THRESHOLD) && (d_side_back < ALIGN_SIDE_THRESHOLD) ) { error_last = error; error = (ALIGN_STEERING_FACTOR *(d_side_back-d_side_front)); // TODO: Use seperate coefficient for Align and Follow??? delta_speed = ((error * R_ParameterTable.f_p) + ((error-error_last) * R_ParameterTable.f_d))/F_O; if (type == ALIGN_LEFT) { ArbitrateCmd.info.output_l = delta_speed; ArbitrateCmd.info.output_r = -delta_speed; } else { ArbitrateCmd.info.output_l = -delta_speed; ArbitrateCmd.info.output_r = delta_speed; } } else // We can't align, report back the failure. { ((AlignMessage*)pAlignCmd)->status = ALIGN_REPORT_FAILURE; // Error. exit = TRUE; } // Clean up on exit if (exit == TRUE) { ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = 0; ArbitrateCmd.info.active = FALSE; } // Apply our decision! AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); if (exit != FALSE) AvrXDelay(&align_timer, ALIGN_LOOP_DELAY); // Give a chance to lower priority tasks to run now. } } // We're done for now. Report back to requestor and await next command. AvrXAckMessage(pAlignCmd); } } /****************************************** Task - goat ******************************************/ NAKEDFUNC(goat) { MessageControlBlock *pGoAtCmd; static ArbitrateMessage ArbitrateCmd; static RotateMessage RotateCmd; int target_x, target_y; int target_bearing; unsigned char exit; int delta_x; int delta_y; int distance; int bearing; int error; int delta_theta; int sign, iTurn; // Reduce code by initializing it once only. ArbitrateCmd.id = GOAT; ArbitrateCmd.info.type = ABSOLUTE; ArbitrateCmd.info.active = TRUE; //#define GOAT_DEBUG 1 while (1) { exit = FALSE; pGoAtCmd = AvrXWaitMessage(&GoAtQueue); target_x = ((GoAtMessage*)pGoAtCmd)->x; target_y = ((GoAtMessage*)pGoAtCmd)->y; target_bearing = ((GoAtMessage*)pGoAtCmd)->theta; // Point robot in right direction delta_x = target_x - iX; delta_y = target_y - iY; bearing = -atan2(delta_y, delta_x) * RAD2DEG; #ifdef GOAT_DEBUG PutChar('['); PutDecSignedWord(target_x); PutChar(','); PutDecSignedWord(target_y); PutChar(':'); PutDecSignedWord(target_bearing); PutChar('='); PutDecSignedWord(bearing); PutChar(']'); #endif RotateCmd.angle = bearing; RotateCmd.type = ABSOLUTE; AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); AvrXWaitMessageAck(&RotateCmd.mcb); // TODO: Make forward speed inversely proportial to the distance to target. (slow down on approach!) // TODO: Use a simple PD controller to have bearing dynamically adjusted to cooperate with AVOID. while (exit != TRUE) { #ifdef GOAT_DEBUG PutCRLF(); PutChar(':'); PutDecSignedWord(iX); PutChar(':'); PutDecSignedWord(iY); #endif SynchWithMotion(); delta_x = target_x - iX; delta_y = target_y - iY; distance = sqrt(square(delta_x) + square(delta_y)); bearing = -atan2(delta_y, delta_x) * RAD2DEG; if (abs(delta_x) < GOAT_DISTANCE_THRESHOLD && abs(delta_y) < GOAT_DISTANCE_THRESHOLD) { exit = TRUE; } else { // Normalize input (force within valid 0-359 range) iTurn = bearing - iTheta; sign = (iTurn > 0) ? 1 : -1; iTurn = abs(iTurn) % 360; // pick shortest route around the circle if (iTurn > 180) { iTurn = 360 - iTurn; sign = -sign; } delta_theta = iTurn * sign; //delta_theta = iTurn; //(((360+bearing)%360)-iTheta); error = delta_theta * 2 / 10; // calculate bearing error #ifdef GOAT_DEBUG PutCRLF(); PutDecSignedWord(iTheta); PutChar(':'); PutDecSignedWord(bearing); PutChar(':'); PutDecSignedWord(delta_theta); PutChar('='); PutDecSignedWord(error); #endif // And correct... ArbitrateCmd.info.output_l = POWER(50) + (error); ArbitrateCmd.info.output_r = POWER(50) + (-error); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); #ifdef GOAT_DEBUG2 PutCRLF(); PutChar(':'); PutDecSignedWord(delta_x); PutChar(':'); PutDecSignedWord(delta_y); PutChar(':'); PutDecSignedWord(distance); #endif AvrXDelay(&goat_timer, GOAT_LOOP_DELAY); // Give a chance to lower priority tasks to run now. } } // Stop here! ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = 0; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // Point robot in final direction RotateCmd.angle = target_bearing; RotateCmd.type = ABSOLUTE; AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); AvrXWaitMessageAck(&RotateCmd.mcb); // Stop here! ArbitrateCmd.info.output_l = 0; ArbitrateCmd.info.output_r = 0; ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); AvrXAckMessage(pGoAtCmd); } } /****************************************** Task - extinguish ******************************************/ NAKEDFUNC(extinguish) { MessageControlBlock *pExtinguishCmd; static ArbitrateMessage ArbitrateCmd; // static is required. static RotateMessage RotateCmd; unsigned char position; signed char error; unsigned char line_sense_l, line_sense_r; unsigned char count; // Reduce code by initializing it once only. ArbitrateCmd.id=EXTINGUISH; ArbitrateCmd.info.type = ABSOLUTE; //#define EXTINGUISH_DEBUG 1 while (1) { pExtinguishCmd = AvrXWaitMessage(&ExtinguishQueue); ArbitrateCmd.info.active = TRUE; // TODO: Add a way to bail out if SCM can't find candle (darn flaky Motion sensor!)? // Ex.: Start a timer and wait for it to expire. // Keep turning Right until we've got the candle in sight if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == FALSE) { ArbitrateCmd.info.output_l = EXTINGUISH_SCAN_SPEED; ArbitrateCmd.info.output_r = -EXTINGUISH_SCAN_SPEED; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); while (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == FALSE) { SynchWithMotion(); } // "emergency" break (because we can't afford to miss the candle with all the latency) EmergencyStop(); } position = i2c_readbyte(SCM_I2C_ADDR, SCM_X); // Refresh line sensors reading line_sense_l = adc_read(LINE_SENSOR_L); line_sense_r = adc_read(LINE_SENSOR_R); count = EXTINGUISH_APPROACH_COUNT; // Proceed forward towards candle until white line is detected while (count) { SynchWithMotion(); // Refresh line sensors reading line_sense_l = adc_read(LINE_SENSOR_L); line_sense_r = adc_read(LINE_SENSOR_R); // Keep moving forward for a few more paces within white circle. if (LINE_SENSOR(line_sense_l) || LINE_SENSOR(line_sense_r)) { count--; } if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS)) { position = i2c_readbyte(SCM_I2C_ADDR, SCM_X); error = (64-position) * E_P / E_O; PutDecSignedByte(error); PutChar('-'); ArbitrateCmd.info.active = TRUE; ArbitrateCmd.info.output_l = EXTINGUISH_FORWARD_SPEED + (-error); ArbitrateCmd.info.output_r = EXTINGUISH_FORWARD_SPEED + (error); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); } else // if we lose target keep going forward. { // TODO: Change for a turn in the opposite dir. of last recorded turn?? ArbitrateCmd.info.output_l = EXTINGUISH_FORWARD_SPEED; ArbitrateCmd.info.output_r = EXTINGUISH_FORWARD_SPEED; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); } } EmergencyStop(); // Stop, we're almost here! ArbitrateCmd.info.output_l = POWER(0); // Stop motion ArbitrateCmd.info.output_r = POWER(0); //ArbitrateCmd.info.active = TRUE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // Recalibrate SCM SCM_calibrate(FALSE); // recalibrate without saving to EEPROM position = i2c_readbyte(SCM_I2C_ADDR, SCM_X); error = (64-position) * E_P / E_O; // TODO: Add a timeout? while (abs(error) > EXTINGUISH_THRESHOLD_ERROR) { SynchWithMotion(); if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS)) { position = i2c_readbyte(SCM_I2C_ADDR, SCM_X); error = (64-position) * E_P / E_O; #ifdef EXTINGUISH_DEBUG PutDecSignedByte(error); PutChar('-'); #endif ArbitrateCmd.info.active = TRUE; ArbitrateCmd.info.output_l = (-error); ArbitrateCmd.info.output_r = (error); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); } else // if we lose target keep Turning { ArbitrateCmd.info.output_l = -EXTINGUISH_SCAN_SPEED; ArbitrateCmd.info.output_r = EXTINGUISH_SCAN_SPEED; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); } } // Stop, we're here! ArbitrateCmd.info.output_l = POWER(0); // Stop motion ArbitrateCmd.info.output_r = POWER(0); //ArbitrateCmd.info.active = TRUE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // Blow the darn thing out! bit_set(FAN_CTL); // Blow for a fixed minimum time. (just in case fire is weakened by fan and SCM can't see it) AvrXDelay(&extinguish_timer, FAN_DELAY); //while (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == TRUE) // AvrXDelay(&extinguish_timer, MILLISECOND(50)); // Enough! bit_clear(FAN_CTL); // Wait a while and then double-check if candle is out. AvrXDelay(&extinguish_timer, SECOND(1)); count = EXTINGUISH_HUNT_ANGLE; // Try stuff until the darn thing is out! while (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == TRUE) { // If not extinguished, turn left and try again. if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == TRUE) { RotateCmd.type = RELATIVE; RotateCmd.scan = FALSE; RotateCmd.angle = -count; AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); AvrXWaitMessageAck(&RotateCmd.mcb); // Blow the darn thing out! bit_set(FAN_CTL); // Blow for a fixed minimum time. (just in case fire is weakened by fan and SCM can't see it) AvrXDelay(&extinguish_timer, FAN_DELAY); // Enough! bit_clear(FAN_CTL); } // Wait a while and then double-check if candle is out. AvrXDelay(&extinguish_timer, SECOND(1)); // Darn! still not out yet, try the right. if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == TRUE) { RotateCmd.type = RELATIVE; RotateCmd.scan = FALSE; RotateCmd.angle = count*2; AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); AvrXWaitMessageAck(&RotateCmd.mcb); // Blow the darn thing out! bit_set(FAN_CTL); // Blow for a fixed minimum time. (just in case fire is weakened by fan and SCM can't see it) AvrXDelay(&extinguish_timer, FAN_DELAY); // Enough! bit_clear(FAN_CTL); } // Increase the angle count += 1; } // I guess we're done, report back. ((ExtinguishMessage*)pExtinguishCmd)->extinguished = TRUE; // We're done here. release. ArbitrateCmd.info.output_l = POWER(0); // Stop motion ArbitrateCmd.info.output_r = POWER(0); ArbitrateCmd.info.active = FALSE; AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); AvrXWaitMessageAck(&ArbitrateCmd.mcb); // report back! AvrXAckMessage(pExtinguishCmd); } } /****************************************** Task - search ******************************************/ NAKEDFUNC(search) { MessageControlBlock *pSearchCmd; //static ArbitrateMessage ArbitrateCmd; // static is required. static ForwardMessage ForwardCmd; static RotateMessage RotateCmd; static AlignMessage AlignCmd; // Reduce code by initializing it once only. //ArbitrateCmd.id=SEARCH; //ArbitrateCmd.info.type = ABSOLUTE; //ArbitrateCmd.info.output_l = POWER(0); // No motion //ArbitrateCmd.info.output_r = POWER(0); while (1) { pSearchCmd = AvrXWaitMessage(&SearchQueue); // Setup status ((SearchMessage*)pSearchCmd)->status = FALSE; // We should already be aligned with door frame so move forward a fixed amount ForwardCmd.lenght = SEARCH_ENTER_DISTANCE; // Move forward enough so the whole base is inside the room ForwardCmd.ignore_door = TRUE; AvrXSendMessage(&ForwardQueue, &ForwardCmd.mcb); AvrXWaitMessageAck(&ForwardCmd.mcb); // Align with adjacent wall to ensure we do an exact 180 degrees scan. if (((SearchMessage*)pSearchCmd)->side == RIGHT) AlignCmd.type = ALIGN_LEFT; else AlignCmd.type = ALIGN_RIGHT; AvrXSendMessage(&AlignQueue, &AlignCmd.mcb); AvrXWaitMessageAck(&AlignCmd.mcb); // Reset Odometry variables ResetOdometry(); // Scan room and signal PLANNER if candle was found. RotateCmd.type = RELATIVE; RotateCmd.angle = (((SearchMessage*)pSearchCmd)->side == RIGHT) ? +180 : -180; // which way? RotateCmd.scan = TRUE; AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); AvrXWaitMessageAck(&RotateCmd.mcb); // Was candle found? if (RotateCmd.status == TRUE) { PutChar('!'); ((SearchMessage*)pSearchCmd)->status = TRUE; } // We're done here. Report back to PLANNER. //ArbitrateCmd.info.active = FALSE; //AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); //AvrXWaitMessageAck(&ArbitrateCmd.mcb); // report back! AvrXAckMessage(pSearchCmd); } } /****************************************** Task - arbitrate ******************************************/ NAKEDFUNC(arbitrate) { static MessageControlBlock *pArbitrateCmd; // static is required. static ArbitrateMessage ArbitrateCmd; // For dummy message to ourself. unsigned char id; unsigned char priority; unsigned char exit; //#define ARBITRATE_DEBUG 1 //#define ARBITRATE_DEBUG2 1 // Send ourselves a dummy message to have the arbitator kick start arbitrator. ArbitrateCmd.id = ROAM; ArbitrateCmd.info.type = ABSOLUTE; ArbitrateCmd.info.active = FALSE; ArbitrateCmd.info.enable = FALSE; ArbitrateCmd.info.output_l = POWER(56); ArbitrateCmd.info.output_r = POWER(56); AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); while (1) { // Wait for a behaviour to send us us an update. pArbitrateCmd = AvrXWaitMessage(&ArbitrateQueue); // Debug if (arbitrate_debug) { PutChar('('); PutDecByte(((ArbitrateMessage*)pArbitrateCmd)->id); PutChar('>'); } // Check for invalid behaviour id if (((ArbitrateMessage*)pArbitrateCmd)->id > BEHAVIOUR_COUNT) { AvrXAckMessage(pArbitrateCmd); continue; } // Update private table // (NB: the 'enable' parameter is not updated as only the EXEC and PLANNER should be allowed to change it!) BehaviourTable[((ArbitrateMessage*)pArbitrateCmd)->id].active = ((ArbitrateMessage*)pArbitrateCmd)->info.active; BehaviourTable[((ArbitrateMessage*)pArbitrateCmd)->id].output_l = ((ArbitrateMessage*)pArbitrateCmd)->info.output_l; BehaviourTable[((ArbitrateMessage*)pArbitrateCmd)->id].output_r = ((ArbitrateMessage*)pArbitrateCmd)->info.output_r; BehaviourTable[((ArbitrateMessage*)pArbitrateCmd)->id].type = ((ArbitrateMessage*)pArbitrateCmd)->info.type; BehaviourTable[((ArbitrateMessage*)pArbitrateCmd)->id].precision = ((ArbitrateMessage*)pArbitrateCmd)->info.precision; // We have all the information we needed; acknowledge message. AvrXAckMessage(pArbitrateCmd); exit = FALSE; for (priority = 0; priority < BEHAVIOUR_PRIORITY_MAX && exit == FALSE ; priority++) { for (id = 0; id < BEHAVIOUR_COUNT && exit == FALSE ; id++) { if (BehaviourTable[id].enable == TRUE && BehaviourTable[id].priority == priority && BehaviourTable[id].active == TRUE) { motors(&BehaviourTable[id]); // Pass command to PID controller. // debug if (arbitrate_debug) { PutChar(id+'0'); PutChar(')'); } #ifdef ARBITRATE_DEBUG2 PutChar('['); PutDecSignedByte(BehaviourTable[id].output_l); PutChar(','); PutDecSignedByte(BehaviourTable[id].output_r); PutChar(']'); #endif exit = TRUE; // can't I use a simple break command? } } } // if nobody was found to be active make sure we default to STOP! if (exit == FALSE) { // Debug if (arbitrate_debug) PutChar('-'); Left.VelocitySetpoint = 0; Right.VelocitySetpoint = 0; } // Show user we are still alive and well! bit_toggle(HEARTBEAT_LED); } } // Function Name: InitBehaviourTable() // // Description: Initialize Behaviour table to known values. // // void InitBehaviourTable(void) { unsigned char id; for (id = 0; id < BEHAVIOUR_COUNT ; id++) { BehaviourTable[id].active = FALSE; BehaviourTable[id].enable = FALSE; BehaviourTable[id].output_l = 0; BehaviourTable[id].output_r = 0; BehaviourTable[id].type = ABSOLUTE; BehaviourTable[id].precision = FALSE; // default is OFF! } // Those Behaviours should always be enabled BehaviourTable[EXEC].enable = TRUE; BehaviourTable[FOLLOW].enable = TRUE; BehaviourTable[GOAT].enable = TRUE; BehaviourTable[ROTATE].enable = TRUE; BehaviourTable[FORWARD].enable = TRUE; BehaviourTable[EXTINGUISH].enable = TRUE; BehaviourTable[SEARCH].enable = TRUE; BehaviourTable[ALIGN].enable = TRUE; BehaviourTable[AVOID].enable = TRUE; // Initial behavioural priority (the lower numbers have higher priority) BehaviourTable[EXEC].priority = 1; BehaviourTable[AVOID].priority = 2; BehaviourTable[FOLLOW].priority = 3; BehaviourTable[GOAT].priority = 6; BehaviourTable[ROTATE].priority = 4; BehaviourTable[FORWARD].priority = 4; BehaviourTable[ROAM].priority = 8; // the lowest priority BehaviourTable[EXTINGUISH].priority = 6; BehaviourTable[SEARCH].priority = 6; BehaviourTable[ALIGN].priority = 6; } // Function Name: main() // // Description: Setup stuff on startup // // int main(void) // Main runs under the AvrX Stack { AvrXSetKernelStack(0); // Configure Kernel Stack MCUCR = MCUCR_INIT; // Enable "Sleep" instruction (is done with external ints. on old megas) TCNT0 = TCNT0_INIT; // Initialize RTC TCCR0 = TCCR0_INIT; // Set up Timer0 for CLK/256 rate TIMSK = TIMSK_INIT; // Enable Timer0 overflow interrupt // Configure Ports (see I/O assignment map in hardware.h) DDRA = DDRA_INIT; PORTA = PORTA_INIT; DDRB = DDRB_INIT; PORTB = PORTB_INIT; DDRC = DDRC_INIT; PORTC = PORTC_INIT; DDRD = DDRD_INIT; PORTD = PORTD_INIT; DDRE = DDRE_INIT; PORTE = PORTE_INIT; DDRF = DDRF_INIT; PORTF = PORTF_INIT; // Setup PWM TCCR1A = TCCR1A_INIT; // Setup 8-bit non-inverted PWM mode for Timer1 at ~30Khz. TCCR1B = TCCR1B_INIT; // Select Timer1 prescale of CLK/1 // Stop motors by default. PWM_L = MAX_PWM_OUTPUT; PWM_R = MAX_PWM_OUTPUT; // Configure wheel encoders ints. //EIMSK = 0; // Disable interrupts just in case any interrupts are pending while changing //EICRA = EICRA_INIT; // Configure default edge detection NOTE: Run this before unmasking the int enable bits below! //EIMSK = EIMSK_INIT; // Enable ext. int #2-3 // Enable and configure ADC channels (left adjusted) InitADC(); // Initialize USART baud rate generator InitSerialIO(UBRR_INIT); // Debug to check if there was a reset! UDR0 = '+'; UDR0 = '+'; // Initialize I2C interface i2c_init(); // Initialise runtime parameter table LoadParameterTable(); // Initialise Behaviours InitBehaviourTable(); // Init SCM SCM_Init(); // DEBUG: Init Character I/O for printf //fdevopen((void *)PutChar, 0,0); // Start up primary set of tasks here. AvrXRunTask(TCB(MotorTask)); AvrXRunTask(TCB(exec)); AvrXRunTask(TCB(arbitrate)); AvrXRunTask(TCB(planner)); //AvrXRunTask(TCB(avoid)); //disabled temporarily for debugging purposes. Feb.25th AvrXRunTask(TCB(follow)); AvrXRunTask(TCB(rotate)); AvrXRunTask(TCB(goat)); AvrXRunTask(TCB(forward)); AvrXRunTask(TCB(extinguish)); AvrXRunTask(TCB(search)); AvrXRunTask(TCB(align)); Epilog(); // Switch from AvrX Stack to first task return(0); // Just to get rid of the warnings! } // // Function Name: Align() // Description: Aligns with Wall or Door frame. // unsigned char Align( unsigned char type ) { static AlignMessage AlignCmd; AlignCmd.type = type; AvrXSendMessage(&AlignQueue, &AlignCmd.mcb); AvrXWaitMessageAck(&AlignCmd.mcb); // sanity check. if (AlignCmd.status != ALIGN_REPORT_SUCCESS) { PrintString(str_error); PutChar('A'); PutDecByte(AlignCmd.status); // Stop motors PWM_L = MAX_PWM_OUTPUT; PWM_R = MAX_PWM_OUTPUT; AvrXSuspend(PID(planner)); } return (AlignCmd.status); } // // Function Name: Forward() // Description: Move Forward. // unsigned char Forward( unsigned char lenght, unsigned char expected) { static ForwardMessage ForwardCmd; ForwardCmd.lenght = lenght; if (expected == FORWARD_REPORT_DOOR) ForwardCmd.ignore_door = FALSE; else ForwardCmd.ignore_door = TRUE; AvrXSendMessage(&ForwardQueue, &ForwardCmd.mcb); AvrXWaitMessageAck(&ForwardCmd.mcb); // sanity check. if (ForwardCmd.status != expected) { PrintString(str_error); PutChar('F'); PutDecByte(ForwardCmd.status); // Stop motors PWM_L = MAX_PWM_OUTPUT; PWM_R = MAX_PWM_OUTPUT; AvrXSuspend(PID(planner)); } return(ForwardCmd.status); } // Function Name: Rotate() // Description: Rotate. // void Rotate( signed int angle, unsigned char scan ) { static RotateMessage RotateCmd; RotateCmd.angle = angle; RotateCmd.scan = scan; AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); AvrXWaitMessageAck(&RotateCmd.mcb); } // // Function Name: Search() // Description: Search. // unsigned char Search(unsigned char side) { static SearchMessage SearchCmd; static ExtinguishMessage ExtinguishCmd; SearchCmd.side = side; AvrXSendMessage(&SearchQueue, &SearchCmd.mcb); AvrXWaitMessageAck(&SearchCmd.mcb); // If candle was found, proceed to extinguish it. if (SearchCmd.status == TRUE) { AvrXSendMessage(&ExtinguishQueue, &ExtinguishCmd.mcb); AvrXWaitMessageAck(&ExtinguishCmd.mcb); return(TRUE); } else return(FALSE); } // // Function Name: Follow() // Description: Follow wall. // unsigned char Follow(unsigned char side, unsigned char expected) { static FollowMessage FollowCmd; FollowCmd.side = side; if (expected == FOLLOW_REPORT_DOOR) FollowCmd.ignore_door = FALSE; else FollowCmd.ignore_door = TRUE; AvrXSendMessage(&FollowQueue, &FollowCmd.mcb); AvrXWaitMessageAck(&FollowCmd.mcb); // sanity check. if (FollowCmd.status != expected) { PrintString(str_error); PutChar('T'); PutDecByte(FollowCmd.status); // Stop motors PWM_L = MAX_PWM_OUTPUT; PWM_R = MAX_PWM_OUTPUT; AvrXSuspend(PID(planner)); } return (FollowCmd.status); } // // Function Name: GoHome() // Description: Return to Home position. // void GoHome(void) { static GoAtMessage GoAtCmd; // In all cases we need to return back to the door. GoAtCmd.x = 0; GoAtCmd.y = 0; GoAtCmd.theta = 180; // Face in opposing direction (towards door) AvrXSendMessage(&GoAtQueue, &GoAtCmd.mcb); AvrXWaitMessageAck(&GoAtCmd.mcb); if (Room == 1 || Room == 2) { if (Room == 1) // Room1 specific path { // Align with Right Wall Align(ALIGN_RIGHT); // And go forward until door is found. Forward(0, FORWARD_REPORT_DOOR); // Align with Door frame //Align(ALIGN_DOOR); // Move forward to clear door frame //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); // Move forward 'till wall Forward(0, FORWARD_REPORT_OBSTACLE); // Rotate Right 90 degrees Rotate(90, FALSE); // Align with Left Wall Align(ALIGN_LEFT); } else if (Room == 2) // Room2 specific path { // Exit room by following Right wall. Follow(RIGHT, FOLLOW_REPORT_DOOR); // Align with Door frame (and clear it) //Align(ALIGN_DOOR); // Move forward to clear door frame //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); // Keep following right wall 'till obstacle Follow(RIGHT, FOLLOW_REPORT_OBSTACLE); // Rotate Left 90 degrees Rotate(-90, FALSE); // Align with Right wall Align(ALIGN_RIGHT); // Go forward a fixed amount so we can find left wall Forward(47, FORWARD_REPORT_SUCCESS); } // Common path Forward(48, FORWARD_REPORT_SUCCESS); // TODO: make more robust (no hardcoded distances) Rotate(90, FALSE); Forward(51, FORWARD_REPORT_SUCCESS); // TODO: make more robust (no hardcoded distances) Follow(RIGHT, FOLLOW_REPORT_DOOR); // Until Home circle is sensed. } else if (Room == 3 || Room == 4) { if (Room == 3) // Room3 specific path { // Exit room by following Right wall. Follow(RIGHT, FOLLOW_REPORT_DOOR); // Align with Door frame (and clear it) //Align(ALIGN_DOOR); // Move forward to clear door frame //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); // Keep following right wall 'till obstacle Follow(RIGHT, FOLLOW_REPORT_OBSTACLE); // Rotate Left 90 degrees Rotate(-90, FALSE); // Align with Right wall Align(ALIGN_RIGHT); // Follow right wall 'till the end Follow(RIGHT, FOLLOW_REPORT_SUCCESS); // Move forward 'till obstacle detected Forward(0, FORWARD_REPORT_OBSTACLE); // Rotate Left 90 degrees Rotate(-90, FALSE); // Align with Right wall Align(ALIGN_RIGHT); // Follow wall for a little bit Follow(RIGHT, FOLLOW_REPORT_SUCCESS); // Move forward to clear intersection Forward(0, FORWARD_REPORT_OBSTACLE); // Align with Right wall Align(ALIGN_LEFT); // Rotate Right 90 degrees Rotate(+90, FALSE); // Align with Left wall Align(ALIGN_LEFT); // Follow Left wall 'till obstacle detected. Follow(LEFT, FOLLOW_REPORT_OBSTACLE); } else if (Room == 4) { // Align with left wall Align(ALIGN_LEFT); // Move forward 'till obstacle detected Forward(0, FORWARD_REPORT_OBSTACLE); // Rotate Right 90 degrees Rotate(+90, FALSE); // Follow Left wall 'till obstacle detected. Follow(LEFT, FOLLOW_REPORT_OBSTACLE); } // Common path Align(ALIGN_LEFT); Rotate(+90, FALSE); Follow(LEFT, FOLLOW_REPORT_DOOR); } else // Something is not right! { PrintString(str_error); PrintString(PSTR("Illegal Room #")); AvrXHalt(); } // TODO: Issue a final stop command and halt everything. AvrXHalt(); } // // Function name: LoadParameterTable() // Description: Reload the run-time paramenter table from EEPROM. // void LoadParameterTable() { eeprom_read_block((ConfigData*)&R_ParameterTable, (ConfigData *)&E_ParameterTable, sizeof(ConfigData)); } // // Function name: InitParameterTable() // Description: Re-Initialises the EEPROM paramenter table (when the ParameterTable changes) // void InitParameterTable() { eeprom_write_byte(&E_ParameterTable.f_p, F_P); eeprom_write_byte(&E_ParameterTable.f_d, F_D); eeprom_write_byte(&E_ParameterTable.Kp, K_P); eeprom_write_byte(&E_ParameterTable.Ki, K_I); eeprom_write_byte(&E_ParameterTable.Kd, K_D); eeprom_write_byte(&E_ParameterTable.line_threshold, LINE_SENSOR_THRESHOLD); eeprom_write_byte(&E_ParameterTable.scm_exposure_h, SCM_DEF_EXPOSURE_H); eeprom_write_byte(&E_ParameterTable.scm_exposure_l, SCM_DEF_EXPOSURE_L); eeprom_write_byte(&E_ParameterTable.scm_threshold, SCM_DEF_THRESHOLD); eeprom_write_byte(&E_ParameterTable.scm_min_size, SCM_DEF_MIN_SIZE); } // // Function name: DumpParameterTable() // Description: Dumps the content of the EEPROM paramenter table. // void DumpParameterTable(void) { PrintString(str_parameter_header); PrintString(str_parameter_f_p); PutDecByte(eeprom_read_byte(&E_ParameterTable.f_p)); PrintString(str_parameter_f_d); PutDecByte(eeprom_read_byte(&E_ParameterTable.f_d)); PrintString(str_parameter_Kp); PutDecByte(eeprom_read_byte(&E_ParameterTable.Kp)); PrintString(str_parameter_Ki); PutDecByte(eeprom_read_byte(&E_ParameterTable.Ki)); PrintString(str_parameter_Kd); PutDecByte(eeprom_read_byte(&E_ParameterTable.Kd)); PrintString(str_parameter_line_threshold); PutDecByte(eeprom_read_byte(&E_ParameterTable.line_threshold)); PrintString(str_parameter_scm_exposure_h); PutDecByte(eeprom_read_byte(&E_ParameterTable.scm_exposure_h)); PrintString(str_parameter_scm_exposure_l); PutDecByte(eeprom_read_byte(&E_ParameterTable.scm_exposure_l)); PrintString(str_parameter_scm_threshold); PutDecByte(eeprom_read_byte(&E_ParameterTable.scm_threshold)); PrintString(str_parameter_scm_min_size); PutDecByte(eeprom_read_byte(&E_ParameterTable.scm_min_size)); } // // Function name: EditParameterTable() // Description: Edit the specified entry of the EEPROM paramenter table. // void EditParameterTable(unsigned char id) { PutChar('='); switch(id) { // f_p case 1: eeprom_write_byte(&E_ParameterTable.f_p, GetDecWord()); break; // f_d case 2: eeprom_write_byte(&E_ParameterTable.f_d, GetDecWord()); break; // Kp case 3: eeprom_write_byte(&E_ParameterTable.Kp, GetDecWord()); break; // Ki case 4: eeprom_write_byte(&E_ParameterTable.Ki, GetDecWord()); break; // Kd case 5: eeprom_write_byte(&E_ParameterTable.Kd, GetDecWord()); break; // Line Sensor Threshold case 6: eeprom_write_byte(&E_ParameterTable.line_threshold, GetDecWord()); break; // SCM Initial exposure High case 7: eeprom_write_byte(&E_ParameterTable.scm_exposure_h, GetDecWord()); break; // SCM Initial exposure Low case 8: eeprom_write_byte(&E_ParameterTable.scm_exposure_l, GetDecWord()); break; // SCM Brightness Threshold case 9: eeprom_write_byte(&E_ParameterTable.scm_threshold, GetDecWord()); break; // SCM Size Threshold case 10: eeprom_write_byte(&E_ParameterTable.scm_min_size, GetDecWord()); break; case 0: default: break; } }