void vtHandleFatalError(int code,int line,char file[]) {
static unsigned int delayCounter = 0;
// There are lots of ways you can (and may want to) handle a fatal error
// In this implementation, I suspend tasks and then flash the LEDs. Note that the stop may not be
// immediate because this task has to be scheduled first for that to happen. In fact, one task might
// call this while another tries to get into
taskENTER_CRITICAL();
taskDISABLE_INTERRUPTS();
/* LEDs on ports 1 and 2 to output (1). */
// Note that all LED access is through the proper LPC library calls (or my own routines that call them)
vtInitLED();
for (;;) {
// Silly delay loop, but we really don't have much choice here w/o interrupts and FreeRTOS...
// This won't be okay to do *anywhere* else in your code
for (delayCounter=0;delayCounter<10000000;delayCounter++) {
}
// Turn off half and on half
vtLEDOn(0xAA);
vtLEDOff(0x55);
// Delay again
for (delayCounter=0;delayCounter<10000000;delayCounter++) {
}
// Toggle
vtLEDOff(0xAA);
vtLEDOn(0x55);
// Here is some dumb code to make sure that the three input parameters are not optimized away by the compiler
if ((code < -55) && (line < -55) && (file == NULL)) {
vtLEDOff(0x0); // We won't get here
}
// End of dumb code
}
// We will never get here
taskEXIT_CRITICAL();
}
// This is the actual task that is run
static portTASK_FUNCTION( navigationUpdateTask, pvParameters )
{
// Get the parameters
navStruct *navData = (navStruct *) pvParameters;
// Buffer for receiving messages
g9Msg msgBuffer;
uint8_t numLap=0;
uint8_t lineDist=0;
uint16_t curHeading=0;
curState = straight;
portTickType ticksAtStart=0; //in ticks
portTickType tagTime=0; //Distance where tag was found
// Like all good tasks, this should never exit
for(;;)
{
int16_t enc[2] = {0,0}; //Encoder value
// Wait for a message from the "otherside"
portBASE_TYPE ret;
if ((ret = xQueueReceive(navData->inQ,(void *) &msgBuffer,1500) ) != pdTRUE) {
//VT_HANDLE_FATAL_ERROR(ret);
printw_err("NavTask Didn't RCV From PIC!\n");
setMotorData(&motorData,64,64);
setState("Stopped");
goto end; //Skip everything send stop
}
g9Msg msg;
// Now, based on the type of the message, we decide on the action to take
switch (msgBuffer.msgType){
case navLineFoundMsg:
//stop we have found the finish line!!!
lineFound = TRUE;
lineDist = totDist;
break;
case navIRDataMsg:
//Save the data
LEFT_FRONT_IR = ir2Dist(msgBuffer.buf[0],LEFT_FRONT_IR);
RIGHT_FRONT_IR = ir2Dist(msgBuffer.buf[1],RIGHT_FRONT_IR);
LEFT_BACK_IR = ir2Dist(msgBuffer.buf[2],LEFT_BACK_IR);
RIGHT_BACK_IR = ir2Dist(msgBuffer.buf[3],RIGHT_BACK_IR);
SONAR_LEFT = ir2Dist(msgBuffer.buf[4],SONAR_LEFT);
SONAR_RIGHT = ir2Dist(msgBuffer.buf[5],SONAR_RIGHT);
oldIR[0][irIndex] = LEFT_FRONT_IR;
oldIR[1][irIndex] = RIGHT_FRONT_IR;
oldIR[2][irIndex] = LEFT_BACK_IR;
oldIR[3][irIndex] = RIGHT_BACK_IR;
oldIR[4][irIndex] = SONAR_LEFT;
oldIR[5][irIndex] = SONAR_RIGHT;
irIndex = (irIndex+1)%5;
/*
LEFT_FRONT_IR = avgIr(oldIR[0]);
RIGHT_FRONT_IR = avgIr(oldIR[1]);
LEFT_BACK_IR = avgIr(oldIR[2]);
RIGHT_BACK_IR = avgIr(oldIR[3]);
SONAR_LEFT = avgIr(oldIR[4]);
SONAR_RIGHT = avgIr(oldIR[5]);*/
msg.msgType = webIRMsg;
msg.id = 0; //internal
msg.length = 6*sizeof(uint8_t);
memcpy(msg.buf,IR,msg.length);
SendConductorMsg(&msg,10);
msg.msgType = webPowerMsg;
msg.id = 0; //internal
msg.length = sizeof(uint16_t);
((uint16_t*)msg.buf)[0] = (uint16_t)(msgBuffer.buf[6])*2;
SendConductorMsg(&msg,10);
break;
case navEncoderMsg:
{
static portTickType oldTick=0;
portTickType newTick = xTaskGetTickCount();
//Get encoder values
enc[0] = ((short*)msgBuffer.buf)[0];
enc[1] = ((short*)msgBuffer.buf)[1];
totDist += enc2Dist((enc[0]+enc[1])/2);
//get current heading
uint16_t tAngle = enc2Ang(enc[0], enc[1]);
if(tAngle<0)
tAngle = 360 - tAngle;
curHeading += tAngle;
if(curHeading>359)
curHeading -= 360;
//calculate poll rate
enc_poll_rate = ((newTick-oldTick)*portTICK_RATE_MS);
oldEnc[0] = enc[0];
oldEnc[1] = enc[1];
oldTick=newTick;
break;
}
case navRFIDFoundMsg:
//Save the data and make a decision
if( (xTaskGetTickCount() - tagTime)*portTICK_RATE_MS > 3000 ){
if( !(tagValue & Finish) && (msgBuffer.buf[0] == Finish) ){
if( (numLap++ == 0) && inputs.loop==1 ){
disableTag(Finish);
}
}
printw_err("Handle tag: 0x%X\n",msgBuffer.buf[0]);
tagTime = xTaskGetTickCount(); //Found tag record where
tagValue |= msgBuffer.buf[0]; //Store tag info
}
break;
case navWebInputMsg:
{
//printw("Inputs are HERE!\n");
uint8_t oldStart = inputs.start;
inputs = ((webInput_t*)msgBuffer.buf)[0];
if( oldStart == 0 && inputs.start == 1 ){
printw("<b style=color:green>Starting Navigation</b>\n");
//Get start time (in ticks)
ticksAtStart = xTaskGetTickCount();
//Reset Distance
totDist = 0;
//Reset RFID tags
tagValue = 0;
}
break;
}
default:
//Invalid message type - Should have been handled in conductor
VT_HANDLE_FATAL_ERROR(INVALID_G9MSG_TYPE);
break;
}
curMemLoc = (totDist/TRACK_MEM_DIST) % TRACK_MEM_SIZE;
trackMem[curMemLoc]->left = LEFT_FRONT_IR;
trackMem[curMemLoc]->right = RIGHT_FRONT_IR;
trackMem[curMemLoc]->heading = curHeading;
if(inputs.start==1){
setState("Navigate");
stateMachine();
handleSpecialEvents(enc);
//Do additional slow down if line was found
if( lineFound ){
int speed = enc2Speed(oldEnc[0],oldEnc[1]);
if( speed > 10 )
speed -= 8;
if( speed < -10 )
speed += 8;
adjustSpeed(oldEnc[0],oldEnc[1],speed,0);
}
//if we can't find the line in 20 cm, lets stop looking
if(lineDist <totDist +20)
lineFound = FALSE;
}
else{
setMotorData(&motorData,64,64);
setState("Stopped");
}
end: msg.msgType = navMotorCmdMsg;
msg.id = msgBuffer.id + 1;
msg.length = 2;
msg.buf[0] = motorData.left;
msg.buf[1] = motorData.right;
vtLEDOn(0x01);
SendZigBeeMsg(navData->zigBeePtr,&msg,portMAX_DELAY);
vtLEDOff(0x01);
//Send off web information
msg.msgType = webMotorMsg;
msg.id = 0; //internal
msg.length = 2;
getMotorData(&motorData,&msg.buf[0],&msg.buf[1]);
SendConductorMsg(&msg,10);
msg.msgType = webSpeedMsg;
msg.id = 0; //internal
msg.length = 2*sizeof(int);
((int*)msg.buf)[0] = (int)enc2Speed(oldEnc[0],oldEnc[1]);
((int*)msg.buf)[1] = (1000*totDist)/((xTaskGetTickCount()-ticksAtStart)*portTICK_RATE_MS); // cm/(ticks*(ms/ticks)) = cm/ms * 1000 ms/s = cm/s
SendConductorMsg(&msg,10);
msg.msgType = webStateMsg;
msg.id = 0; //internal
msg.length = strlen(state);
strcpy((char*)msg.buf,state);
SendConductorMsg(&msg,10);
if( ~(tagValue & Finish) && (inputs.start == 1)){
msg.msgType = webTimeMsg;
msg.id = 0; //internal
msg.length = 2*sizeof(unsigned int);
((unsigned int*)msg.buf)[1] = (xTaskGetTickCount()-ticksAtStart)*portTICK_RATE_MS; //actual
((unsigned int*)msg.buf)[0] = (((unsigned int*)msg.buf)[1]*33)/((1000*totDist)/((unsigned int*)msg.buf)[1]); // nominal = (actual)* 33/(avg Speed)
SendConductorMsg(&msg,10);
}
msg.msgType = webNavMsg;
msg.id = 0; //internal
msg.length = 3*sizeof(uint8_t) + 2*sizeof(int16_t);
msg.buf[0] = (tagValue & Finish) > 0;
msg.buf[1] = numLap;
msg.buf[2] = tagValue;
memcpy(&(msg.buf[3]),oldEnc,2*sizeof(int16_t));
SendConductorMsg(&msg,10);
}
}
