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 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)
}
