unsigned char sensorcommunication_getByteISR()
{
COMMUNICATION_MESSAGE theMessage;
if(xQueueReceiveFromISR(sensorcommunicationData.sensorIntQueue, (void*)&(theMessage), 0))
{
int i = 0;
if( theMessage.type == sensorcommunicationData.sensorIntRxMsgSeq)
{
sensorcommunicationData.sensorRxMsgSeq++;
return theMessage.msg;
}
else
{
while( theMessage.type != sensorcommunicationData.sensorIntRxMsgSeq)
{
i++;
if(i == COMMUNICATIONQUEUESIZE)
{
xQueueReset(sensorcommunicationData.sensorIntQueue); //clear queue. we dropped the packet
}
xQueueSendToBackFromISR(sensorcommunicationData.sensorIntQueue, (void*)&(theMessage), 0); //send to back
xQueueReceiveFromISR(sensorcommunicationData.sensorIntQueue, (void*)&(theMessage), 0 ); //get another
}
debugU("COM message loaded\n");
return theMessage.msg;
}
}
}
void CONTROL_Initialize ( void )
{
debugU("Init control thread.");
/* Place the App state machine in its initial state. */
// controlData.state = CONTROL_STATE_INIT;
/* TODO: Initialize your application's state machine and other
* parameters.
*/
}
int motor_PIDLEncode()
{
float PErrorFix;
float IErrorFix;
int test;
motorData.LDist = (float)motorData.LEncode * MMPERTICK; //mm traveled
motorData.LMes = motorData.LDist * 20.0; //mm/sec
debugU("\nLeft:");
debugUFloat(motorData.LMes);
if(motorData.state != 3)
{
float error = motorData.LSet - motorData.LMes;
motorData.LAvg = ( (error) + (motorData.LAvg * (motorData.n - 1)) ) / motorData.n; //error in mm/sec average
PErrorFix = PFACTOR * error;
IErrorFix = IFACTOR * (motorData.LAvg);
float total = PErrorFix + IErrorFix;
if(total < 0.0)
test = (int)(total -0.5);
else
test = (int)(total +0.5);
motorData.LOCSet = motorData.LOCSet + test;
}
else
{
if(motorData.RMes == 0) //update RMes if it DNE yet.
{
motorData.RMes = (float)motorData.REncode * MMPERTICK * 20.0;
}
float currRatio = (float)motorData.LMes / (float)motorData.RMes;
float error = (motorData.ratio - currRatio); //*4.0 here orig...
motorData.LAvg = ( (error) + (motorData.LAvg * (motorData.n - 1)) ) / motorData.n; //error in mm/sec average
PErrorFix = PFACTORTURN * error;
IErrorFix = IFACTORTURN * (motorData.LAvg);
float total = PErrorFix + IErrorFix;
if(total < 0.0)
test = (int)(total -0.5);
else
test = (int)(total +0.5);
motorData.LOCSet = motorData.LOCSet + test;
}
if(motorData.LOCSet < 0)
motorData.LOCSet = 0;
// if(motorData.LOCSet > 600 && motorData.state == 2 )
// motorData.LOCSet = 600;
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
///*
debugUFloat(motorData.LAvg);
debugUFloat(PErrorFix);
debugUFloat(IErrorFix);
debugUInt(motorData.LOCSet);
debugUInt(test);//*/
return motorData.LEncode;
}
int motor_PIDREncode()
{
float PErrorFix;
float IErrorFix;
int test;
motorData.RDist = (float)motorData.REncode * MMPERTICK;
motorData.RMes = motorData.RDist * 20.0;
debugU("\nRight:");
debugUFloat(motorData.RMes);
if(motorData.state != 2)
{
float error = motorData.RSet - motorData.RMes;
motorData.RAvg = ( (error) + (motorData.RAvg * (motorData.n - 1)) ) / motorData.n; //error in mm/sec average
PErrorFix = PFACTOR * error;
IErrorFix = IFACTOR * (motorData.RAvg);
float total = PErrorFix + IErrorFix;
if(total < 0.0)
test = (int)(total -0.5);
else
test = (int)(total +0.5);
motorData.ROCSet = motorData.ROCSet + test;
}
else
{
float currRatio = (float)motorData.RMes / (float)motorData.LMes;
float error = 4.0*(motorData.ratio - currRatio);
motorData.RAvg = ( (error) + (motorData.RAvg * (motorData.n - 1)) ) / motorData.n; //error in mm/sec average
PErrorFix = PFACTORTURN * error;
IErrorFix = IFACTORTURN * (motorData.RAvg);
float total = PErrorFix + IErrorFix;
if(total < 0.0)
test = (int)(total -0.5);
else
test = (int)(total +0.5);
motorData.ROCSet = motorData.ROCSet + test;
}
if(motorData.ROCSet < 0)
motorData.ROCSet = 0;
// if(motorData.ROCSet > 600 && motorData.state == 3 )
// motorData.ROCSet = 600;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
///*
debugUFloat(motorData.RAvg);
debugUFloat(PErrorFix);
debugUFloat(IErrorFix);
debugUInt(motorData.ROCSet);
debugUInt(test);//*/
return motorData.REncode;
}
void CONTROL_Tasks ( void )
{
//if(sensorcommunicationData.sensortheQueue != 0)
//{
while(1)
{
//receive a message and store into rxMessage ... block 5 ticks if empty queue
if(xQueuePeek(controlData.theQueue, (void*)&(controlData.message), portMAX_DELAY ))
{
debugU("Control has a message.");
}
//}
}
}
void MOTOR_Tasks ( void )
{
//debugU("RUNNING");
// motor_sendmsg(1, 1000);
// motor_sendmsg(1, 1615);
// motor_sendmsg(3, 1350);
while(1)
{
if(motorData.theQueue != 0)
{
if(xQueuePeek(motorData.theQueue, (void*)&(motorData.rxMessage), portMAX_DELAY ))
{
xQueueReceive(motorData.theQueue, (void*)&(motorData.rxMessage), portMAX_DELAY );
motorData.state = motorData.rxMessage.command;
// motorData.state = 1;
// debugUInt(motorData.state);
// debugUInt(motorData.duration);
// communication_sendIntMsg(motorData.rxMessage.command, motorData.rxMessage.duration);
/* Check the application's current state. */
switch ( motorData.state )
{
/* Application's initial state. */
case MOTOR_STATE_INIT: //state 0, do nothing
{
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
DRV_OC1_Stop();
DRV_OC0_Stop();
break;
}
case MOTOR_STATE_STRAIGHT: //state 1, go straight.
{
debugU("Straight");
//stop command
if(motorData.rxMessage.int2 == 0)
{
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
DRV_OC1_Stop();
DRV_OC0_Stop();
float time = (float)motorData.n * 0.05;
float Ldistance = MMPERTICK * motorData.LTestTick;
float Rdistance = MMPERTICK * motorData.RTestTick;
motorData.LTestTick = 0;
motorData.RTestTick = 0;
debugUFloat(time);
debugU("Right:\n");
debugUFloat(motorData.RSet);
debugUFloat(Rdistance);
debugU("Left:\n");
debugUFloat(motorData.LSet);
debugUFloat(Ldistance);
debugUFloat(motorData.ratio);
motorData.LSet = 0.0;
motorData.RSet = 0.0;
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
}
else //just go straight
{
motorData.LSet = MAXSPEED;
motorData.RSet = MAXSPEED;
motorData.LOCSet = MAXSPEEDOCSET;
motorData.ROCSet = MAXSPEEDOCSET;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
DRV_OC1_Start();
DRV_OC0_Start();
DRV_TMR0_Start();
DRV_TMR1_Start();
DRV_TMR2_Start();
}
break;
}
case MOTOR_STATE_TURNRIGHT: //state 2, turn right
{
debugU("Right");
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
float angular = MAXSPEED / (float)motorData.rxMessage.int2;
int outRad = motorData.rxMessage.int2 + 5;
motorData.LSet = angular * (float)outRad;
//motorData.LSet = MAXSPEED;
motorData.LOCSet = (int)(motorData.LSet * 6);
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);// - 100);
if(outRad < 11)
{
motorData.RSet = 0.0;
motorData.ROCSet = 0;
// PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
// DRV_OC0_Start();
DRV_OC0_Stop();
}
else
{
float temp = (float)outRad;
motorData.ratio = ((temp - 10.0) / temp);
motorData.RSet = (temp - 10.0) * angular;
// motorData.RSet = ( motorData.ratio * (MAXSPEED));
motorData.ROCSet = (int)(motorData.RSet * 6); //motorData.RSet * 5.5;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
DRV_OC0_Start();
}
DRV_OC1_Start();
DRV_TMR0_Start();
DRV_TMR1_Start();
DRV_TMR2_Start();
break;
}
case MOTOR_STATE_TURNLEFT: //state 3 turn left.
{
debugU("Left");
DRV_TMR0_Stop();
DRV_TMR1_Stop();
DRV_TMR2_Stop();
motorData.LAvg = 0.0;
motorData.RAvg = 0.0;
motorData.LEncode = 0;
motorData.REncode = 0;
motorData.LEncodeTotal = 0;
motorData.REncodeTotal = 0;
motorData.n = 0;
DRV_TMR0_CounterClear();
DRV_TMR1_CounterClear();
DRV_TMR2_CounterClear();
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_G, PORTS_BIT_POS_1, 0);
PLIB_PORTS_PinWrite (PORTS_ID_0, PORT_CHANNEL_C, PORTS_BIT_POS_14, 0);
float angular = MAXSPEED / (float)motorData.rxMessage.int2;
int outRad = motorData.rxMessage.int2 + 5;
motorData.RSet = angular * (float)outRad;
motorData.ROCSet = (int)(motorData.RSet * 6);
// motorData.RSet = MAXSPEED;
// motorData.ROCSet = MAXSPEEDOCSET;
PLIB_OC_PulseWidth16BitSet(OC_ID_1, motorData.ROCSet);
if(outRad < 11)
{
motorData.LSet = 0.0;
motorData.LOCSet = 0;
// PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
// DRV_OC1_Start();
DRV_OC1_Stop();
}
else
{
float temp = (float)outRad;
motorData.ratio = ((temp - 10.0) / temp);
motorData.LSet = (temp - 10.0) * angular;
motorData.LOCSet = (int)(motorData.LSet * 6); //motorData.RSet * 5.5;
PLIB_OC_PulseWidth16BitSet(OC_ID_2, motorData.LOCSet);
DRV_OC1_Start();
}
DRV_OC0_Start();
DRV_TMR0_Start();
DRV_TMR1_Start();
DRV_TMR2_Start();
break;
}
/* The default state should never be executed. */
default:
{
DRV_TMR0_Stop();
DRV_TMR2_Stop();
DRV_TMR0_CounterClear();
DRV_TMR2_CounterClear();
DRV_OC1_Stop();
DRV_OC0_Stop();
motorData.duration = 0;
/* TODO: Handle error in application's state machine. */
break;
}
} //end switch
}
}
else
{
crash("No Motor MsgQ");
}
}
}
void APP_WIFLY_Tasks ( void )
{
while(1)
{
// debugChar(0x22);
// app_wifly_sendmsg(2,"TEST");
//check if queue exists
if(appData.theQueue != 0)
{
//receive a message and store into rxMessage ... block 5 ticks if empty queue
if(xQueuePeek(appData.theQueue, &(appData.rxChar), portMAX_DELAY ))
{
xQueueReceive(appData.theQueue, &(appData.rxChar), portMAX_DELAY );
//received messageQ message will tell use the state which will be used
//##appData.state = appData.rxMessage.type;
appData.state = 2;
//run the state according to the message's state
switch ( appData.state )
{
/* Application's initial state. */
case APP_STATE_INIT: //0
break;
case APP_STATE_READ: //1
{
debugU("Read");
debugChar(0x0F);
app_wifly_UartRx();
appData.state = 0;
app_wifly_sendmsg(2,appData.rxBuffer);
appData.rxMessage.type = 0;
appData.rxMessage.string = "";
break;
}
case APP_STATE_WRITE: //2
{
debugU("Write");
debugChar(0xF0);
app_wifly_UartTxChar(appData.rxChar);
appData.state = 0;
appData.rxMessage.type = 0;
appData.rxMessage.string = "";
//appData.rxChar = ' ';
break;
}
case APP_STATE_RECEIVE: //3
{
debugU("Receiving");
debugChar(appData.rxBufferIndex);
if(appData.rxBufferIndex < appData.rxBufferSize) //if we are within buffer bounds, copy char
appData.rxBuffer[appData.rxBufferIndex] = appData.rxMessage.string[0];
appData.rxBufferIndex++;
appData.state = 0;
appData.rxMessage.type = 0;
appData.rxMessage.string = "";
break;
}
/* The default state should never be executed. */
default:
{
/* TODO: Handle error in application's state machine. */
break;
}
}//end of case
}//end of receive message
}//end of check message queue != 0
else //attempt to create the queue again if it doesn't exist
{
//something terrible happens normally so have it exit rather than recreate
debugChar(0xFF);
//run exit interrupt to OS... or forever loop message
return;
//appData.theQueue = xQueueCreate(10, sizeof(APP_STATES));
}
}//end of while(1)
}
void SENSORCOMMUNICATION_Tasks ( void )
{
int roverSimCounter = 0;
while(1)
{
//check if queue exists
if(sensorcommunicationData.sensortheQueue != 0)
{
//receive a message and store into rxMessage ... block 5 ticks if empty queue
if(xQueuePeek(sensorcommunicationData.sensortheQueue, (void*)&(sensorcommunicationData.rxMessage), portMAX_DELAY ))
{
xQueueReceive(sensorcommunicationData.sensortheQueue, (void*)&(sensorcommunicationData.rxMessage), portMAX_DELAY);
//received messageQ message will tell use the state which will be used
sensorcommunicationData.state = sensorcommunicationData.rxMessage.type;
//communicationData.state = 1;
//run the state according to the message's state
switch ( sensorcommunicationData.state )
{
/* Application's initial state. */
case SENSORCOMMUNICATION_STATE_INIT: //0
break;
case SENSORCOMMUNICATION_STATE_RECEIVE: //1
//debugU("COM rx: ");
//debugUInt(communicationData.rxMessage.msg);
if(sensorcommunicationData.rxMessage.msg == STARTBYTE) //received start transmit bit
{
if(sensorcommunicationData.sensorrxByteCount > 0) //we had part of a previous message...
{
debugU("NACK");
//NACK;
}
sensorcommunicationData.sensorrxBuffer[0] = sensorcommunicationData.rxMessage.msg;
sensorcommunicationData.sensorrxByteCount = 1;
}
else if( sensorcommunicationData.sensorrxByteCount > 0) //continuing message
{
sensorcommunicationData.sensorrxBuffer[sensorcommunicationData.sensorrxByteCount] = sensorcommunicationData.rxMessage.msg;
debugUInt(CharToInt(sensorcommunicationData.rxMessage.msg));
sensorcommunicationData.sensorrxByteCount++;
if(sensorcommunicationData.sensorrxByteCount == 10)
{
//check start byte
if(sensorcommunicationData.sensorrxBuffer[0] == STARTBYTE)
{
//###CHECK seq number
int i = 0;
while(sensorcommunicationData.sensorrxBuffer[1] != sensorcommunicationData.sensorRxMsgSeq)
{
sensorcommunicationData.sensorRxMsgSeq++;
sensorcommunicationData.msgErr++;
debugU("Lost a seq number\r # dropped: ");
debugUInt(sensorcommunicationData.msgErr);
i++;
if(i == 10)
{
crash("E: COM too many lost rx seqnum\n");
}
}
int x = 0;
x += CharToInt(sensorcommunicationData.sensorrxBuffer[2]) * 1000;
x += CharToInt(sensorcommunicationData.sensorrxBuffer[3]) * 100;
x += CharToInt(sensorcommunicationData.sensorrxBuffer[4]) * 10;
x += CharToInt(sensorcommunicationData.sensorrxBuffer[5]);
int y = 0;
y += CharToInt(sensorcommunicationData.sensorrxBuffer[6]) * 1000;
y += CharToInt(sensorcommunicationData.sensorrxBuffer[7]) * 100;
y += CharToInt(sensorcommunicationData.sensorrxBuffer[8]) * 10;
y += CharToInt(sensorcommunicationData.sensorrxBuffer[9]);
if (x != 0 && y != 0)
{
roverLocation[0] = x;
roverLocation[1] = y;
}
communication_sendIntMsg(0, 1000);
// else if (roverSimCounter < 11)
// communication_sendIntMsg(1, 1);
// else if (roverSimCounter == 2)
// communication_sendIntMsg(1, 10);
// else if (roverSimCounter == 3)
// communication_sendIntMsg(3, 1);
// else if (roverSimCounter == 4)
// communication_sendIntMsg(1, 6);
// else if (roverSimCounter == 5)
// communication_sendIntMsg(3, 1);
// else if (roverSimCounter == 6)
// communication_sendIntMsg(1, 10);
// else if (roverSimCounter == 7)
// communication_sendIntMsg(3, 1);
// else if (roverSimCounter == 8)
// communication_sendIntMsg(1, 4);
//
// else // Tel the rover to not do anything...
// communication_sendIntMsg(0, 0);
roverSimCounter++;
debugUInt(roverSimCounter);
debugU("sensor msg period: ");
debugUInt(sensor_debugGetTime());
debugU("Sensor1: ");
//char msg[12];
//sprintf(msg, "%d", x);
//debugU(msg);
debugUInt(roverLocation[0]);
debugU("Sensor2: ");
//sprintf(msg, "%d", y);
//debugU(msg);
debugUInt(roverLocation[1]);
debugU("\n");
//communication_sendIntMsg(x,y);
sensorcommunicationData.sensorrxByteCount = 0;
sensorcommunicationData.sensorRxMsgSeq++;
if(sensorcommunicationData.sensorRxMsgSeq == 0x7F)
sensorcommunicationData.sensorRxMsgSeq = 0x00;
//ACK
//debugU("\nsensor comm ACK\n");
//#ifdef TEST
// testData.count++; // Increment message count
// int j =0;
// for (j; j<10; j++)
// {
// testData.sensorrxBuffer[j] = sensorcommunicationData.sensorrxBuffer[j];
// }
// // Receive buffer contents recorded.
//
// communication_sendIntMsg(x, y);
//#else
// if(sensorcommunicationData.sensorRxMsgSeq % 4 == 0)
// communication_sendIntMsg(3, 100);
// else if(sensorcommunicationData.sensorRxMsgSeq % 4 == 2)
// communication_sendIntMsg(2, 100);
//#endif
sensorcommunicationData.state = 0;
}//end if check start byte
}//end if bytecount == 10
}//end if bytecount > 0
else //failed to receive start bit and catch all... nack and reset message
{
//NACK
debugU("\nsensor NACK");
sensorcommunicationData.sensorrxByteCount = 0; //no start byte and bytecount != 0, so unknown char drop it
}
break;
/* The default state should never be executed. */
default:
{
/* TODO: Handle error in application's state machine. */
break;
}
}//end of case
}//end of receive message
}//end of check message queue != 0
else //attempt to create the queue again if it doesn't exist
{
//something terrible happens normally so have it exit rather than recreate
crash("E: No Comm Q");
//run exit interrupt to OS... or forever loop message
return;
//communicationData.sensortheQueue = xQueueCreate(10, sizeof(APP_STATES));
}
}//end of while(1)
}
