// This is the actual task that is run
static portTASK_FUNCTION( vi2cUpdateTask, pvParameters )
{
// Get the parameters
param = (myI2CStruct *) pvParameters;
// Get the I2C device pointer
devPtr = param->dev;
// 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) {
VT_HANDLE_FATAL_ERROR(Q_RECV_ERROR);
}
switch(getMsgType(&msgBuffer)) {
case i2cTimerMsgType: {
// Poll local 2680 for data
notifyRequestSent();
if (vtI2CEnQ(devPtr,vtI2CReadMsgType,SLAVE_ADDR,0,0,I2C_MSG_SIZE) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(VT_I2C_Q_FULL);
}
break;
}
case vtI2CMotorMsgType: {
// Send motor command to local 2680
if (vtI2CEnQ(devPtr,vtI2CMotorMsgType,SLAVE_ADDR,msgBuffer.length,msgBuffer.buf,0) != pdTRUE){
VT_HANDLE_FATAL_ERROR(VT_I2C_Q_FULL);
}
break;
}
case notifyRqstRecvdMsgType: {
if(requestSent == 0){
// Send I2C Error Message to Web Server
}
requestSent = 0;
break;
}
default: {
VT_HANDLE_FATAL_ERROR(UNKNOWN_I2C_MSG_TYPE);
break;
}
}
}
}
// 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;
}
}
}
}
// This is the actual task that is run
static portTASK_FUNCTION( I2CTask, pvParameters )
{
portTickType xUpdateRate, xLastUpdateTime;
const uint8_t ReturnADCValue = 0xAA;
//const uint8_t InsteonReserved; //reserved for Insteon if we are only requesting insteon data
//uint8_t InsteonSendValue[12]; //reserved for Insteon send
uint8_t MidiSendCount = 1;
uint8_t MidiSendValue[9] = {0xAF, 0x80, 0x64, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00};
uint8_t InstSendValue[9] = {0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
int msgcount = 0;
uint8_t temp1, rxLen, status;
uint8_t ADCValueReceived[12];
uint8_t MidiReceived[2];
// Get the parameters
i2cParamStruct *param = (i2cParamStruct *) pvParameters;
// Get the I2C device pointer
vtI2CStruct *devPtr = param->i2cDev;
// Get the LCD information pointer
vtLCDMsgQueue *lcdData = param->lcdQ;
vtLCDMsg lcdBuffer;
MasterMsgQueue * masterData = param->masterQ;
MasterMsgQueueMsg masterBuffer;
I2CMsgQueue * i2cQ = param->i2cQ;
I2CMsgQueueMsg i2cBuffer;
vTaskDelay(10/portTICK_RATE_MS);
xUpdateRate = i2cREAD_RATE_BASE / portTICK_RATE_MS;
/* We need to initialise xLastUpdateTime prior to the first call to vTaskDelayUntil(). */
xLastUpdateTime = xTaskGetTickCount();
for(;;)
{
//delay for some amount of time before looping again
//vTaskDelayUntil( &xLastUpdateTime, xUpdateRate );
//Send a request to the PIC for ADC values
if (vtI2CEnQ(devPtr,0x01,0x4F,1,&ReturnADCValue,12) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
//wait for message from I2C
//bit0 is first portion of ADC value
//bit1 is last portion of ADC value
//bit2 is timer value
//bit3 thru bit7 are garbage (all 0's)
//bit8 is the size of the Midi buffer on the PIC
//bit9 is the count
//bit10 is the ADC channel number
//bit11 is the OPCode we sent (0xAA)
if (vtI2CDeQ(devPtr,12,&ADCValueReceived[0],&rxLen,&status) != pdTRUE) {
//VT_HANDLE_FATAL_ERROR(0);
}
//check the message returned for errors:
//0xAA signifying it's the correct op-code, verify the ADC is in the right order,
//ensure the proper range of channel numbers is used.
if ((ADCValueReceived[11] != 0xAA || ADCValueReceived[0] > 3 || ADCValueReceived[10] < 0 || ADCValueReceived[10] > 5) && ADCValueReceived[11] != 0xBB)
{
FlipBit(6);
}
else
{
//check the inbound i2c message queue from messages either from the Main Thread or the LCD
//thread. Forward them to the PIC depending on op-code
if (ADCValueReceived[8] < 4 && uxQueueMessagesWaiting(i2cQ->inQ) > 0)//check for room on the PIC and if a message exists
{
if (xQueueReceive(i2cQ->inQ,(void *) &i2cBuffer,portMAX_DELAY) != pdTRUE) //receive message from message queue
{
VT_HANDLE_FATAL_ERROR(0);
}
/*if (i2cBuffer.buf[0] == 0x13)
{
InstSendValue[0] = i2cBuffer.buf[0];
InstSendValue[1] = 0x02;
InstSendValue[2] = 0x62;
InstSendValue[3] = 0x12;
InstSendValue[4] = 0x07;
InstSendValue[5] = 0x4F;
InstSendValue[6] = 0x00;
InstSendValue[7] = 0x11;
InstSendValue[8] = i2cBuffer.buf[1];
if (i2cBuffer.buf[1] == 8)
FlipBit(3);
if (vtI2CEnQ(devPtr,0x00,0x4F,9,InstSendValue,0) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
//wait for message from I2C
if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
//VT_HANDLE_FATAL_ERROR(0);
}
} */
else if (i2cBuffer.buf[0] == 0x4) //MIDI message to be forwarded
{
MidiSendValue[1] = i2cBuffer.buf[1]; //Here are the three MIDI control bytes sent
MidiSendValue[2] = i2cBuffer.buf[2]; //They are formulated in the mainthread and sent here
MidiSendValue[3] = i2cBuffer.buf[3];
//Midi message to I2C to the PIC, there is also a count to maintain messages
if (MidiSendCount > 100)
MidiSendCount = 1;
MidiSendValue[4] = MidiSendCount;
if (vtI2CEnQ(devPtr,0x00,0x4F,9,MidiSendValue,0) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
//wait for message from I2C
if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
//VT_HANDLE_FATAL_ERROR(0);
}
//Whenever a "note on" message is being sent to the MIDI device, we want to update the LED display as well
//this checks the command portion of the MIDI message then changes the LED accordingly. The 0x90 signifies
//that the message came from instrument 1 since it operates on MIDI channel 0; 0x91 means Instrument 2 since
//it operates on MIDI channel 1
if (i2cBuffer.buf[1] == 0x90)
{
if (i2cBuffer.buf[2] == 60)
InstSendValue[2] = 0x80;
else if (i2cBuffer.buf[2] == 62)
InstSendValue[2] = 0x40;
else if (i2cBuffer.buf[2] == 64)
InstSendValue[2] = 0x20;
else if (i2cBuffer.buf[2] == 65)
InstSendValue[2] = 0x10;
else if (i2cBuffer.buf[2] == 67)
InstSendValue[2] = 0x08;
else if (i2cBuffer.buf[2] == 69)
InstSendValue[2] = 0x04;
else if (i2cBuffer.buf[2] == 71)
InstSendValue[2] = 0x02;
else if (i2cBuffer.buf[2] == 72)
InstSendValue[2] = 0x01;
InstSendValue[0] = 0x00;
InstSendValue[1] = 0x16;
//InstSendValue[2] = 0x80;
InstSendValue[3] = 0x00;
InstSendValue[4] = 0x00;
InstSendValue[5] = 0x00;
if (vtI2CEnQ(devPtr,0x00,0x38,6,InstSendValue,0) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
//VT_HANDLE_FATAL_ERROR(0);
}
MidiSendCount++;
}
else if (i2cBuffer.buf[1] == 0x91)
{
if (i2cBuffer.buf[2] == 60)
InstSendValue[2] = 0x80;
else if (i2cBuffer.buf[2] == 62)
InstSendValue[2] = 0x40;
else if (i2cBuffer.buf[2] == 64)
InstSendValue[2] = 0x20;
else if (i2cBuffer.buf[2] == 65)
InstSendValue[2] = 0x10;
else if (i2cBuffer.buf[2] == 67)
InstSendValue[2] = 0x08;
else if (i2cBuffer.buf[2] == 69)
InstSendValue[2] = 0x04;
else if (i2cBuffer.buf[2] == 71)
InstSendValue[2] = 0x02;
else if (i2cBuffer.buf[2] == 72)
InstSendValue[2] = 0x01;
InstSendValue[0] = 0x00;
InstSendValue[1] = 0x16;
//InstSendValue[2] = 0x80;
InstSendValue[3] = 0x00;
InstSendValue[4] = 0x00;
InstSendValue[5] = 0x00;
if (vtI2CEnQ(devPtr,0x00,0x3B,6,InstSendValue,0) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
//VT_HANDLE_FATAL_ERROR(0);
}
MidiSendCount++;
}
//Here are similar checks on the MIDI messages for a change in pitch. The same checkes are made
//for different channels
if (i2cBuffer.buf[1] == 0xE0)
{
if (i2cBuffer.buf[3] == 0x00)
InstSendValue[1] = 0x04;
else if (i2cBuffer.buf[3] == 0x40)
InstSendValue[1] = 0x02;
else if (i2cBuffer.buf[3] == 0x7F)
InstSendValue[1] = 0x01;
InstSendValue[0] = 0x02;
InstSendValue[3] = 0x00;
InstSendValue[2] = 0x00;
InstSendValue[4] = 0x00;
InstSendValue[5] = 0x00;
if (vtI2CEnQ(devPtr,0x00,0x38,6,InstSendValue,0) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
//VT_HANDLE_FATAL_ERROR(0);
}
MidiSendCount++;
}
else if (i2cBuffer.buf[1] == 0xE1)
{
if (i2cBuffer.buf[3] == 0x00)
InstSendValue[1] = 0x04;
else if (i2cBuffer.buf[3] == 0x40)
InstSendValue[1] = 0x02;
else if (i2cBuffer.buf[3] == 0x7F)
InstSendValue[1] = 0x01;
InstSendValue[0] = 0x02;
InstSendValue[3] = 0x00;
InstSendValue[2] = 0x00;
InstSendValue[4] = 0x00;
InstSendValue[5] = 0x00;
if (vtI2CEnQ(devPtr,0x00,0x3B,6,InstSendValue,0) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
//VT_HANDLE_FATAL_ERROR(0);
}
MidiSendCount++;
}
//wait for message from I2C
}
}
//If the message is not NULL then it is an ADC message, this message gets forwarded to MainThread.c where
//the data is used for calculations.
FlipBit(7);
if (lcdData != NULL)
{
//message sent to the master message queue
masterBuffer.length = 13;
masterBuffer.buf[0] = 0x08; //means the message is from I2C - change to 0x09 for Nick's program
masterBuffer.buf[1] = ADCValueReceived[0];
masterBuffer.buf[2] = ADCValueReceived[1];
masterBuffer.buf[3] = ADCValueReceived[2];
masterBuffer.buf[4] = ADCValueReceived[3];
masterBuffer.buf[5] = ADCValueReceived[4];
masterBuffer.buf[6] = ADCValueReceived[5];
masterBuffer.buf[7] = ADCValueReceived[6];
masterBuffer.buf[8] = ADCValueReceived[7];
masterBuffer.buf[9] = ADCValueReceived[8];
masterBuffer.buf[10] = ADCValueReceived[9];
masterBuffer.buf[11] = ADCValueReceived[10];
masterBuffer.buf[12] = ADCValueReceived[11];
if (xQueueSend(masterData->inQ,(void *) (&masterBuffer),portMAX_DELAY) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
}
}
}
}
static portTASK_FUNCTION(vmotorTask, pvParameters) {
motorStruct *param = (motorStruct *) pvParameters;
motorMsg msg;
const uint8_t *motorCommand;
const uint8_t forward_ten[]= {0xBA, 0x9F, 0x1F, 0x64, 0x00};
const uint8_t forward_five[]= {0xBA, 0x9F, 0x1F, 0x32, 0x00};
const uint8_t turn_right[]= {0xBA, 0x9F, 0x62, 0x0B, 0x00};
const uint8_t turn_left[]= {0xBA, 0xE1, 0x1F, 0x0B, 0x00};
const uint8_t backwards_five[]= {0xBA, 0xE1, 0x62, 0x32, 0x00};
const uint8_t stop[]= {0xBA, 0x00, 0x00, 0x00, 0x00};
unsigned int demoInt = 0;
SendLCDPrintMsg(param->lcdData,20,"motorTask Init",portMAX_DELAY);
for( ;; ) {
//wait forever or until queue has something
if (xQueueReceive(param->inQ,(void *) &msg,portMAX_DELAY) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
switch(getMsgType(&msg)) {
//only one type of message so far
case SENSORTASK_MSG: {
//this is where the motorTask will translate from human readable movement to block of sabertooth stuff
//current slave address is 0x4F, take note
if(demoInt == 0)
motorCommand = forward_ten;
else if(demoInt == 1)
motorCommand = turn_left;
else if (demoInt == 2)
motorCommand = forward_five;
else
motorCommand = stop;
if (vtI2CEnQ(param->dev,vtRoverMovementCommand,0x4F, 5, motorCommand, 3) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
demoInt = demoInt + 1;
SendLCDPrintMsg(param->lcdData,20,"SND: Move Command",portMAX_DELAY);
break;
}
case ROVERACK_ERROR: {
//this is where the arm will re-request the movement ack from the rover
if(demoInt == 0)
motorCommand = forward_ten;
else if(demoInt == 1)
motorCommand = turn_left;
else if (demoInt == 2)
motorCommand = forward_five;
else
motorCommand = stop;
if (vtI2CEnQ(param->dev,vtRoverMovementCommand,0x4F, 5, motorCommand, 3) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
demoInt = demoInt + 1;
SendLCDPrintMsg(param->lcdData,20,"RSND: Move Command",portMAX_DELAY);
break;
}
}
}
}
// This is the actual task that is run
static portTASK_FUNCTION( oScopeUpdateTask, pvParameters )
{
uint8_t rcvByte[2];
// Get the parameters
oScopeStruct *param = (oScopeStruct *) pvParameters;
// Get the I2C device pointer
vtI2CStruct *devPtr = param->dev;
// Get the LCD information pointer
lcdOScopeStruct *lcdData = param->lcdData;
// Buffer for receiving messages
oScopeMsg msgBuffer;
uint8_t currentState;
// Assumes that the I2C device (and thread) have already been initialized
// This task is implemented as a Finite State Machine. The incoming messages are examined to see
// whether or not the state should change.
//
// Temperature sensor configuration sequence (DS1621) Address 0x4F
if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempInit,0x4F,sizeof(i2cCmdInit),i2cCmdInit,0) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
currentState = fsmRead1;
// 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) {
VT_HANDLE_FATAL_ERROR(0);
}
// Now, based on the type of the message and the state, we decide on the new state and action to take
switch(getMsgType(&msgBuffer)) {
case oScopeTimerMsg: {
if (vtI2CEnQ(devPtr,oScopeRead1Msg,0x4F,sizeof(i2cCmdReadByte1),i2cCmdReadByte1,2) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
if (vtI2CEnQ(devPtr,oScopeRead2Msg,0x4F,sizeof(i2cCmdReadByte2),i2cCmdReadByte2,1) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
break;
}
case oScopeRead1Msg: {
printf("Read1Msg:");
if (currentState == fsmRead1) {
currentState = fsmRead2;
rcvByte[0] = getValue(&msgBuffer);
} else {
// unexpectedly received this message
VT_HANDLE_FATAL_ERROR(0);
}
break;
}
case oScopeRead2Msg: {
printf("Read2Msg:");
if (currentState == fsmRead2) {
currentState = fsmRead1;
rcvByte[1] = getValue(&msgBuffer);
uint16_t msgData = (rcvByte[0])|(rcvByte[1]<<8);
if (SendLCDOScopeMsg(lcdData,msgData,portMAX_DELAY) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
} else {
// unexpectedly received this message
VT_HANDLE_FATAL_ERROR(0);
}
break;
}
default: {
VT_HANDLE_FATAL_ERROR(getMsgType(&msgBuffer));
break;
}
}
}
}
static portTASK_FUNCTION(vmotorTask, pvParameters) {
motorStruct *param = (motorStruct *) pvParameters;
motorMsg msg;
const uint8_t bc_half_forward[] = {0xBC, 0xA0, 0x20, 0xFF, 0xFF, 0x00};
const uint8_t ba_15cm_forward[] = {0xBA, 0xA0, 0x20, 0x0F, 0x0F, 0x00}; // 0.5ft
const uint8_t ba_45cm_forward[] = {0xBA, 0xA0, 0x20, 0x2D, 0x2D, 0x00}; // 1.5ft
const uint8_t ba_90_left[] = {0xBA, 0xE0, 0x20, 0x12, 0x12, 0x00};
const uint8_t ba_90_right[] = {0xBA, 0xA0, 0x60, 0x12, 0x12, 0x00};
uint8_t ba_custom_forward[] = {0xBA, 0xA0, 0x20, 0x2D, 0x2D, 0x00};
const uint8_t *motorCommand = bc_half_forward;
SendLCDPrintMsg(param->lcdData,20,"motorTask Init",portMAX_DELAY);
for( ;; ) {
//wait forever or until queue has something
if (xQueueReceive(param->inQ,(void *) &msg,portMAX_DELAY) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
switch(getMsgType(&msg)) {
//only one type of message so far
case SENSORTASK_MSG: {
switch(getMoveType(&msg)) {
case ROVERMOVE_FORWARD_ABSOLUTE:
motorCommand = ba_custom_forward;
uint8_t distance = getDistance(&msg);
ba_custom_forward[3] = distance;
ba_custom_forward[4] = distance;
break;
case ROVERMOVE_FORWARD_CORRECTED:
motorCommand = bc_half_forward;
break;
case ROVERMOVE_TURN_LEFT:
motorCommand = ba_90_left;
break;
case ROVERMOVE_TURN_RIGHT:
motorCommand = ba_90_right;
break;
}
//current slave address is 0x4F, take note
if (vtI2CEnQ(param->dev, vtRoverMovementCommand, 0x4F, 6, motorCommand, 3) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
SendLCDPrintMsg(param->lcdData,20,"SND: Move Command",portMAX_DELAY);
break;
}
case ROVERACK_ERROR: {
//this is where the arm will re-request the movement ack from the rover
if (vtI2CEnQ(param->dev, vtRoverMovementCommand, 0x4F, 6, motorCommand, 3) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
SendLCDPrintMsg(param->lcdData,20,"RSND: Move Command",portMAX_DELAY);
break;
}
}
}
}
// This is the actual task that is run
static portTASK_FUNCTION( vi2cTempUpdateTask, pvParameters )
{
int byte1 = 0;
int byte2 = 0;
//float countPerC = 100.0, countRemain=0.0;
// Get the parameters
vtTempStruct *param = (vtTempStruct *) pvParameters;
// Get the I2C device pointer
vtI2CStruct *devPtr = param->dev;
// Get the LCD information pointer
lcdOScopeStruct *lcdData = param->lcdData;
// String buffer for printing
char lcdBuffer[vtOScopeMaxLen+1];
// Buffer for receiving messages
vtTempMsg msgBuffer;
uint8_t currentState;
// Assumes that the I2C device (and thread) have already been initialized
// This task is implemented as a Finite State Machine. The incoming messages are examined to see
// whether or not the state should change.
//
// Temperature sensor configuration sequence (DS1621) Address 0x4F
//if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempInit,0x4F,sizeof(i2cCmdInit),i2cCmdInit,0) != pdTRUE) {
// VT_HANDLE_FATAL_ERROR(0);
//}
currentState = vtI2CMsgTypeTempRead1;
// 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) {
VT_HANDLE_FATAL_ERROR(0);
}
// Now, based on the type of the message and the state, we decide on the new state and action to take
switch(getMsgType(&msgBuffer)) {
case vtI2CMsgTypeTempInit: {
if (currentState == fsmStateInit1Sent) {
currentState = fsmStateInit2Sent;
// Must wait 10ms after writing to the temperature sensor's configuration registers(per sensor data sheet)
vTaskDelay(10/portTICK_RATE_MS);
// Tell it to start converting
if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempInit,0x4F,sizeof(i2cCmdStartConvert),i2cCmdStartConvert,0) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
} else if (currentState == fsmStateInit2Sent) {
currentState = fsmStateTempRead1;
} else {
// unexpectedly received this message
VT_HANDLE_FATAL_ERROR(0);
}
break;
}
case TempMsgTypeTimer: {
// Timer messages never change the state, they just cause an action (or not)
if ((currentState != fsmStateInit1Sent) && (currentState != fsmStateInit2Sent)) {
printf("got a timer call yo");
// Read in the values from the temperature sensor
// We have three transactions on i2c to read the full temperature
// we send all three requests to the I2C thread (via a Queue) -- responses come back through the conductor thread
// Temperature read -- use a convenient routine defined above
//if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempRead1,0x4F,sizeof(i2cCmdReadVals),i2cCmdReadVals,2) != pdTRUE) {
// VT_HANDLE_FATAL_ERROR(0);
//}
printf("TIMER GET\n");
if (vtI2CEnQ(devPtr,voidMsg,0x4F,sizeof(i2cCmdReadVals),i2cCmdReadVals,4) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
// Read in the read counter
/*if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempRead2,0x4F,sizeof(i2cCmdReadCnt),i2cCmdReadCnt,2) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}*/
// Read in the slope;
//if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempRead3,0x4F,sizeof(i2cCmdReadSlope),i2cCmdReadSlope,1) != pdTRUE) {
// VT_HANDLE_FATAL_ERROR(0);
//}
} else {
// just ignore timer messages until initialization is complete
}
break;
}
case vtI2CMsgTypeTempRead1: {
if (currentState == fsmStateTempRead1) {
currentState = fsmStateTempRead1;
byte1 = msgBuffer.buf[0];
byte2 = msgBuffer.buf[1];
uint16_t data = (byte2<<8)|byte1;
SendLCDOScopeMsg(lcdData,data,portMAX_DELAY);
} else {
// unexpectedly received this message
VT_HANDLE_FATAL_ERROR(0);
}
break;
}
case vtI2CMsgTypeTempRead2: {
if (currentState == fsmStateTempRead2) {
currentState = fsmStateTempRead1;
byte2 = getValue(&msgBuffer);
uint16_t data = (byte2<<8)|byte1;
SendLCDOScopeMsg(lcdData,data,portMAX_DELAY);
} else {
// unexpectedly received this message
VT_HANDLE_FATAL_ERROR(0);
}
break;
}
default: {
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);
}
}*/
}
}
// This is the actual task that is run
static portTASK_FUNCTION( SomeTask, pvParameters )
{
portTickType xUpdateRate, xLastUpdateTime;
const uint8_t i2cCmdReadVals[]= {0xAA};
uint8_t temp1, rxLen, status;
uint8_t messageReceived[2];
// Get the parameters
i2cTempStruct *param = (i2cTempStruct *) pvParameters;
// Get the I2C device pointer
vtI2CStruct *devPtr = param->dev;
// Get the LCD information pointer
vtLCDStruct *lcdData = param->lcdData;
vtLCDMsg lcdBuffer;
vTaskDelay(10/portTICK_RATE_MS);
xUpdateRate = i2cREAD_RATE_BASE / portTICK_RATE_MS;
/* We need to initialise xLastUpdateTime prior to the first call to vTaskDelayUntil(). */
xLastUpdateTime = xTaskGetTickCount();
for(;;)
{
//delay for some amount of time before looping again
vTaskDelayUntil( &xLastUpdateTime, xUpdateRate );
int i = 0;
//toggle the bottom LED
uint8_t ulCurrentState = GPIO2->FIOPIN;
if( ulCurrentState & 0x40 )
{
GPIO2->FIOCLR = 0x40;
}
else
{
GPIO2->FIOSET = 0x40;
}
//Ask for message from I2C
if (vtI2CEnQ(devPtr,0x01,0x48,sizeof(i2cCmdReadVals),0x00/*i2cCmdReadVals*/,2) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
//wait for message from I2C
if (vtI2CDeQ(devPtr,2,&messageReceived[0],&rxLen,&status) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
uint8_t testint = 0;
//load the read message from I2C into the lcd Buffer
lcdBuffer.buf[0] = messageReceived[1];
lcdBuffer.buf[1] = messageReceived[0];
if (lcdData != NULL && lcdBuffer.buf[0] != 0xFF) {
// Send a message to the LCD task for it to print (and the LCD task must be configured to receive this message)
lcdBuffer.length = strlen((char*)(lcdBuffer.buf))+1;
if (xQueueSend(lcdData->inQ,(void *) (&lcdBuffer),portMAX_DELAY) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
}
}
}
