// 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);
}
}*/
}
}
