コード例 #1
0
ファイル: motor.c プロジェクト: madcat101010/rover_ece4535
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;
}
コード例 #2
0
ファイル: motor.c プロジェクト: madcat101010/rover_ece4535
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;
}
コード例 #3
0
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)
}