/**
* @Function RunTimerService(ES_Event ThisEvent)
* @param ES_Event - the event to process
* @return ES_NO_EVENT or ES_ERROR
* @brief accepts the timer events and updates the state arrays
* @author Max Dunne 2013.01.04 */
ES_Event RunTimerService(ES_Event ThisEvent) {
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
switch (ThisEvent.EventType) {
case ES_INIT:
break;
case ES_TIMEOUT:
case ES_TIMERACTIVE:
case ES_TIMERSTOPPED:
UserTimerStates[ThisEvent.EventParam] = ThisEvent.EventType;
break;
default:
//ReturnEvent.EventType = ES_ERROR;
break;
}
#ifdef TIMER_SERVICE_TEST
{
uint8_t i;
switch (timerServiceTestingState) {
case init:
if (ThisEvent.EventType == ES_INIT) {
printf("Timer Module INITED succesfully\r\n");
break;
}
printf("Timer %d had event %d at time %d\r\n", ThisEvent.EventParam, ThisEvent.EventType, ES_Timer_GetTime());
if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMERACTIVE)) {
timerServiceTestingState = expired;
printf("Testing timer user functions [expired][stopped][active]{state}\r\n");
}
break;
case expired:
for (i = 0; i < TIMERS_USED; i++) {
printf("(%d):[%d,%d,%d]{%d} ", i, IsTimerExpired(i), IsTimerStopped(i), IsTimerActive(i), GetUserTimerState(i));
}
printf("\r\n");
if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMEOUT)) {
timerServiceTestingState = runstop;
ES_Timer_InitTimer(0, 500);
for (i = 1; i < TIMERS_USED; i++) {
ES_Timer_SetTimer(i, 1000);
ES_Timer_StartTimer(i);
}
}
break;
case runstop:
printf("Timer %d had event %d at time %d\r\n", ThisEvent.EventParam, ThisEvent.EventType, ES_Timer_GetTime());
if ((ThisEvent.EventParam == (0)) && (ThisEvent.EventType == ES_TIMEOUT)) {
for (i = 1; i < TIMERS_USED; i++) {
ES_Timer_StopTimer(i);
}
}
if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMERSTOPPED)) {
printf("Testing of User Timer Functions is complete.\r\n");
}
break;
default:
ReturnEvent.EventType = ES_ERROR;
break;
}
}
#endif
//ES_Tail();
return ReturnEvent;
}
//The interrupt response for our phototransistor
void PhotoTransistor_InterruptResponse(void)
{
// Clear Interrupt
clearCaptureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
// Determine Capture time
uint32_t ThisCapture = captureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
// Calculate period
uint32_t Period = ((ThisCapture - LastCapture) * MICROSECONDS_DIVISOR ) / TICKS_PER_MS;
//Store the Last Cpature
LastCapture = ThisCapture;
//Iterate Through the different frequency options
for (int i = 0; i < NUMBER_BEACON_FREQUENCIES; i++)
{
// If the period matches a beacon period
if (ToleranceCheck(Period, beacons[i].period, PERIOD_MEASURING_ERROR_TOLERANCE))
{
// If we're searching for a beacon
if (Bucketing)
{
buckets[i]++;
}
// If we're supposed to align to the bucket, and the number of pulses we've seen for
// this beacon is greater than our threshold
if ((AligningToBucket) && (buckets[i] >= NUMBER_PULSES_FOR_BUCKET))
{
// If this is the beacon corresponding to our target bucket
if (((MyColor() == COLOR_BLUE) && (i == BEACON_INDEX_NW)) || ((MyColor() == COLOR_RED) && (i == BEACON_INDEX_SE)))
{
// stop our drive
clearDriveAligningToBucket();
// Post an aligned event
ES_Event AlignedEvent;
AlignedEvent.EventType = ES_ALIGNED_TO_BUCKET;
PostMasterSM(AlignedEvent);
// stop aligning
AligningToBucket = false;
// reset all buckets
for (int j = 0; j < NUMBER_BEACON_FREQUENCIES; j++)
{
buckets[j] = 0;
}
// reset average information
ResetAverage();
// return to searching for beacons
Bucketing = true;
LastBeacon = NULL_BEACON;
ES_Timer_StopTimer(AVERAGE_BEACONS_TIMER);
return;
}
}
// If we're not aligning to a bucket, and the number of pulses we've seen for this
// beacon is greater than our threshold
else if (buckets[i] >= NUMBER_PULSES_TO_BE_ALIGNED && LastBeacon == NULL_BEACON && !AligningToBucket)
{
// store the beacon to be recorded
LastBeacon = i;
// reset all buckets
for (int j = 0; j < NUMBER_BEACON_FREQUENCIES; j++)
{
buckets[j] = 0;
}
// restart the timer to evaluate the beacon
ES_Timer_InitTimer(AVERAGE_BEACONS_TIMER, AVERAGE_BEACONS_T);
Bucketing = false;
}
// increment the sum of all encoder angles for this beacon
sums[i] += GetPeriscopeAngle();
// increment the number of pulses seen for this beacon
numSamples[i]++;
// If the evaluation timer is already running, restart it
ES_Timer_SetTimer(AVERAGE_BEACONS_TIMER, AVERAGE_BEACONS_T);
}
}
}
/**
* @Function RunTemplateService(ES_Event ThisEvent)
* @param ThisEvent - the event (type and param) to be responded.
* @return Event - return event (type and param), in general should be ES_NO_EVENT
* @brief This function is where you implement the whole of the service,
* as this is called any time a new event is passed to the event queue.
* @note Remember to rename to something appropriate.
* Returns ES_NO_EVENT if the event have been "consumed."
* @author J. Edward Carryer, 2011.10.23 19:25 */
ES_Event RunTestBumpService(ES_Event ThisEvent)
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
static unsigned int bumperArray[] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, LastbumpState = 0x00, currentLoadTapeState[] = {0, 0, 0, 0, 0}, lastLoadTapeState = 0;
// static unsigned int bumperArray2[] = {0x00,0x00,0x00,0x00,0x00,0x00}, LastbumpState2 = 0x00;
static uint8_t index = 0, loadindex = 0;
int count = 0, loadcount = 0;
ES_Event LoaderEvent;
/********************************************
in here you write your service code
*******************************************/
if (ThisEvent.EventType == ES_TIMEOUT)// only respond to ES_Init
{
ES_Timer_SetTimer(bumptimer, bumptimervalue); // Bump sensor check every 5ms
ES_Timer_StartTimer(bumptimervalue);
// No hardware initialization or single time setups, those
// go in the init function above.
//
// This section is used to reset service for some reason
bumperArray[index] = Dagobot_ReadBumpers();
currentLoadTapeState[index] = Dagobot_ReadLoaderTape();
int i;
for(i=1; i<10; i++){
if(bumperArray[i-1] != bumperArray[index] ){
LastbumpState = bumperArray[index];
count++;
}
else
count--;
}
for (i = 1; i < 5; i++) {
if (currentLoadTapeState[i - 1] != currentLoadTapeState[index]) {
loadcount ++;
lastLoadTapeState = currentLoadTapeState[loadindex];
}
else
loadcount --;
}
if(count > 0) {
ThisEvent.EventType = BUMPED;
ThisEvent.EventParam = bumperArray[index];
PostDagobotHSM(ThisEvent);
}
if(loadcount > 0) {
LoaderEvent.EventType = AMMO;
LoaderEvent.EventParam = currentLoadTapeState[loadindex];
PostDagobotHSM(LoaderEvent);
}
}
index++;
if(index == 10) index = 0;
if (loadindex == 5) loadindex = 0;
return ReturnEvent;
}
