// This is the actual task that is run static portTASK_FUNCTION( vi2cSensorUpdateTask, pvParameters ) { // Get the parameters vtInfraredStruct *param = (vtInfraredStruct *) pvParameters; // Get the I2C device pointer vtI2CStruct *devPtr = param->dev; // Get the LCD information pointer vtLCDStruct *lcdData = param->lcdData; // Get the Navigation task pointer vtNavStruct *navData = param->navData; // String buffer for printing char lcdBuffer[vtLCDMaxLen+1]; // Buffer for receiving messages vtInfraredMsg msgBuffer; // Assumes that the I2C device (and thread) have already been initialized // Like all good tasks, this should never exit for(;;) { // Wait for a message from either a timer or from an I2C operation if (xQueueReceive(param->inQ,(void *) &msgBuffer,portMAX_DELAY) != pdTRUE) { printf("error getting sensor msg\n"); //VT_HANDLE_FATAL_ERROR(0); continue; } // Now, based on the type of the message, we decide on the action to take switch(getMsgType(&msgBuffer)) { case SensorMsgTypeTimer: { #if DEBUG == 1 GPIO_SetValue(0,0x20000); #endif // Send query to the sensors if (vtI2CEnQ(devPtr,vtI2CMsgTypeSensorRead,0x4F,sizeof(i2cCmdReadVals),i2cCmdReadVals,vtInfraredMaxLen) != pdTRUE) { // If we can't get a complete message from the rover in time, give up and try again printf("couldn't get sensor data back in time\n"); break; } #if DEBUG == 1 GPIO_ClearValue(0,0x20000); #endif break; } case vtI2CMsgTypeSensorRead: { // Ensure msg was intended for this task. If not, break out and wait for next sensor msg if (msgBuffer.buf[0] != 0xF0) { break; } // Check msg integrity. uint8_t i; uint8_t badMsg = 0; for (i = 1; i < vtInfraredMaxLen; i++) { if (msgBuffer.buf[i] == 0xF0 || msgBuffer.buf[i] == 0xF1) badMsg = 1; } // If we've gotten a bad msg, break and wait for next sensor msg if (badMsg) { break; } #if DEBUG == 1 printf("Distance1 %d inches\n",msgBuffer.buf[1]); printf("Distance2 %d inches\n",msgBuffer.buf[2]); printf("Distance3 %d inches\n",msgBuffer.buf[3]); printf("Distance4 %d inches\n",msgBuffer.buf[4]); printf("Distance5 %d inches\n",msgBuffer.buf[5]); printf("Distance6 %d inches\n",msgBuffer.buf[6]); #endif // if (lcdData != NULL) { // for (i = 0; i < vtInfraredMaxLen; i++) { // sprintf(lcdBuffer,"d%d=%d inches",i,msgBuffer.buf[i]); // if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),lcdBuffer,portMAX_DELAY) != pdTRUE) { // VT_HANDLE_FATAL_ERROR(0); // } // if (distance1 != 0) { // if (SendLCDGraphMsg(lcdData,distance1,portMAX_DELAY) != pdTRUE) { // VT_HANDLE_FATAL_ERROR(0); // } // } // } // } // Send data to navigation task if (navData != NULL) { if (SendNavValueMsg(navData,vtNavMsgSensorData,&msgBuffer.buf[1],portMAX_DELAY) != pdTRUE) { printf("error sending sensor data to nav\n"); VT_HANDLE_FATAL_ERROR(0); } } break; } default: { printf("bad sensor msg\n"); VT_HANDLE_FATAL_ERROR(getMsgType(&msgBuffer)); break; } } } }
static portTASK_FUNCTION( vConductorUpdateTask, pvParameters ) { uint8_t rxLen, status; uint8_t Buffer[vtI2CMLen]; uint8_t *valPtr = &(Buffer[0]); // Get the parameters vtConductorStruct *param = (vtConductorStruct *) pvParameters; // Get the I2C device pointer vtI2CStruct *devPtr = param->dev; // Get the LCD information pointer vtTempStruct *tempData = param->tempData; myNavStruct *navData = param->navData; myMapStruct *mapData = param->mapData; uint8_t recvMsgType; mapStruct.mappingFlag = 0; uint8_t i2cRoverMoveStop[] = {0x05, 0x00}; // 255 will always be a bad message countDefArray[255] = BadMsg; uint8_t i; for(i = 0; i < 255; i++) { countDefArray[i] = CleanMsg; } uint8_t i2cRoverMoveStop[] = {0x05, 0x00}; // Like all good tasks, this should never exit for(;;) { // Wait for a message from an I2C operation if (vtI2CDeQ(devPtr,vtI2CMLen,Buffer,&rxLen,&recvMsgType,&status) != pdTRUE) { VT_HANDLE_FATAL_ERROR(0); } // Decide where to send the message // This just shows going to one task/queue, but you could easily send to // other Q/tasks for other message types // This isn't a state machine, it is just acting as a router for messages // If it is the initialization message if ( recvMsgType == vtI2CMsgTypeTempInit ) { SendTempValueMsg(tempData,recvMsgType,Buffer,portMAX_DELAY); } else { // Switch on the definition of the incoming count switch(countDefArray[Buffer[0]]) { case RoverMsgSensorAllData: { //printf("SensorData\n"); GPIO_ClearValue(0,0x78000); GPIO_SetValue(0, 0x48000); SendMapValueMsg(mapData,RoverMsgSensorAllData, Buffer, portMAX_DELAY); if (!speedRun) SendNavValueMsg(navData,0x11,Buffer,portMAX_DELAY); break; } case RoverMsgSensorForwardRight: { //SendTempValueMsg(tempData,recvMsgType,Buffer,portMAX_DELAY); break; } case RoverMsgMotorLeftData: { //printf("MotorLeftData\n"); GPIO_ClearValue(0,0x78000); GPIO_SetValue(0, 0x40000); SendTempValueMsg(tempData,RoverMsgMotorLeftData,Buffer,portMAX_DELAY); if (!speedRun) SendNavValueMsg(navData,RoverMsgMotorLeftData,Buffer,portMAX_DELAY); SendMapValueMsg(mapData,RoverMsgMotorLeftData,Buffer,portMAX_DELAY); break; } case RoverMsgMotorLeft90: { printf("RoverData\n"); break; // SendTempValueMsg(tempData,recvMsgType,Buffer,portMAX_DELAY); } case BadMsg: { printf("Bad Message; Restart Pics\n"); break; } default: { //printf("ConductDefault\n"); SendMapValueMsg(mapData,0x11,Buffer,portMAX_DELAY); break; } } if(Buffer[6] == 1) { if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempRead1,0x4F,sizeof(i2cRoverMoveStop),i2cRoverMoveStop,10) != pdTRUE) { VT_HANDLE_FATAL_ERROR(0); } mapStruct.mappingFlag = 0; mapStruct.timerFlag = 0; restart_nav(); printf("\nFINISH LINE DETECTED!\n"); stopGettingMotor("DATASTOP"); } } // Clear the count defition countDefArray[Buffer[0]] = CleanMsg; // Check if retransmission is needed if ( countDefArray[Buffer[0]-3] == RoverMsgMotorLeft90 ) { printf("Retrans of motor\n"); SendNavValueMsg(navData,0x89,Buffer,portMAX_DELAY); } // Check to see if the last five have been retrieved /*uint8_t i; for (i = Buffer[0]-1; i > Buffer[0] - 5; i--) { printf("Buf Check: %d\n", i); if ( countDefArray[i] == RoverMsgMotorLeft90 ) { printf("Retrans of motor\n"); SendNavValueMsg(navData,0x89,Buffer,portMAX_DELAY); } }*/ } }