Code Listings

8/12/2019 Code Listings

1/61

Full Code Listing

Brain Module:

Header:

/**************************************************************************** Header file for the Brain module****************************************************************************/

#ifndef Brain_H#define Brain_H// Event Definitions#include"ES_Configure.h"/* gets us event definitions */#include"ES_T!es.h" /* gets ool t!e for returns */// t!edefs for the states// State definitions for use ith the $uer functiont!edefenum% &aitingToStart' (ound)' (ecess)' (ound' (ecess' (ound+'

(ecess+' SuddenDeath , (oundState_t -// ulic unction rotot!esool0nitBrain1 uint2_triorit 3-oolostBrain1 ES_Event ThisEvent 3-ES_Event (unBrain1 ES_Event ThisEvent 3-(oundState_t 4uerBrain1 void3-/* comm functions */voidset5lloed(eload1 oolred(eloadStatus' ooldar6(eloadStatus 3-voidset7ameScore1 unsigned char(8score' unsigned charD8score 3-#endif/* Brain_H */

Souce:

/****************************************************************************9odule Brain.c(evision ).:.)Descri!tion Brain module is the master state machine for the root****************************************************************************//*;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 0nclude iles ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;*//* include header files for this state machine as ell as an machines at the ne#include"EventChec6ers.h"#include =hidef.h> /* common defines and macros */#include =mc?s)e)2.h> /* derivative information */#include =S)e)2its.h> /* it definitions */


2/61

#include =Bin_Const.h> /* macros to allo s!ecifing inarconstants */#include =termio.h> /* to get !rotot!e fo 6hit13 */#include"s)evec.h" /* E)2 interru!t vectors */#include =s)vec.h>#include"[email protected]"#include"navigation.h"#include"Shooting.h"#include"Aousting.h"#include"BallDis!ensing.h"#include"Comm.h"/*;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule Defines ;;;;;;;;;;;;;;;;;;;;;;;;;;;;*/// Brain inner current state defines#define @T_0@_(@D :#define D(00@7_T_75 )#define SHT0@7_B5S #define CECT0@7_B5S +#define T(@0@7_T_T5E F#define D(00@7_T_T5E G#define T(@0@7_T_&5

#define D(00@7_T_D0SE@SE( I#define 507@0@7_&0TH_&5 2#define D(00@7_T_8@07HT ?#define 5TT5C80@7 ):#define 507@ ))#define D(00@7_5ST )/*;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule unctions ;;;;;;;;;;;;;;;;;;;;;;;;;;;*//* !rotot!es for !rivate functions for this machine.The should e functions relevant to the ehavior of this state machine*/// Command unctionsstatic voidcommand@avigation1unsigned charcommand3-static voidcommandShooting1unsigned charcommand3-

static voidcommandAousting1unsigned charcommand3-static voidcommandBallDis!ensing1unsigned charcommand3-// (ound s!ecific state machinesstatic void(ound)S91ES_Event ThisEvent3-static void(ecess)S91ES_Event ThisEvent3-static void(oundS91ES_Event ThisEvent3-static void(ecessS91ES_Event ThisEvent3-static void(ound+S91ES_Event ThisEvent3-static void(ecess+S91ES_Event ThisEvent3-static voidSuddenDeathS91ES_Event ThisEvent3-/*;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule ariales ;;;;;;;;;;;;;;;;;;;;;;;;;;;*/// everod needs a state variale' ou ma need others as ell.// t!e of state variale should match htat of enum in header file

static(oundState_t Current(ound-// ith the introduction of 7en' e need a module level riorit var as ellstatic uint2_t9riorit-static signed intscore-static unsigned char6nightColor-// AS( varialesstatic oolis&aitingorStart J false-static oolis0n9atch J false-static oolis(ecess J false-static oolisEnteringSuddenDeath J false-


3/61

static oolisEndf9atch J false-static oolisnhorsed J false-static oolis5lloed(eload J false-static signed intscore J :-/*;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule Code ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;*//****************************************************************************unction 0nitBrainarameters uint2_t K the !riort of this service(eturns ool' false if error in initialiLation' true otheriseDescri!tion Saves aa the !riorit' sets u! the Brain****************************************************************************/ool0nitBrain1 uint2_triorit 3%// ES_Event ThisEvent- 9riorit J riorit- // Start first communication

ES_Timer_0nitTimer1(E4EST_ST5TS_T09E(' G::3- // !ut us into the 0nitial seudoState Current(ound J &aitingToStart- // (ead in!ut !in to get 6night color 8@07HT_S&0TCH_0@E J 0@T- 6nightColor J 8@07HT_S&0TCH- // (ed J :' Dar6 J ) !rintf1"8night Color K MdNr Nn"' 6nightColor3- // Set game match status ED to out!ut 759E_ST5TS_ED_0@E J TT- return true-,/****************************************************************************unction

ostBrainarameters E_Event ThisEvent ' the event to !ost to the $ueue(eturns oolean alse if the En$ueue o!eration failed' True otheriseDescri!tion osts an event to the Brain****************************************************************************/oolostBrain1 ES_Event ThisEvent 3% returnES_ostToService1 9riorit' ThisEvent3-,/****************************************************************************

unction (un(50@arameters ES_Event K the event to !rocess(eturns ES_Event' ES_@_EE@T if no error ES_E((( otheriseDescri!tion (uns the Brain state machine ; a hierarchical state machine****************************************************************************/ES_Event (unBrain1 ES_Event ThisEvent 3


4/61

% ES_Event (eturnEvent- ES_Event Sto!Event- ES_Event TestCommand- ooltest J false- (eturnEvent.EventT!e J ES_@_EE@T- // assume no errors Sto!Event.EventT!e J ES_(@D_E(- // Sam!le AS( if1 ThisEvent.EventT!e JJ ES_T09ET 3 % if1ThisEvent.Eventaram JJ (E4EST_ST5TS_T09E(3 % // ost ne command re$uest to Comm S9 ES_Event CommEvent- CommEvent.EventT!e J ES_SE@D_C995@D- CommEvent.Eventaram J C995@D_759E_ST5TS- ostComm1 CommEvent 3- //!uts1"!osted command game statusNrNn"3- // (estart comm re$uest timer ES_Timer_0nitTimer1(E4EST_SC(E_T09E(' (E4EST_SC(E_T09E3-

return(eturnEvent- , if1ThisEvent.Eventaram JJ (E4EST_SC(E_T09E(3 % // ost ne command re$uest to Comm S9 ES_Event CommEvent- CommEvent.EventT!e J ES_SE@D_C995@D- CommEvent.Eventaram J C995@D_759E_SC(E- ostComm1 CommEvent 3- //!uts1"!osted command game scoreNrNn"3- // (estart comm re$uest timer ES_Timer_0nitTimer1(E4EST_ST5TS_T09E(' (E4EST_ST5TS_T09E3- return(eturnEvent-

, , sitch1 Current(ound 3 % // &aiting To Start case&aitingToStart K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % !uts1"Entering (ound )Nr"3- Current(ound J (ound)- (ound)S91ThisEvent3- , rea6-

// (ound ) case(ound) K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % !uts1"Entering (ecess )Nr"3- // @e round event' sto! (ound)S9' start (ecess)S9 Current(ound J (ecess)- (ound)S91Sto!Event3- (ecess)S91ThisEvent3- ,


5/61

else % // (un round ) state machine (ound)S91ThisEvent3- , rea6- // (ecess ) case(ecess) K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % !uts1"Entering (ound Nr"3- // @e round event' sto! (ecess)S9' start (oundS9 Current(ound J (ound- (ecess)S91Sto!Event3- (oundS91ThisEvent3- , else % // (un recess ) state machine (ecess)S91ThisEvent3-

, rea6- // (ound case(ound K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % !uts1"Entering (ecess Nr"3- // @e round event' sto! (oundS9' start (ecessS9 Current(ound J (ecess- (oundS91Sto!Event3- (ecessS91ThisEvent3- , else

% // (un round ) state machine (oundS91ThisEvent3- , rea6- // (ecess case(ecess K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % !uts1"Entering (ound +Nr"3- // @e round event' sto! (ecessS9' start (ound+S9 Current(ound J (ound+- (ecessS91Sto!Event3-

(ound+S91ThisEvent3- , else % // (un recess state machine (ecessS91ThisEvent3- , rea6- // (ound + case(ound+ K


6/61

if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % !uts1"Entering (ecess +Nr"3- // @e round event' sto! (ound+S9' start (ecess+S9 Current(ound J (ecess+- (ound+S91Sto!Event3- (ecess+S91ThisEvent3- , else % // (un round + state machine (ound+S91ThisEvent3- , rea6- // (ecess + case(ecess+ K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % !uts1"Entering Sudden DeathNr"3- // @e round event' sto! (ecess+S9' start SuddenDeath

Current(ound J SuddenDeath- (ecess+S91Sto!Event3- SuddenDeathS91ThisEvent3- , else % // (un recess + state machine (ecess)S91ThisEvent3- , rea6- // Sudden Death caseSuddenDeath K if1ThisEvent.EventT!e JJ ES_@E&_(@D3

% !uts1"End of matchNr"3- // @e round event' sto! (ecess+S9' start SuddenDeath Current(ound J &aitingToStart- (ecess+S91Sto!Event3- SuddenDeathS91ThisEvent3- , else % // (un sudden death state machine SuddenDeathS91ThisEvent3- rea6- ,

, // 7o ac6 to &aitingToStart if the game is over if1ThisEvent.EventT!e JJ ES_759E_E(3 % 759E_ST5TS_ED J &- commandShooting1ST_0TCH0@73- Current(ound J &aitingToStart- , return(eturnEvent-,


7/61

/****************************************************************************unction 4uerBrainarameters @one(eturns Tem!lateState_t The current state of the Tem!late state machineDescri!tion returns the current state of the Brain****************************************************************************/(oundState_t 4uerBrain1 void3% return1Current(ound3-,/***************************************************************************!rivate functions***************************************************************************/

/****************************************************************************unction (ound)S9

arameters ES_Event K the event to !rocess(eturns @oneDescri!tion (uns the state machine for round )****************************************************************************/static void(ound)S91ES_Event ThisEvent3% static unsigned charCurrentState J @T_0@_(@D- // (eset the state machine and sto! the root if the round or game is over if11ThisEvent.EventT!e JJ ES_759E_E(3 OO 1ThisEvent.EventT!e JJES_(@D_E(33

% command@avigation1ST_(BT3- CurrentState J @T_0@_(@D- return- , sitch1CurrentState3 % // 0nitialiLe the state machine for round ) here case@T_0@_(@D K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % 759E_ST5TS_ED J H07H- command@avigation1D(0E_T_B3-

CurrentState J D(00@7_5ST- ES_Timer_0nitTimer1B(50@_T09E(' 5ST_D(0E_T09E3- , rea6- // Driving reall fast caseD(00@7_5ST K if1ThisEvent.EventT!e JJ ES_T09ET PP ThisEvent.Eventaram JJ B(50@_T09E(3 % !uts1"Sloing donNr"3- command@avigation1S&_D&@3-


8/61

CurrentState J D(00@7_T_75- , rea6- // Driving to Home B caseD(00@7_T_75 K if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % command@avigation1ST_(BT3- CurrentState J @T_0@_(@D- , rea6- ,,/****************************************************************************unction (ecess)S9arameters ES_Event K the event to !rocess(eturns @one

Descri!tion (uns the state machine for recess )****************************************************************************/static void(ecess)S91ES_Event ThisEvent3% static unsigned charCurrentState J @ot0n(ound- // (eset the state machine and sto! the root if the round or game is over if11ThisEvent.EventT!e JJ ES_759E_E(3 OO 1ThisEvent.EventT!e JJES_(@D_E(33 % command@avigation1ST_(BT3- CurrentState J @T_0@_(@D- return-

, sitch1CurrentState3 % // 0nitialiLe the state machine for round ) here case@T_0@_(@D K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % command@avigation1D(0E_T_753- CurrentState J D(00@7_T_75- , rea6- // Driving to Home B caseD(00@7_T_75 K

if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % command@avigation1ST_(BT3- commandShooting1ST5(T_0TCH0@73- CurrentState J @T_0@_(@D- , rea6- ,,/****************************************************************************


9/61


10/61

,/****************************************************************************unction (ecessS9arameters ES_Event K the event to !rocess(eturns @oneDescri!tion (uns the state machine for recess ****************************************************************************/static void(ecessS91ES_Event ThisEvent3% static unsigned charCurrentState J @T_0@_(@D- // (eset the state machine and sto! the root if the round or game is over if11ThisEvent.EventT!e JJ ES_759E_E(3 OO 1ThisEvent.EventT!e JJES_(@D_E(33 % command@avigation1ST_(BT3- CurrentState J @T_0@_(@D-

return- , sitch1CurrentState3 % //5ctual code // 0nitialiLe the state machine for round ) here case@T_0@_(@D K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % command@avigation1507@_&0TH_&53- CurrentState J 507@- , rea6-

// 5ligning ith all efore turning case507@ K if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % command@avigation1T(@_(BT_C&_?:3- CurrentState J T(@0@7_T_T5E- , rea6- // Turning to ta!e caseT(@0@7_T_T5E K if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % !uts1"5ligned ith ta!eNr"3-

command@avigation1D(0E_T_T5E3- CurrentState J D(00@7_T_T5E- , rea6- // Driving to ta!e caseD(00@7_T_T5E K if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % !uts1"Ta!e reachedNr"3- command@avigation1T(@_(BT_CC&_?:3-


11/61

CurrentState J T(@0@7_T_&5- , rea6- // Turning ac6 to all caseT(@0@7_T_&5 K if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % !uts1"5ligned ith allNr"3- command@avigation1507@_&0TH_&53- CurrentState J 507@0@7_&0TH_&5- , rea6- // 5ligning ith all case507@0@7_&0TH_&5 K if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % !uts1"5ligned ith allNr"3- command@avigation1D(0E_T_D0SE@SE(3- CurrentState J D(00@7_T_D0SE@SE(- ,

rea6- // Driving to dis!enser caseD(00@7_T_D0SE@SE( K if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % !uts1"5t all dis!enser"3- command@avigation1507@_&0TH_&53- commandBallDis!ensing1ES_CECT_B53- CurrentState J CECT0@7_B5S- , rea6- // CollectingBalls caseCECT0@7_B5S K

if1ThisEvent.EventT!e JJ ES_B5_CECTED3 % commandBallDis!ensing1ES_CECT_B53- , rea6- ,,/****************************************************************************unction (ound+S9arameters ES_Event K the event to !rocess(eturns

@oneDescri!tion (uns the state machine for round +****************************************************************************/static void(ound+S91ES_Event ThisEvent3% static unsigned charCurrentState J @T_0@_(@D- // (eset the state machine and sto! the root if the round or game is over if11ThisEvent.EventT!e JJ ES_759E_E(3 OO 1ThisEvent.EventT!e JJES_(@D_E(33


12/61

% command@avigation1ST_(BT3- CurrentState J @T_0@_(@D- return- , sitch1CurrentState3 % // (ound+S9 0nitialiLation case@T_0@_(@D K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % !uts1"Entering round +Nr"3- commandShooting1ST5(T_0TCH0@73- command@avigation1D(0E_T_B3- CurrentState J 5TT5C80@7- ES_Timer_0nitTimer1B(50@_T09E(' 5ST_D(0E_T09E / F3- , rea6- // 5ttac6 root case5TT5C80@7 K

if1ThisEvent.EventT!e JJ ES_T09ET PP ThisEvent.Eventaram JJ B(50@_T09E(3 % CurrentState J D(00@7_5ST- commandShooting1SHT_B53- ES_Timer_0nitTimer1B(50@_T09E(' 5ST_D(0E_T09E * +/ F3- commandAousting1DEQ_AST_HE5D3- , rea6- // Driving reall fast caseD(00@7_5ST K if1ThisEvent.EventT!e JJ ES_T09ET PP ThisEvent.Eventaram JJ B(50@_T09E(3 % command@avigation1S&_D&@3-

CurrentState J D(00@7_T_75- , rea6- // Driving to Home B caseD(00@7_T_75 K if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % !uts1"5t goalNr"3- command@avigation1ST_(BT3- CurrentState J @T_0@_(@D- , rea6- ,

,/****************************************************************************unction (ecess+S9arameters ES_Event K the event to !rocess(eturns @oneDescri!tion (uns the state machine for recess +


13/61

****************************************************************************/static void(ecess+S91ES_Event ThisEvent3% static unsigned charCurrentState J @T_0@_(@D- // (eset the state machine and sto! the root if the round or game is over if11ThisEvent.EventT!e JJ ES_759E_E(3 OO 1ThisEvent.EventT!e JJES_(@D_E(33 % command@avigation1ST_(BT3- CurrentState J @T_0@_(@D- return- , sitch1CurrentState3 % // 0nitialiLe the state machine for round ) here case@T_0@_(@D K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % 759E_ST5TS_ED J H07H- command@avigation1D(0E_T_B3-

command@avigation1S&_D&@3- CurrentState J D(00@7_T_75- ES_Timer_0nitTimer1B(50@_T09E(' 5ST_D(0E_T09E3- , rea6- // Driving to Home B caseD(00@7_T_75 K if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % command@avigation1ST_(BT3- CurrentState J @T_0@_(@D- , rea6-

,,/****************************************************************************unction SuddenDeathS9arameters ES_Event K the event to !rocess(eturns @oneDescri!tion (uns the state machine for sudden death****************************************************************************/static voidSuddenDeathS91ES_Event ThisEvent3

% static unsigned charCurrentState J @T_0@_(@D- sitch1CurrentState3 % // (ound+S9 0nitialiLation case@T_0@_(@D K if1ThisEvent.EventT!e JJ ES_@E&_(@D3 % !uts1"Entering round +Nr"3- commandShooting1ST5(T_0TCH0@73-


14/61

command@avigation1D(0E_T_53- CurrentState J 5TT5C80@7- ES_Timer_0nitTimer1B(50@_T09E(' 5ST_D(0E_T09E / F3- , rea6- // 5ttac6 root case5TT5C80@7 K if1ThisEvent.EventT!e JJ ES_T09ET PP ThisEvent.Eventaram JJ B(50@_T09E(3 % CurrentState J D(00@7_5ST- commandShooting1SHT_B53- ES_Timer_0nitTimer1B(50@_T09E(' 5ST_D(0E_T09E * +/ F3- commandAousting1DEQ_AST_BDQ3- , rea6- // Driving reall fast caseD(00@7_5ST K if1ThisEvent.EventT!e JJ ES_T09ET PP ThisEvent.Eventaram JJ B(50@_T09E(3 % command@avigation1S&_D&@3-

CurrentState J D(00@7_T_75- , rea6- // Driving to Home B caseD(00@7_T_75 K if1ThisEvent.EventT!e JJ ES_@5075T0@_C9ETE3 % !uts1"5t goalNr"3- command@avigation1ST_(BT3- CurrentState J @T_0@_(@D- , rea6- ,

,/****************************************************************************unction command@avigationarameters 5n unsigned char s!ecifing the command(eturns @oneDescri!tion send a command to the navigation service****************************************************************************/static voidcommand@avigation1unsigned charcommand3%

ES_Event commandEvent- commandEvent.EventT!e J ES_C995@D- commandEvent.Eventaram J command- ost@avigation1commandEvent3-,/****************************************************************************unction commandAoustingarameters 5n unsigned char s!ecifing the command


15/61

(eturns @oneDescri!tion send a command to the Rousting service****************************************************************************/static voidcommandAousting1unsigned charcommand3% ES_Event commandEvent- commandEvent.EventT!e J ES_C995@D- commandEvent.Eventaram J command- ostAousting1commandEvent3-,/****************************************************************************unction commandShootingarameters 5n unsigned char s!ecifing the command(eturns @oneDescri!tion

send a command to the shooting service****************************************************************************/static voidcommandShooting1unsigned charcommand3% ES_Event commandEvent- commandEvent.EventT!e J ES_C995@D- commandEvent.Eventaram J command- ostShooting1commandEvent3-,/****************************************************************************unction commandBallDis!ensingarameters

5n unsigned char s!ecifing the command(eturns @oneDescri!tion send a command to the all dis!ensing service****************************************************************************/static voidcommandBallDis!ensing1unsigned charcommand3% ES_Event commandEvent- commandEvent.EventT!e J ES_CECT_B5- commandEvent.Eventaram J command- ostBallDis!ensing1commandEvent3-,

/***************************************************************************!ulic functions***************************************************************************/

/****************************************************************************unction set5lloed(eloadarameters ool ; alloed reload or not.(eturns @othing.


16/61

Descri!tion !dates is5lloed(eload module variale from CommS9.****************************************************************************/voidset5lloed(eload1 oolred(eloadStatus' ooldar6(eloadStatus 3% if16nightColor JJ (ED3 % is5lloed(eload J red(eloadStatus- , else if16nightColor JJ D5(83 % is5lloed(eload J dar6(eloadStatus- ,,/****************************************************************************unction set7ameScorearameters unsigned char ; (ed 6night score unsigned char ; Dar6 6night score

(eturns @othing.Descri!tion !dates red8nightScore and dar68nightScore module variales from CommS9.****************************************************************************/voidset7ameScore1 unsigned char(8score' unsigned charD8score 3% if16nightColor JJ (ED3 % score J (8score ; D8score- , else if16nightColor JJ D5(83 %

score J D8score ; (8score- ,

,


17/61

Navigation Module:

Header:

/**************************************************************************** Header file for the navigation module****************************************************************************/

#ifndef @avigation_H#define @avigation_H// Event Definitions#include"ES_Configure.h"#include"ES_T!es.h"// t!edefs for the states// State definitions for use ith the $uer functiont!edefenum% &aitorCommand' DriveToSuRect'DrivingTo9idfield'DrivingTolace'DrivingTo7oal' DrivingTo8night' DrivingToDis!enser'5ligning'5ligning&ith8night' 5ligning&ith&all' 5ligning&ith7oal'5ligning&ithDis!enser' 5ligning&ith&all_Dis!enser' Drivingull' Turning' DrivingToTa!e' 0nit0nner' DrivingToHome5' DrivingToHomeB' (e$uestingBall , a2State_t -// ulic unction rotot!esool0nit@avigation1 uint2_triorit 3-oolost@avigation1 ES_Event ThisEvent 3-ES_Event (un@avigation1 ES_Event ThisEvent 3-a2State_t 4uer@avigation1 void3-

floatgetBac6ltrasonicDistance1void3-floatgetSideltrasonicDistance)1void3-floatgetSideltrasonicDistance1void3-

#endif/* @avigation_H */

Source:

/****************************************************************************9odule @avigation.c(evision ).:.)Descri!tion 0m!lements the navigation module****************************************************************************//*;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 0nclude iles ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;*//* include header files for this state machine as ell as an machines at the ne


18/61

#include"navigation.h"#include =stdio.h>#include"EventChec6ers.h"#include =hidef.h> /* common defines and macros */#include =mc?s)e)2.h> /* derivative information */#include =S)e)2its.h> /* it definitions */#include =Bin_Const.h> /* macros to allo s!ecifing inarconstants */#include =termio.h> /* to get !rotot!e fo 6hit13 */#include"s)evec.h" /* E)2 interru!t vectors */#include =s)vec.h>#include"Brain.h"/*;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule Defines ;;;;;;;;;;;;;;;;;;;;;;;;;;;;*/#include"[email protected]"#define ST5T0@5(Q #define C@T(_E@5BED )#define C@T(_D0S5BED :#define 95_SE_C@T ):#define :#define DEB@CE_T09E +

/*;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule unctions ;;;;;;;;;;;;;;;;;;;;;;;;;;;*//* !rotot!es for !rivate functions for this machine.The should e functions relevant to the ehavior of this state machine*/static voiddrivingTolaceS91ES_Event ThisEvent' unsigned chardirection3-static voiddrivingToDis!enserS91ES_Event ThisEvent3-static voidsetortDirections1void3-static voidinitTimers1void3-static voidinit&91void3-static voidinitS01void3-static voiddrive(oot1chardirection' chars!eed3-static voidslo(oot1void3-static voidturn(oot1chardirection3-

static voidsto!(oot1void3-static voidres!ondToCommand1ES_Event ThisEvent3-static void!ost@avigationCom!lete1void3-static voidenaleositionControl1unsigned chardirection3-static voidenaleStationarControl1void3-static voiddisaleositionControl1void3-/*;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule ariales ;;;;;;;;;;;;;;;;;;;;;;;;;;;*/// everod needs a state variale' ou ma need others as ell.// t!e of state variale should match htat of enum in header filestatic unsigned charecho_flag)J:' echo_flagJ:' echo_flag+J:'ultrasonicEnaleJ:'controlDirection-static unsigned inttrigger_sendJ:'echo_detectJ:'echo_detectJ:'echo_detect+J:-static unsigned charleftS!eed'rightS!eed'testcountJ:-

static floatuseconds'useconds'useconds+'distance'distance'distance+' referenceDistance-statica2State_t CurrentState-// ith the introduction of 7en' e need a module level riorit var as ellstatic uint2_t9riorit-static oolisBeacon J false-/*;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule Code ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;*//****************************************************************************unction 0nit@avigationarameters


19/61

uint2_t K the !riort of this service(eturns oolean' alse if error in initialiLation' True otheriseDescri!tion Saves aa the !riorit' sets u! the initial transition and does an other re$uired initialiLation for this state machine@otes5uthor A. Edard Carrer' ):/+/))' )2KGG****************************************************************************/ool0nit@avigation1 uint2_triorit 3% 9riorit J riorit- // !ut us into the 0nitial seudoState CurrentState J &aitorCommand- // Start $uer timer initTimers13- // 0nitialiLe &9 init&913- // Set !ort directions

setortDirections13- !uts1"@avigation initialiLation com!letedNr"3- return true-,/****************************************************************************unction ost@avigationarameters E_Event ThisEvent ' the event to !ost to the $ueue(eturns oolean alse if the En$ueue o!eration failed' True otheriseDescri!tion osts an event to this state machines $ueue

@otes5uthor A. Edard Carrer' ):/+/))' )?KG****************************************************************************/oolost@avigation1 ES_Event ThisEvent 3% returnES_ostToService1 9riorit' ThisEvent3-,/****************************************************************************unction (un@avigationarameters ES_Event K the event to !rocess

(eturns ES_Event' ES_@_EE@T if no error ES_E((( otheriseDescri!tion add our descri!tion here@otes uses nested sitch/case to im!lement the machine.5uthor A. Edard Carrer' :)/)G/)' )GK+****************************************************************************/ES_Event (un@avigation1 ES_Event ThisEvent 3


20/61

% //static unsigned char CurrentDirection'5lignTarget- ES_Event (eturnEvent- ES_Event BrainEvent- (eturnEvent.EventT!e J ES_@_EE@T- // assume no errors // (es!ond to a ne command from rain regardless of CurrentState if1ThisEvent.EventT!e JJ ES_C995@D3 % !uts1"@e command from rainNr"3- res!ondToCommand1ThisEvent3- , // 0f the navigation timer times out' sto! the root and !ost a failure to rain if11ThisEvent.EventT!e JJ ES_T09ET3 PP 1ThisEvent.Eventaram JJ@5075T0@_T09E(33 % sto!(oot13- enaleositionControl1ST5T0@5(Q3- BrainEvent.EventT!e J ES_@5075T0@_50(E- ostBrain1BrainEvent3- return(eturnEvent-

, // De!ending on CurrentState' e


21/61

!ost@avigationCom!lete13- ES_Timer_Sto!Timer1@5075T0@_T09E(3- , else if11ThisEvent.EventT!e JJ ES_T09ET3 PP 1ThisEvent.Eventaram JJ507@9E@T_DEB@CE_T09E(33 % if18@07HT_BE5C@ JJ 507@ED3 % // 5ligned' sto! root and !ost navigation com!lete to rain sto!(oot13- CurrentState J &aitorCommand- !ost@avigationCom!lete13- ES_Timer_Sto!Timer1@5075T0@_T09E(3- , else % // @ot aligned' restart deounce timer ES_Timer_0nitTimer1507@9E@T_DEB@CE_T09E(' DEB@CE_T09E3- , ,

rea6- caseDrivingTo7oalK if1ThisEvent.EventT!e JJ ES_75_DETECTED3 % // 7oal reached' sto! root' !ost to rain and set current state ac6 to&aitorCommand enaleositionControl1ST5T0@5(Q3- sto!(oot13- CurrentState J &aitorCommand- !ost@avigationCom!lete13- ES_Timer_Sto!Timer1@5075T0@_T09E(3- , rea6-

caseDrivingToDis!enser K if1ThisEvent.EventT!e JJ ES_D0SE@SE(_DETECTED3 % // 7oal reached' sto! root' !ost to rain and set current state ac6 to&aitorCommand enaleositionControl1ST5T0@5(Q3- sto!(oot13- CurrentState J &aitorCommand- !ost@avigationCom!lete13- , rea6- case5ligning&ith8nightK if11ThisEvent.EventT!e JJ ES_T09ET3 PP 1ThisEvent.Eventaram JJ

507@9E@T_DEB@CE_T09E(33 % if18@07HT_BE5C@ JJ 507@ED3 % // 5ligned' sto! root and !ost navigation com!lete to rain sto!(oot13- CurrentState J &aitorCommand- !ost@avigationCom!lete13- ES_Timer_Sto!Timer1@5075T0@_T09E(3- ,


22/61

else % // @ot aligned' restart deounce timer ES_Timer_0nitTimer1507@9E@T_DEB@CE_T09E(' DEB@CE_T09E3- , , rea6- case5ligning&ith7oalK if11ThisEvent.EventT!e JJ ES_T09ET3 PP 1ThisEvent.Eventaram JJ507@9E@T_DEB@CE_T09E(33 % if175_BE5C@ JJ 507@ED3 % // 5ligned' sto! root and !ost navigation com!lete to rain disaleositionControl13- sto!(oot13- CurrentState J &aitorCommand- !ost@avigationCom!lete13- , else

% // @ot aligned' restart deounce timer ES_Timer_0nitTimer1507@9E@T_DEB@CE_T09E(' DEB@CE_T09E3- , , rea6- case5ligning&ith&all K if1ThisEvent.EventT!e JJ ES_&5_507@9E@T_C9ETE3 % // (oot is aligned ith all' sto! turning and move to &aitorCommand disaleositionControl13- sto!(oot13- CurrentState J &aitorCommand-

!ost@avigationCom!lete13- , rea6- caseTurning K if1ThisEvent.EventT!e JJ ES_T09ET PP ThisEvent.Eventaram JJ T(@_T09E(3 % // Turning com!lete' sto! root and !ost navigation com!lete to rain sto!(oot13- enaleositionControl1ST5T0@5(Q3- CurrentState J &aitorCommand- !ost@avigationCom!lete13- , rea6-

caseDrivingToTa!e K if1ThisEvent.EventT!e JJ ES_T5E_DETECTED3 % // Ta!e reached' sto! root and !ost navigation com!lete to rain sto!(oot13- CurrentState J &aitorCommand- !ost@avigationCom!lete13- ,

rea6- , // End sitch on CurrentState


23/61

return(eturnEvent-,/****************************************************************************unction 4uer@avigationarameters @one(eturns Tem!lateState_t The current state of the Tem!late state machineDescri!tion returns the current state of the Tem!late state machine@otes5uthor A. Edard Carrer' ):/+/))' )?K)****************************************************************************/a2State_t 4uer@avigation1 void3% return1CurrentState3-,/***************************************************************************

!rivate functions***************************************************************************/

/****************************************************************************unction res!ondToCommandarameters unsigned char The S0 command to e


24/61

CurrentState J DrivingTo8night- drive(oot1DC_' (&5(D3- enaleositionControl1(&5(D3- rea6- caseD(0E_(EE(SE_T_8@07HT K // Set CurrentState to DrivingTo8night and drive in reverse ES_Timer_0nitTimer1507@9E@T_DEB@CE_T09E(' DEB@CE_T09E3- CurrentState J DrivingTo8night- drive(oot1DC_' (EE(SE3- enaleositionControl1(EE(SE3- rea6- caseD(0E_T_75 K // Set CurrentState to DrivingTo7oal and drive forard ES_Timer_0nitTimer1507@9E@T_DEB@CE_T09E(' DEB@CE_T09E3- CurrentState J DrivingTo7oal- drive(oot1DC_H5' (&5(D3- enaleositionControl1(&5(D3- rea6- caseD(0E_T_D0SE@SE( K !uts1"Driving to dis!enserNr"3-

CurrentState J DrivingToDis!enser- drive(oot1DC_H5' (EE(SE3- enaleositionControl1(EE(SE3- rea6- case507@_&0TH_8@07HT K // 5lign ith 6night CurrentState J 5ligning&ith8night- disaleositionControl13- ES_Timer_0nitTimer1507@9E@T_DEB@CE_T09E(' DEB@CE_T09E3- turn(oot1C&3- rea6- case507@_&0TH_75 K // 5lign ith goal

CurrentState J 5ligning&ith7oal- disaleositionControl13- turn(oot1C&3- ES_Timer_0nitTimer1507@9E@T_DEB@CE_T09E(' DEB@CE_T09E3- rea6- case507@_&0TH_&5 K // 5lign ith all CurrentState J 5ligning&ith&all- sto!(oot13- enaleositionControl1ST5T0@5(Q3- rea6- caseT(@_(BT_C&_?: K // Turn root cloc6ise ?: degrees

CurrentState J Turning- disaleositionControl13- turn(oot1C&3- ES_Timer_0nitTimer1T(@_T09E(' T(@_?:_T09E3- //!uts1"Turn root cloc6ise ?: Nr"3- rea6- caseT(@_(BT_CC&_?: K // Turn root counter;cloc6ise ?: degrees CurrentState J Turning- turn(oot1CC&3-


25/61

disaleositionControl13- ES_Timer_0nitTimer1T(@_T09E(' T(@_?:_T09E3- //!uts1"Turn root counter;cloc6ise ?: Nr"3- rea6- caseD(0E_T_T5E K // Drive root to ta!e CurrentState J DrivingToTa!e- disaleositionControl13- drive(oot1DC_H5' (EE(SE3- rea6- caseST_(BT K // Sto! the root and move to ait for command CurrentState J &aitorCommand- !uts1"Command sto! rootNr"3- sto!(oot13- rea6- caseS&_D&@ K // Slo the root don slo(oot13- rea6-

, // End sitch on Eventaram,/****************************************************************************unction setortDirectionsarameters @othing.(eturns @othing.Descri!tion Sets the !ort directions for lines used on the E)2.****************************************************************************/static voidsetortDirections1void3

% // Set motor lines as out!uts 9T(_ET_D0(ECT0@_0@E J TT- 9T(_(07HT_D0(ECT0@_0@E J TT- 9T(_ET_0@E J TT- 9T(_(07HT_0@E J TT- // Set lines as in!uts 75_BE5C@_0@E J 0@T- 8@07HT_BE5C@_0@E J 0@T- (E5D_ED_0@E J TT- (E5D_ED J &- //Sets ltrasonic Trigger as ut!ut T(5S@0C_B5C8_T_0@E J TT-

T(5S@0C_ET_T_0@E J TT- T(5S@0C_(07HT_T_0@E J TT- T(5S@0C_B5C8_T J &- T(5S@0C_ET_T J &- T(5S@0C_(07HT_T J &- // Sto! the root initiall sto!(oot13-,/****************************************************************************unction


26/61

!ost@avigationCom!letearameters @othing.(eturns @othing.Descri!tion osts a navigation com!lete event to the rain.****************************************************************************/static void!ost@avigationCom!lete1void3% ES_Event BrainEvent- BrainEvent.EventT!e J ES_@5075T0@_C9ETE- ostBrain1BrainEvent3-,/****************************************************************************unction initTimersarameters @othing.(eturns

@othing.Descri!tion 0nitialiLes channel F of timer : for in!ut ca!ture of eacon.****************************************************************************/static voidinitTimers1void3% //0nitialiLe ltrasonic Timers T09:_TSC() J _S)_TE@- //Enale timer //Set !rescale to divide ) T09:_TSC( J_S)_(- T09:_T0SOJ_S)_0SF- //Set 0SF to ut!ut Com!are T09:_T0SPJU_S)_0SG- //Set 0SG to 0n!ut Ca!ture T09:_T0SPJU_S)_0S- //Set 0S to 0n!ut Ca!ture

T09:_T0SPJU_S)_0SI- //Set 0SI to 0n!ut Ca!ture //Set Channel F on Timer : ut!ut Com!are to toggle. T09:_TCT+ OJ _S)_F- T09:_TCT+ PJ U_S)_9F- //Set Chanel G on Timer : to 0n!ut Ca!ture alling Edge T09:_TCT+ PJ U1_S)_ED7G53- T09:_TCT+ OJ _S)_ED7GB- //Set Chanel on Timer : to 0n!ut Ca!ture alling Edge T09:_TCT+ PJ U1_S)_ED753- T09:_TCT+ OJ _S)_ED7B- //Set Chanel I on Timer : to 0n!ut Ca!ture alling Edge T09:_TCT+ PJ U1_S)_ED7I53- T09:_TCT+ OJ _S)_ED7IB-

//Tie timers to 0nterru!t T09:_T0E OJ _S)_CF0- //5ssociates out!ut com!are ith interru!t. T09:_T0E OJ _S)_CG0- //5ssociates in!ut ca!ture ith interru!t. T09:_T0E OJ _S)_C0- //5ssociates in!ut ca!ture ith interru!t. T09:_T0E OJ _S)_CI0- //5ssociates in!ut ca!ture ith interru!t. T09:_TCF J T09:_TC@T V T09E(_E(0D_C@T- //Sets !eriod in hich Timer triggersflag T09:_T7) J _S)_CF- //Clears lag T09:_T7) J _S)_CG- //Clears lag T09:_T7) J _S)_C- //Clears lag


27/61

T09:_T7) J _S)_CI- //Clears lag Enale0nterru!ts-,/****************************************************************************unction init&9arameters @othing.(eturns @othing.Descri!tion 0nitialiLes channel F of timer : for in!ut ca!ture of eacon.****************************************************************************/static voidinit&91void3% // Set u! &9 sstem &9E OJ 1_S)_&9E: O _S)_&9E) O _S)_&9E3- // Enale&9E:' &9E)' &9E &9(C8 OJ _S)_C85)- // Set motor!rescaler to F

&9(C8 OJ 1_S)_C8B O _S)_C8B:3- //Set irled !rescaler to F &9SC5 J - // G:::HL &9SCB J )G:- &9 OJ 1_S)_: O _S)_) O _S)_3- // olarit initiall high &9C8 J 1_S)_C8: O_S)_C8) O_S)_C83- // se cloc6 S5 &9CT J :- //@o concatenate &9 channels &9C5E J :- //eft align 9D(( OJ 1_S)_9D((: O _S)_9D(() O _S)_9D((3- // Turn on ort T9a!!ing &9E(: J )::- //

Set !eriod to ):: tic6s &9E() J )::- //Set !eriod to ):: tic6s &9E( J )::- //Set !eriod to ):: tic6s &9DTQ: J :- // Dut ccleinitiall set to : &9DTQ) J :-//:- //Dut ccle initiall set to : &9DTQ J :- // Dut ccleinitiall set to :,/****************************************************************************

unction turn(ootarameters char The direction the root is su!!osed to turnK C& or CC&(eturns @othing.Descri!tion De!ending on turn' set directions for the corres!onding motors.****************************************************************************/static voidturn(oot1chardirection3


28/61

% // Tem!oraril sto! root sto!(oot13- // Set motor directions if1direction JJ C&3 % 9T(_ET_D0(ECT0@ J (&5(D- 9T(_(07HT_D0(ECT0@ J (EE(SE- , else if1direction JJ CC&3 % 9T(_ET_D0(ECT0@ J (EE(SE- 9T(_(07HT_D0(ECT0@ J (&5(D- , // Turn on motors 9T(_ET_DTQ J DC_T(@- 9T(_(07HT_DTQ J DC_T(@-,/****************************************************************************unction

drive(ootarameters char The direction of the root to move. char The s!eed at hich the root should move.(eturns @othing.Descri!tion Sets the direction of the root and the s!eed for it to move.****************************************************************************/static voiddrive(oot1chars!eed' chardirection3% // Tem!oraril sto! root sto!(oot13-

// Set motor directions if1direction JJ (&5(D3 % //&9 OJ 1_S)_: O _S)_) O _S)_3- 9T(_ET_D0(ECT0@ J (&5(D- 9T(_(07HT_D0(ECT0@ J (&5(D- , else if1direction JJ (EE(SE3 % //&9 PJ U1_S)_: O _S)_) O _S)_3- 9T(_ET_D0(ECT0@ J (EE(SE- 9T(_(07HT_D0(ECT0@ J (EE(SE- ,

// Set motor s!eeds 9T(_ET_DTQ J s!eed-//;F- leftS!eed J s!eed-//;F- //Stores left motor s!eed variales that ill e used for control loo!. 9T(_(07HT_DTQ J s!eed V +- rightS!eed J s!eed V +- //Stores right motor s!eed variales that ill e used for control loo!.,static voidslo(oot13% // Set motor s!eeds 9T(_ET_DTQ J DC_S&-//;F-


29/61

leftS!eed J DC_S&-//;F- //Stores left motor s!eed variales that ill e used for controlloo!. 9T(_(07HT_DTQ J DC_S& V +- rightS!eed J DC_S& V +- //Stores right motor s!eed variales that ill e used for controlloo!.,/****************************************************************************unction sto!(ootarameters @othing.(eturns @othing.Descri!tion Sto!s the root from moving.****************************************************************************/static voidsto!(oot1void3% //Set Dut Ccles of 9otors to : to sto! root 9T(_ET_DTQ J -

leftS!eed J - 9T(_(07HT_DTQ J - rightS!eed J -,static voidenaleositionControl1unsigned chardirection3% ultrasonicEnale J C@T(_E@5BED- controlDirection J direction- referenceDistance J 1distance V distance3 / - !uts1"Enale osition controlNr"3-,static voidenaleStationarControl1void3%

controlDirection J ST5T0@5(Q- !uts1"Enale Stationar controlNr"3-,static voiddisaleositionControl13% ultrasonicEnale J C@T(_D0S5BED- !uts1"disale controlNr"3-,floatgetBac6ltrasonicDistance1void3% returndistance+-,floatgetSideltrasonicDistance)1void3

% returndistance-,floatgetSideltrasonicDistance1void3% returndistance-,/***************************************************************************!ulic functions***************************************************************************/


30/61

/***************************************************************************interru!t res!onse routines***************************************************************************/

voidinterru!t _ec_tim:chF sonic_trigger13% static float8!J).2' 8dJ:.G' 8dorardJ:.2' 8d(everseJ)'!ositionError'lastErrorJ:-//!ositionError)' !ositionError'lastErrorJ:- signed chardutChange- static unsigned char!ulse_countJ:'stationar9ulti!lierJ- ES_Event (eturnEvent- (eturnEvent.EventT!e J ES_@_EE@T- // assume no errors T09:_T7)J_S)_CF- if1!ulse_count JJ :3 % T(5S@0C_ET_T J H07H- T(5S@0C_(07HT_T J H07H- !ulse_count J )- T09:_TCF J T09:_TCFVT09E(_E(0D_C@T- , else if1!ulse_count JJ )3

% T(5S@0C_ET_T J &- T(5S@0C_(07HT_T J &- trigger_send J T09:_TCF- T09:_TCF J T09:_TCFVT09E(_E(0D_57_C@T- !ulse_countVV- , else if1!ulse_count JJ 3 % !ulse_count J !ulse_countVV- if1ultrasonicEnale JJ C@T(_E@5BED3 //Control motors if motor control isenaled %

floatavgDistance J 11distance V distance3/3- !ositionError J referenceDistance ; 1 1 distance V distance 3 / 3- sitch1controlDirection3 //Different influences of control ased on the direction oftravel % case(&5(DK dutChange J 1char318!*!ositionError V 8dorard*1!ositionError ; lastError33- if1dutChange > G3 % dutChange J G- , else if1dutChange = ;G3 %

dutChange J ;G- , if11!ositionError > :.G3 OO 1!ositionError = ;:.G33 % 9T(_ET_DTQ J leftS!eed V dutChange- 9T(_(07HT_DTQ J rightS!eed ; dutChange-// V +- , else % if1dutChange = :3


31/61

% dutChange J ; dutChange- , if1 distance = distance 3 % 9T(_ET_DTQ J leftS!eed V dutChange- 9T(_(07HT_DTQ J rightS!eed ; dutChange- , else if1 distance > distance 3 % 9T(_ET_DTQ J leftS!eed ; dutChange- 9T(_(07HT_DTQ J rightS!eed V dutChange- , , lastError J !ositionError- rea6- case(EE(SEK dutChange J 1char318!*!ositionError V 8d(everse*1!ositionError ; lastError33- if1dutChange > G3 %

dutChange J G- , else if1dutChange = ;G3 % dutChange J ;G- , if11!ositionError > :.G3 OO 1!ositionError = ;:.G33 % 9T(_ET_DTQ J leftS!eed V dutChange- 9T(_(07HT_DTQ J rightS!eed ; dutChange V -// V +- , else %

if1dutChange = :3 % dutChange J ; dutChange- , if1 distance = distance 3 % 9T(_ET_DTQ J leftS!eed ; dutChange- 9T(_(07HT_DTQ J rightS!eed V dutChange V - , else if1 distance > distance 3 % 9T(_ET_DTQ J leftS!eed V dutChange- 9T(_(07HT_DTQ J rightS!eed ; dutChange V -

, , lastError J !ositionError- rea6- caseST5T0@5(QK % float!ositionError J distance ; distance- if11!ositionError > ;G3 PP 1!ositionError = G33 % if1!ositionError = ;:.G3


32/61

% 9T(_ET_D0(ECT0@ J (EE(SE- 9T(_(07HT_D0(ECT0@ J (&5(D- 9T(_ET_DTQ J 507@_T(@- 9T(_(07HT_DTQ J 507@_T(@- //9T(_ET_DTQ J stationar9ulti!lier*dutChange- , else if1!ositionError > :.G3 % 9T(_ET_D0(ECT0@ J (&5(D- 9T(_(07HT_D0(ECT0@ J (EE(SE- 9T(_ET_DTQ J 507@_T(@- 9T(_(07HT_DTQ J 507@_T(@- //9T(_ET_DTQ J stationar9ulti!lier*dutChange- , else % 9T(_ET_DTQ J - 9T(_(07HT_DTQ J - ,

, rea6- , , , T(5S@0C_B5C8_T J H07H- T09:_TCF J T09:_TCFVT09E(_E(0D_C@T- , else if1!ulse_count JJ +3 % !ulse_count J :- T(5S@0C_B5C8_T J &- trigger_send J T09:_TCF-

T09:_TCF J T09:_TCFVT09E(_E(0D_57_C@T- ,,voidinterru!t _ec_tim:chG echo_detect_function13% static floatcurrDistance J :- static floatlastDistance J :- T09:_T7)J_S)_CG- //!uts1"Enter echo) interru!t Nr"3- echo_detect J T09:_TCG- useconds J T0C8_(EST0@_uS*1echo_detect;trigger_send3- //Calculates microsecondscorres!onding to tic6s. currDistance J 1useconds3/1)F23;.?-

if11lastDistance WJ :3 PP 1currDistance > :3 PP 111currDistance ; lastDistance3 =S@0C_TE(5@CE3 PP 11lastDistance ; currDistance3 = S@0C_TE(5@CE333 % distance J currDistance- , lastDistance J currDistance-,voidinterru!t _ec_tim:ch echo_detect_function13% static floatcurrDistance J :-


33/61


34/61

Jousting Module:

Header:

/**************************************************************************** Header file for the Rousting module****************************************************************************/

#ifndef Aousting_H#define Aousting_H// Event Definitions

#include"ES_Configure.h"#include"ES_T!es.h"// t!edefs for the states// State definitions for use ith the $uer functiont!edefenum% (eadToAoust' Aoust(etracted' Aousting , Tem!lateState_t -// ulic unction rotot!esool0nitAousting1 uint2_triorit 3-oolostAousting1 ES_Event ThisEvent 3-ES_Event (unAousting1 ES_Event ThisEvent 3-Tem!lateState_t 4uerAousting1 void3-#endif/* Aousting_H */

Source:

/****************************************************************************9odule Aousting.c(evision ).:.)Descri!tion Aousting service for the )2B root****************************************************************************//*;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 0nclude iles ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;*//* include header files for this state machine as ell as an machines at the

ne#include"EventChec6ers.h"#include =hidef.h> /* common defines and macros */#include =mc?s)e)2.h> /* derivative information */#include =S)e)2its.h> /* it definitions */


35/61

#include =Bin_Const.h> /* macros to allo s!ecifing inarconstants */#include =termio.h> /* to get !rotot!e fo 6hit13 */#include"s)evec.h" /* E)2 interru!t vectors */#include =s)vec.h>#include"Servos.h"#include"Brain.h"/*;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule Defines ;;;;;;;;;;;;;;;;;;;;;;;;;;;;*/#include"[email protected]"/*;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule unctions ;;;;;;;;;;;;;;;;;;;;;;;;;;;*//* !rotot!es for !rivate functions for this machine.The should e functions relevant to the ehavior of this state machine*/static voidRoustingHardare0nit1void3-static voidde!loAoust1unsigned int!osition3-static voidretractAoust1void3-/*;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule ariales ;;;;;;;;;;;;;;;;;;;;;;;;;;;*/// everod needs a state variale' ou ma need others as ell.// t!e of state variale should match htat of enum in header filestaticTem!lateState_t CurrentState-

// ith the introduction of 7en' e need a module level riorit var as ellstatic uint2_t9riorit-/*;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule Code ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;*//****************************************************************************unction 0nitAoustingarameters uint2_t K the !riort of this service(eturns oolean' alse if error in initialiLation' True otheriseDescri!tion 0nitialiLes the Rousting module****************************************************************************/

ool0nitAousting1 uint2_triorit 3% 9riorit J riorit- // 0nit hardare for Rousting RoustingHardare0nit13- // Start in (eadToAoust state CurrentState J (eadToAoust- return true-,/****************************************************************************unction ostAoustingarameters

E_Event ThisEvent ' the event to !ost to the $ueue(eturns oolean alse if the En$ueue o!eration failed' True otheriseDescri!tion osts an event to the Aousting module****************************************************************************/oolostAousting1 ES_Event ThisEvent 3% returnES_ostToService1 9riorit' ThisEvent3-,


36/61

/****************************************************************************unction (unAoustingarameters ES_Event K the event to !rocess(eturns ES_Event' ES_@_EE@T if no error ES_E((( otheriseDescri!tion (uns the Aousting state machine****************************************************************************/ES_Event (unAousting1 ES_Event ThisEvent 3% ES_Event (eturnEvent- (eturnEvent.EventT!e J ES_@_EE@T- // assume no errors sitch1 CurrentState 3 % case(eadToAoust K // This state means that the root is read to Roust' ut the Roust is not de!loed if11ThisEvent.EventT!e JJ ES_C995@D3 PP 1ThisEvent.Eventaram JJDEQ_AST_BDQ33

% // Brain ants to Roust od. De!lo the Roust and set the current state to Rousting CurrentState J Aousting- de!loAoust1509_AST_BDQ3- , else if11ThisEvent.EventT!e JJ ES_C995@D3 PP 1ThisEvent.Eventaram JJDEQ_AST_HE5D33 % // Brain ants to Roust head. De!lo the Roust and set the current state to Rousting CurrentState J Aousting- de!loAoust1509_AST_HE5D3- , rea6-

caseAousting K // This state means the roots Roust is de!loed if11ThisEvent.EventT!e JJ ES_T09ET3 PP 1ThisEvent.Eventaram JJAST0@7_T09E(33 % // Three second limit is u!' retract the Roust CurrentState J Aoust(etracted- retractAoust13- , rea6- caseAoust(etracted K // This state means the roots Roust is retracted and it is not alloed // to Roust ecause it must ait ) second

if11ThisEvent.EventT!e JJ ES_T09ET3 PP 1ThisEvent.Eventaram JJAST0@7_T09E(33 % // ne second limit is u!' move to read to Roust state ES_Event ThisEvent- ThisEvent.EventT!e J ES_AST_(E5DQ- ostBrain1ThisEvent3- CurrentState J (eadToAoust- , rea6-


37/61

, return(eturnEvent-,/****************************************************************************unction 4uerAoustingarameters @one(eturns Tem!lateState_t The current state of the Aousting state machineDescri!tion returns the current state of the Aousting state machine****************************************************************************/Tem!lateState_t 4uerAousting1 void3% return1CurrentState3-,/***************************************************************************!rivate functions***************************************************************************/

/****************************************************************************unction RoustingHardare0nitarameters @othing.(eturns @othing.Descri!tion 0nitialiLes all hardare for the Rousting module' including the servo****************************************************************************/static voidRoustingHardare0nit1void3% enaleServo1AST_SE(3-

setulse&idth1AST_SE(' AST_(ET(5CTED3-,/****************************************************************************unction de!loAoustarameters @othing.(eturns @othing.Descri!tion De!los the Roust and starts a three second timer efore the Roust must

e retracted****************************************************************************/

static voidde!loAoust1unsigned int!osition3% setulse&idth1AST_SE(' !osition3- ES_Timer_0nitTimer1AST0@7_T09E(' TH(EE_SEC@DS3-,/****************************************************************************unction retractAoustarameters @othing.


38/61

(eturns @othing.Descri!tion

(etracts the Roust and starts a one second timer efore the Roust cane de!loed again

****************************************************************************/static voidretractAoust1void3% setulse&idth1AST_SE(' AST_(ET(5CTED3- ES_Timer_0nitTimer1AST0@7_T09E(' @E_SEC@D3-

,

Shooting Module:

Header:

/****************************************************************************


39/61

Header file for the shooting module****************************************************************************/

#ifndef Shooting_H#define Shooting_H// Event Definitions#include"ES_Configure.h"#include"ES_T!es.h"// t!edefs for the states// State definitions for use ith the $uer functiont!edefenum% Ball(ead' 7ate!en , Tem!lateState_t -// ulic unction rotot!esool0nitShooting1 uint2_triorit 3-oolostShooting1 ES_Event ThisEvent 3-ES_Event (unShooting1 ES_Event ThisEvent 3-Tem!lateState_t 4uerShooting1 void3-#endif/* Shooting_H */

Source:

/****************************************************************************9odule Shooting.c(evision ).:.)Descri!tion Shooting service for the )2B root****************************************************************************//*;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 0nclude iles ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;*//* include header files for this state machine as ell as an machines at the ne#include"EventChec6ers.h"#include =hidef.h> /* common defines and macros */#include =mc?s)e)2.h> /* derivative information */#include =S)e)2its.h> /* it definitions */#include =Bin_Const.h> /* macros to allo s!ecifing inarconstants */#include =termio.h> /* to get !rotot!e fo 6hit13 */#include"s)evec.h" /* E)2 interru!t vectors */#include =s)vec.h>

#include"Servos.h"#include"Brain.h"/*;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule Defines ;;;;;;;;;;;;;;;;;;;;;;;;;;;;*/#include"[email protected]"/*;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule unctions ;;;;;;;;;;;;;;;;;;;;;;;;;;;*//* !rotot!es for !rivate functions for this machine.The should e functions relevant to the ehavior of this state machine*/static voidshootingHardare0nit1void3-


40/61

static voidstartitching9achine9otor1void3-static voidsto!itching9achine9otor1void3-static voido!en7ate1unsigned chargate3-static voidclose7ate1unsigned chargate3-/*;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule ariales ;;;;;;;;;;;;;;;;;;;;;;;;;;;*/// everod needs a state variale' ou ma need others as ell.// t!e of state variale should match htat of enum in header filestaticTem!lateState_t CurrentState-// ith the introduction of 7en' e need a module level riorit var as ellstatic uint2_t9riorit-/*;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule Code ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;*//****************************************************************************unction 0nitShootingarameters uint2_t K the !riort of this service(eturns oolean' alse if error in initialiLation' True otheriseDescri!tion 0nitialiLes the shooting module

****************************************************************************/ool0nitShooting1 uint2_triorit 3% ES_Event ThisEvent- 9riorit J riorit- // 0nit shooting hardare shootingHardare0nit13- // Set CurrentState to Ball(ead and sto! the !itching motor CurrentState J Ball(ead- sto!itching9achine9otor13- return true-,/****************************************************************************

unction ostShootingarameters E_Event ThisEvent ' the event to !ost to the $ueue(eturns oolean alse if the En$ueue o!eration failed' True otheriseDescri!tion osts an event to the shooting module****************************************************************************/oolostShooting1 ES_Event ThisEvent 3% returnES_ostToService1 9riorit' ThisEvent3-,

/****************************************************************************unction (unShootingarameters ES_Event K the event to !rocess(eturns ES_Event' ES_@_EE@T if no error ES_E((( otheriseDescri!tion State machine for the shooting module****************************************************************************/


41/61

ES_Event (unShooting1 ES_Event ThisEvent 3% ES_Event (eturnEvent- (eturnEvent.EventT!e J ES_@_EE@T- // assume no errors sitch1 CurrentState 3 % caseBall(ead K if1ThisEvent.EventT!e JJ ES_C995@D3 % if1ThisEvent.Eventaram JJ SHT_B53 % // Brain ants to shoot all the alls' turn on the !itching machine // motor and release a all !uts1"Shooting alls commandNr"3- CurrentState J 7ate!en- startitching9achine9otor13- o!en7ate1B5_75TE3- ES_Timer_0nitTimer1B5_(EE5SE_T09E(' 75TE_E@_T09E3- , else if1ThisEvent.Eventaram JJ ST5(T_0TCH0@73

% startitching9achine9otor13- , else if1ThisEvent.Eventaram JJ ST_0TCH0@73 % sto!itching9achine9otor13- , , rea6- case7ate!en K if11ThisEvent.EventT!e JJ ES_T09ET3 PP 1ThisEvent.Eventaram JJB5_(EE5SE_T09E(33 %

// 5ll alls have een shot' !ost to rain and close the gate ES_Event BrainEvent- BrainEvent.EventT!e J ES_B5_SHT- ostBrain1BrainEvent3- CurrentState J Ball(ead- !uts1"Shooting com!lete' close gateNr"3- close7ate1B5_75TE3- sto!itching9achine9otor13- , rea6- , return(eturnEvent-,

/****************************************************************************unction 4uerShootingarameters @one(eturns Tem!lateState_t The current state of the shooting state machineDescri!tion returns the current state of the shooting state machine****************************************************************************/


42/61

Tem!lateState_t 4uerShooting1 void3% return1CurrentState3-,/***************************************************************************!rivate functions***************************************************************************/

/****************************************************************************unction shootingHardare0nitarameters @othing.(eturns @othing.Descri!tion 0nitialiLes all hardare for the shooting module' including the !itching

motor' all detector' and servo gate****************************************************************************/static voidshootingHardare0nit1void3%

B5_DETECT(_0(_0@E J 0@T- 0TCH0@7_9T(_0@E J TT- 0TCH0@7_9T( J &- enaleServo1B5_75TE3- setulse&idth1B5_75TE' 75TE_CSED3-,/****************************************************************************unction startitching9otorarameters @othing.(eturns @othing.

Descri!tion Turns the !itching motor on and starts a timer to turn the motor off

after a !re;determined amount of time****************************************************************************/static voidstartitching9achine9otor1void3% 0TCH0@7_9T( J H07H-,/****************************************************************************unction sto!itching9otorarameters @othing.

(eturns @othing.Descri!tion Turns off the !itching motor****************************************************************************/static voidsto!itching9achine9otor1void3% 0TCH0@7_9T( J &-,/****************************************************************************


43/61

unction o!en7atearameters gate numer(eturns @othing.Descri!tion (eleases a all o!ening the ac6 servo gate.****************************************************************************/static voido!en7ate1unsigned chargate3% setulse&idth1gate' 75TE_E@3-,/****************************************************************************unction closeront7atearameters gate numer(eturns @othing.

Descri!tion Closes the front servo gate****************************************************************************/static voidclose7ate1unsigned chargate3% setulse&idth1gate' 75TE_CSED3-,

Ball Dispensing Module:

Header:

/**************************************************************************** Header file for the all dis!ensing module****************************************************************************/

#ifndef BallDis!ensing_H#define BallDis!ensing_H// Event Definitions

#include"ES_Configure.h"#include"ES_T!es.h"// t!edefs for the states// State definitions for use ith the $uer functiont!edefenum% (eadToCollect' CollectingBall' ulsingEDs , Tem!lateState_t -// ulic unction rotot!esool0nitBallDis!ensing1 uint2_triorit 3-oolostBallDis!ensing1 ES_Event ThisEvent 3-ES_Event (unBallDis!ensing1 ES_Event ThisEvent 3-


44/61

Tem!lateState_t 4uerBallDis!ensing1 void3-#endif/* BallDis!ensing_H */

Source:

/****************************************************************************9odule BallDis!ensing.c(evision ).:.)Descri!tion Ball dis!ensing service for the )2B root****************************************************************************//*;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 0nclude iles ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;*//* include header files for this state machine as ell as an machines at the ne#include"EventChec6ers.h"#include =hidef.h> /* common defines and macros */#include =mc?s)e)2.h> /* derivative information */#include =S)e)2its.h> /* it definitions */#include =Bin_Const.h> /* macros to allo s!ecifing inarconstants */#include =termio.h> /* to get !rotot!e fo 6hit13 */#include"s)evec.h" /* E)2 interru!t vectors */#include =s)vec.h>#include"Brain.h"/*;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule Defines ;;;;;;;;;;;;;;;;;;;;;;;;;;;;*/#include"[email protected]"/*;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule unctions ;;;;;;;;;;;;;;;;;;;;;;;;;;;*//* !rotot!es for !rivate functions for this machine.The should e functions relevant to the ehavior of this state machine*/static voidallDis!ensingHardare0nit1void3-static void!ulse0(s1void3-static voidsto!ulsing0(s1void3-/*;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule ariales ;;;;;;;;;;;;;;;;;;;;;;;;;;;*/// everod needs a state variale' ou ma need others as ell.// t!e of state variale should match htat of enum in header filestaticTem!lateState_t CurrentState-

static unsigned charre$uestCountJ:'!ulseCountJ:-static unsigned inttimeoJFG:::'timeHighJ)G:::-// ith the introduction of 7en' e need a module level riorit var as ellstatic uint2_t9riorit-/*;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9odule Code ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;*//****************************************************************************unction 0nitBallDis!ensingarameters


45/61

uint2_t K the !riort of this service(eturns oolean' alse if error in initialiLation' True otheriseDescri!tion 0nitialiLes the all dis!ensing module****************************************************************************/ool0nitBallDis!ensing1 uint2_triorit 3% ES_Event ThisEvent- 9riorit J riorit- // 0nitialiLe hardare allDis!ensingHardare0nit13- // Start in the read state CurrentState J (eadToCollect- return true-,/****************************************************************************unction ostBallDis!ensingarameters

E_Event ThisEvent ' the event to !ost to the $ueue(eturns oolean alse if the En$ueue o!eration failed' True otheriseDescri!tion osts an event to the all dis!ensing module****************************************************************************/oolostBallDis!ensing1 ES_Event ThisEvent 3% returnES_ostToService1 9riorit' ThisEvent3-,/****************************************************************************unction (unBallDis!ensing

arameters ES_Event K the event to !rocess(eturns ES_Event' ES_@_EE@T if no error ES_E((( otheriseDescri!tion (uns the all dis!ensing state machine****************************************************************************/ES_Event (unBallDis!ensing1 ES_Event ThisEvent 3% ES_Event (eturnEvent- ES_Event BrainEvent- (eturnEvent.EventT!e J ES_@_EE@T- // assume no errors sitch1 CurrentState 3

% case(eadToCollectK // This state means the all dis!enser is read to collect a all if1ThisEvent.EventT!e JJ ES_CECT_B53 % !uts1"Collect a all commandNr"3- // The rain ants to collect a all. ulse the 0(s' start the timeout // timer and set the current state to CollectingBall !ulse0(s13- ES_Timer_0nitTimer1D0SE@S0@7_T09E(' D0SE@S0@7_T09E3-


46/61

CurrentState J ulsingEDs- , rea6- caseulsingEDsK if11ThisEvent.EventT!e JJ ES_T09ET3 PP 1ThisEvent.Eventaram JJ(E4EST_B5_T09E(33 % // Ball re$uest sent' sto! !ulsing- sto!ulsing0(s13- CurrentState J CollectingBall- , else if1ThisEvent.EventT!e JJ ES_CECT_B53 % // The rain is tring to collect a all efore it is read. ost an error BrainEvent.EventT!e J ES_B5_D0SE@S0@7_E(((- ostBrain1ThisEvent3- , rea6- caseCollectingBall K // This state means a all is eing collected. The rain should not tr to collect

// another all hile a all is eing collected. if11ThisEvent.EventT!e JJ ES_T09ET3 PP 1ThisEvent.Eventaram JJD0SE@S0@7_T09E(33 % // 0t has een three seconds since the start of all collection so it // is safe to collect another all CurrentState J (eadToCollect- BrainEvent.EventT!e J ES_B5_CECTED- ostBrain1BrainEvent3- , else if1ThisEvent.EventT!e JJ ES_B5_(E5DQ3 % // The all has een collected so move to read

CurrentState J (eadToCollect- BrainEvent.EventT!e J ES_B5_CECTED- ostBrain1BrainEvent3- , else if1ThisEvent.EventT!e JJ ES_CECT_B53 % // The rain is tring to collect a all efore it is read. ost an error BrainEvent.EventT!e J ES_B5_D0SE@S0@7_E(((- ostBrain1ThisEvent3- , rea6- , return(eturnEvent-

,/****************************************************************************unction 4uerBallDis!ensingarameters @one(eturns Tem!lateState_t The current state of the Tem!late state machineDescri!tion $ueries the state of the all dis!ensing module


47/61

****************************************************************************/Tem!lateState_t 4uerBallDis!ensing1 void3% return1CurrentState3-,/***************************************************************************!rivate functions***************************************************************************/

/****************************************************************************unction allDis!ensingHardare0nitarameters @othing.(eturns @othing.Descri!tion 0nitialiLes all hardare for the all dis!ensing module' including the

0( ED and the visile ED****************************************************************************/static voidallDis!ensingHardare0nit1void3

% 0(_ED_0@E J TT- (E5D_ED_0@E J TT- 0(_ED J &- (E5D_ED J &-,/****************************************************************************unction allDis!ensingTimer0nitarameters @othing.(eturns @othing.

Descri!tion 0nitialiLes the timer for doing !ulsing****************************************************************************//****************************************************************************unction !ulse0(sarameters @othing.(eturns @othing.Descri!tion Schedules the first out!ut com!are and resets the counter****************************************************************************/

static void!ulse0(s1void3% 0(_ED_DTQ J G- ES_Timer_0nitTimer1(E4EST_B5_T09E(' G::3-,/****************************************************************************unction sto!ulsing0(sarameters @othing.


48/61

(eturns @othing.Descri!tion Schedules the first out!ut com!are and resets the counter****************************************************************************/static voidsto!ulsing0(s1void3% 0(_ED_DTQ J :-,

Event Checkers:

Header:

/****************************************************************************9odule EventChec6ers.hDescri!tion header file for the event chec6ing functions@otes

Histor&hen &ho &hat/&h;;;;;;;;;;;;;; ;;; ;;;;;;;;:2/:/)+ )FK+I Rec started coding*****************************************************************************/#ifndef EventChec6ers_H#define EventChec6ers_H// !rotot!es for event chec6ers// !rotot!es for test event chec6ersoolBall(eadChec6er1void3-ool7oal(eachedChec6er1void3-ool&all5lignmentChec6er1void3-oolChec6F8estro6e1void3-

#include =itdefs.h>// #defines that go ith the Event Chec6ers#endif/* EventChec6ers_H */

Source:

/****************************************************************************


49/61

9odule EventChec6ers.c(evision ).:.)Descri!tion This is the sam!le for riting event chec6ers along ith the event chec6ers used in the asic frameor6 test harness.@otes @ote the use of static variales in sam!le event chec6er to detect @Q transitions.Histor&hen &ho &hat/&h;;;;;;;;;;;;;; ;;; ;;;;;;;;:2/:/)+ )+K+ Rec initial version****************************************************************************/// this ill !ull in the smolic definitions for events' hich e ill ant// to !ost in res!onse to detecting events#include"ES_Configure.h"// this ill get us the structure definition for events' hich e ill need// in order to !ost events in res!onse to detecting events

#include"ES_Events.h"// if ou ant to use distriution lists then ou need those function// definitions too.#include"ES_ostist.h"// This include ill !ull in all of the headers from the service modules// !roviding the !rotot!es for all of the !ost functions#include"ES_ServiceHeaders.h"// this test harness for the frameor6 references the serial routines that// are defined in ES_ort.c#include"ES_ort.h"// include our on !rotot!es to insure consistenc eteen header P// actual functionsdefinition#include"EventChec6ers.h"

// This include ill !ull in all of the headers from the service modules// !roviding the !rotot!es for all of the !ost functions#include"ES_ServiceHeaders.h"#include"[email protected]"#include"Brain.h"#include"@avigation.h"oolBall(eadChec6er1void3% static unsigned charastinState J :- unsigned charCurrentinState- ool(eturnal J false- CurrentinState J B5_DETECT(_0(- // chec6 for !in lo and different from last time

if11CurrentinState WJ astinState3 PP 1CurrentinState JJ &33 % // ost to shooting that a all is read ES_Event ThisEvent- ThisEvent.EventT!e J ES_B5_(E5DQ- //ost@avigation1ThisEvent3- //ostShooting1ThisEvent3- //TE9(5(Q C99E@TT &H0E C900@7 ST (eturnal J true- , astinState J CurrentinState-


50/61

return(eturnal-,ool7oal(eachedChec6er1void3% ool(eturnal J false- a2State_t navigationState J 4uer@avigation13- floatcurrDistance J getBac6ltrasonicDistance13- if1navigationState JJ DrivingToHome53 % if1currDistance =J H9E_5_S@0C_D0ST5@CE3 % ES_Event ThisEvent- ThisEvent.EventT!e J ES_H9E_5_DETECTED- ost@avigation1ThisEvent3- (eturnal J true- !rintf1"Distance J MfNr Nn"' currDistance3- , , else if1navigationState JJ DrivingToHomeB3 %

if1currDistance >J H9E_B_S@0C_D0ST5@CE3 % ES_Event ThisEvent- ThisEvent.EventT!e J ES_H9E_B_DETECTED- ost@avigation1ThisEvent3- (eturnal J true- !rintf1"Distance J MfNr Nn"' currDistance3- , , else if1navigationState JJ DrivingToDis!enser3 % if1currDistance =J D0SE@SE(_S@0C_D0ST5@CE3 %

ES_Event ThisEvent- ThisEvent.EventT!e J ES_D0SE@SE(_DETECTED- ost@avigation1ThisEvent3- (eturnal J true- !rintf1"Distance J MfNr Nn"' currDistance3- , , else if1navigationState JJ DrivingToTa!e3 % if1currDistance =J 1T5E_S@0C_D0ST5@CE V )33 % ES_Event ThisEvent- ThisEvent.EventT!e J ES_T5E_DETECTED-

ost@avigation1ThisEvent3- (eturnal J true- !rintf1"Distance J MfNr Nn"' currDistance3- , , else if1navigationState JJ DrivingTo7oal3 % if1currDistance >J 75_S@0C_D0ST5@CE3 % ES_Event ThisEvent-


51/61

ThisEvent.EventT!e J ES_75_DETECTED- ost@avigation1ThisEvent3- (eturnal J true- !rintf1"Distance J MfNr Nn"' currDistance3- , , return(eturnal-,ool&all5lignmentChec6er1void3% ool(eturnal J false- a2State_t navigationState J 4uer@avigation13- floatcurrDistanceDifference J getSideltrasonicDistance)13 ; getSideltrasonicDistance13- if1navigationState JJ 5ligning&ith&all3 % if11currDistanceDifference = :.)3 PP 1currDistanceDifference > ;:.)33 % ES_Event ThisEvent- ThisEvent.EventT!e J ES_&5_507@9E@T_C9ETE- ost@avigation1ThisEvent3-

(eturnal J true- !rintf1"&all 5lignment com!leteNr Nn"3- , , return(eturnal-,oolChec6F8estro6e1void3% if1 0s@e8e(ead13 3 // ne 6e aitingX % ES_Event ThisEvent- ES_Event @eEvent- ThisEvent.EventT!e J ES_@E&_8EQ-

ThisEvent.Eventaram J 7et@e8e13- // test distriution list functionalit sending the 6e out via // a distriution list. sitch1ThisEvent.Eventaram3 % case(K !uts1"7enerated ne round eventNr"3- @eEvent.EventT!e J ES_@E&_(@D- set5lloed(eload1true' true3- ostBrain1@eEvent3- rea6- case5K !uts1"7enerated home 5 detected eventNr"3-

@eEvent.EventT!e J ES_H9E_5_DETECTED- ost@avigation1@eEvent3- rea6- caseBK !uts1"7enerated home B detected eventNr"3- @eEvent.EventT!e J ES_H9E_B_DETECTED- ost@avigation1@eEvent3- rea6- caseDK !uts1"7enerated dis!enser detected eventNr"3-


52/61

@eEvent.EventT!e J ES_D0SE@SE(_DETECTED- ost@avigation1@eEvent3- rea6- case8K !uts1"7enerated 6night detected eventNr"3- @eEvent.EventT!e J ES_8@07HT_DETECTED- ost@avigation1@eEvent3- rea6- case7K !uts1"7enerated goal detected eventNr"3- @eEvent.EventT!e J ES_75_DETECTED- ost@avigation1@eEvent3- rea6- , return true- , return false-,


53/61

Servo Librar:

Header:

// Servos.h#ifndef Servos_H#define Servos_HvoidenaleServo1unsigned charchannel3-voidsetulse&idth1unsigned charchannel' unsigned intne&idth3-

#endif/* Servos_H */

Source:

#include =string.h>#include =stdio.h>#include =hidef.h> /* common defines and macros */#include =mc?s)e)2.h> /* derivative information */#include =S)e)2its.h> /* it definitions */

#include =Bin_Const.h> /* macros to allo s!ecifing inarconstants */#include =termio.h> /* to get !rotot!e fo 6hit13 */#include =s)vec.h>#include"Servos.h"#define @9_CH5@@ES F#define 0@0T05_S0T0@ :


54/61

%_S)_0SI' PT09)_TCT)' 1_S)_9I O _S)_I3' _S)_TI' PT09)_TCI ,,-/****************************************************************************unction enaleServoarameters Channel on ort T to enale servo 1eteen F and I3(eturns @othing.Descri!tion

0nitialiLes the corres!onding !in on ort T as a servo !inand sets it to the center !osition

****************************************************************************/voidenaleServo1unsigned charchannel3% unsigned charinde< J channel ; @9_CH5@@ES- T09)_TSC() OJ _S)_TE@- // Enale Timer ) T09)_TSC( J _S)_() O _S)_(:- // Set !rescaler to /2 1+9HL timer cloc63 T09)_T0S OJ ChannelsYinde


55/61

ES!Con"igure#h

/****************************************************************************9odule ES_Configure.hDescri!tion This file contains macro definitions that are edited the user to ada!t the Events and Services frameor6 to a !articular a!!lication.@otesHistor&hen &ho &hat/&h;;;;;;;;;;;;;; ;;; ;;;;;;;;):/)/)+ :KGF Rec lots of added entries to ring the numer of timers and services u! to ) each:2/:/)+ )FK): Rec removed ost8eunc stuff since e are moving that

functionalit out of the frameor6 and !utting it e

#define @9_SE(0CES /****************************************************************************/// These are the definitions for Service :' the loest !riorit service.// Ever Events and Services a!!lication must have a Service :. urther// services are added in numeric se$uence 1)''+'...3 ith increasing// !riorities#define SE(_:_HE5DE("Brain.h"// the name of the 0nit function#define SE(_:_0@0T 0nitBrain// the name of the run function#define SE(_:_(@ (unBrain// Ho ig should this services 4ueue eX#define SE(_:_4EE_S0[E +

/****************************************************************************/// The folloing sections are used to define the !arameters for each of the// services. Qou onl need to fill out as man as the numer of services// defined @9_SE(0CES/****************************************************************************/// These are the definitions for Service )#if @9_SE(0CES > )// the header file ith the !ulic fuction !rotot!es#define SE(_)_HE5DE("@avigation.h"


56/61

// the name of the 0nit function#define SE(_)_0@0T 0nit@avigation// the name of the run function#define SE(_)_(@ (un@avigation// Ho ig should this services 4ueue eX#define SE(_)_4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service #if @9_SE(0CES > // the header file ith the !ulic fuction !rotot!es#define SE(__HE5DE("Aousting.h"// the name of the 0nit function#define SE(__0@0T 0nitAousting// the name of the run function#define SE(__(@ (unAousting// Ho ig should this services 4ueue eX#define SE(__4EE_S0[E +#endif/****************************************************************************/

// These are the definitions for Service +#if @9_SE(0CES > +// the header file ith the !ulic fuction !rotot!es#define SE(_+_HE5DE("Shooting.h"// the name of the 0nit function#define SE(_+_0@0T 0nitShooting// the name of the run function#define SE(_+_(@ (unShooting// Ho ig should this services 4ueue eX#define SE(_+_4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service F

#if @9_SE(0CES > F#define SE(_F_HE5DE("BallDis!ensing.h"// the name of the 0nit function#define SE(_F_0@0T 0nitBallDis!ensing// the name of the run function#define SE(_F_(@ (unBallDis!ensing// Ho ig should this services 4ueue eX#define SE(_F_4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service G#if @9_SE(0CES > G// the header file ith the !ulic fuction !rotot!es

#define SE(_G_HE5DE("Comm.h"// the name of the 0nit function#define SE(_G_0@0T 0nitComm// the name of the run function#define SE(_G_(@ (unComm// Ho ig should this services 4ueue eX#define SE(_G_4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service


57/61

#if @9_SE(0CES > // the header file ith the !ulic fuction !rotot!es#define SE(__HE5DE("TestHarnessService.h"// the name of the 0nit function#define SE(__0@0T 0nitTestHarnessService// the name of the run function#define SE(__(@ (unTestHarnessService// Ho ig should this services 4ueue eX#define SE(__4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service I#if @9_SE(0CES > I// the header file ith the !ulic fuction !rotot!es#define SE(_I_HE5DE("TestHarnessServiceI.h"// the name of the 0nit function#define SE(_I_0@0T 0nitTestHarnessServiceI// the name of the run function#define SE(_I_(@ (unTestHarnessServiceI// Ho ig should this services 4ueue eX

#define SE(_I_4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service 2#if @9_SE(0CES > 2// the header file ith the !ulic fuction !rotot!es#define SE(_2_HE5DE("TestHarnessService2.h"// the name of the 0nit function#define SE(_2_0@0T 0nitTestHarnessService2// the name of the run function#define SE(_2_(@ (unTestHarnessService2// Ho ig should this services 4ueue eX#define SE(_2_4EE_S0[E +

#endif/****************************************************************************/// These are the definitions for Service ?#if @9_SE(0CES > ?// the header file ith the !ulic fuction !rotot!es#define SE(_?_HE5DE("TestHarnessService?.h"// the name of the 0nit function#define SE(_?_0@0T 0nitTestHarnessService?// the name of the run function#define SE(_?_(@ (unTestHarnessService?// Ho ig should this services 4ueue eX#define SE(_?_4EE_S0[E +#endif

/****************************************************************************/// These are the definitions for Service ):#if @9_SE(0CES > ):// the header file ith the !ulic fuction !rotot!es#define SE(_):_HE5DE("TestHarnessService):.h"// the name of the 0nit function#define SE(_):_0@0T 0nitTestHarnessService):// the name of the run function#define SE(_):_(@ (unTestHarnessService):// Ho ig should this services 4ueue eX


58/61

#define SE(_):_4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service ))#if @9_SE(0CES > ))// the header file ith the !ulic fuction !rotot!es#define SE(_))_HE5DE("TestHarnessService)).h"// the name of the 0nit function#define SE(_))_0@0T 0nitTestHarnessService))// the name of the run function#define SE(_))_(@ (unTestHarnessService))// Ho ig should this services 4ueue eX#define SE(_))_4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service )#if @9_SE(0CES > )// the header file ith the !ulic fuction !rotot!es#define SE(_)_HE5DE("TestHarnessService).h"// the name of the 0nit function

#define SE(_)_0@0T 0nitTestHarnessService)// the name of the run function#define SE(_)_(@ (unTestHarnessService)// Ho ig should this services 4ueue eX#define SE(_)_4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service )+#if @9_SE(0CES > )+// the header file ith the !ulic fuction !rotot!es#define SE(_)+_HE5DE("TestHarnessService)+.h"// the name of the 0nit function#define SE(_)+_0@0T 0nitTestHarnessService)+

// the name of the run function#define SE(_)+_(@ (unTestHarnessService)+// Ho ig should this services 4ueue eX#define SE(_)+_4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service )F#if @9_SE(0CES > )F// the header file ith the !ulic fuction !rotot!es#define SE(_)F_HE5DE("TestHarnessService)F.h"// the name of the 0nit function#define SE(_)F_0@0T 0nitTestHarnessService)F// the name of the run function

#define SE(_)F_(@ (unTestHarnessService)F// Ho ig should this services 4ueue eX#define SE(_)F_4EE_S0[E +#endif/****************************************************************************/// These are the definitions for Service )G#if @9_SE(0CES > )G// the header file ith the !ulic fuction !rotot!es#define SE(_)G_HE5DE("TestHarnessService)G.h"// the name of the 0nit function


59/61

#define SE(_)G_0@0T 0nitTestHarnessService)G// the name of the run function#define SE(_)G_(@ (unTestHarnessService)G// Ho ig should this services 4ueue eX#define SE(_)G_4EE_S0[E +#endif/****************************************************************************/// @ame/define the events of interest// niversal events occu! the loest entries' folloed user;defined eventst!edefenum% ES_@_EE@T J :' ES_E(((' /* used to indicate an error from the service */ ES_0@0T' /* used to transition from initial !seudo;state */ ES_T09ET' /* signals that the timer has e

ES_SE@D_C995@D' /* tells comm to send F te message to AS( */ ES_BQTE_(E5D' /* !osted S0 interru!t that it read a te */ // @avigation Events ES_C995@D' ES_@5075T0@_C9ETE' ES_@5075T0@_50(E' ES_0@@E(_0@0T' ES_8@07HT_DETECTED' ES_75_DETECTED' ES_H9E_5_DETECTED' ES_H9E_B_DETECTED' ES_&5_507@9E@T_C9ETE' ES_T5E_DETECTED'

ES_DET_(E5CHED' ES_D0SE@SE(_DETECTED' // Shooting events ES_B5_(E5DQ' ES_SHT_B5' ES_B5_SHT' ES_B5_E(((' // Aousting events ES_DEQ_AST_BDQ' ES_DEQ_AST_HE5D' ES_(ET(5CT_AST' ES_AST_(E5DQ' ES_AST_E((('

// Ball dis!ensing events ES_CECT_B5' ES_B5_CECTED' ES_B5_D0SE@S0@7_E((( , ES_EventT!_t -/****************************************************************************/// These are the definitions for the Distriution lists. Each definition// should e a comma se!erated list of !ost functions to indicate hich// services are on that distriution list.#define @9_D0ST_0STS


60/61


61/61

// to different timers if the need arises. 8ee! these definitons close to the// definitions for the res!onse functions to ma6e it easier to chec6 that// the timer numer matches here the timer event ill e routed// These smolic names should e changed to e relevant to our a!!lication#define SE(0CE:_T09E( )G#define SE(0CE)_T09E( )F#define SE(0CE_T09E( )+#define SE(0CE+_T09E( )#define (E4EST_B5_T09E( ))#define (E4EST_ST5TS_T09E( ):#define (E4EST_SC(E_T09E( ?#define C99_T09E( 2#define B(50@_T09E( I#define 507@9E@T_DEB@CE_T09E( #define D0SE@S0@7_T09E( G#define AST0@7_T09E( F#define B5_(EE5SE_T09E( +#define 0TCH0@7_T09E( #define @5075T0@_T09E( )#define T(@_T09E( :

#endif/* C@07(E_H */

Documents

Code Listings