nette sonntagsaufgabe: fehler im programm...

[andieh]

Hallo Freunde!

Seit ca. 2 Wochen bin ich jetzt stolzer Besitzer des RP6 und hatte ja schon seit Anfang an die Idee, meinen Roboter per RS232 zu steuern. Der "Server" läuft jedoch nicht auf dem PC, sondern auf einem modifizierten Fonera Router. Die Verbindung steht, erste Lenkversuche haben auch geklappt. Jetzt haben wir uns eine Gui zusammengebaut, die auch noch verschiedene Informationen (Bumper gedrückt, Obstacles in Sicht, Motorgeschwindigkeit) vom Roboter anfragt und geliefert bekommt. Nur jetzt haben wir angefangen, eben auch mit diesen Daten zu arbeiten und da liegt das Problem, bei zu vielen Anfragen verschluckt sich der RP6 anscheinend und schickt komische Dinge.

Zum Testen verbinde ich mich mit Telnet auf den Server, der auf der Fonera läuft und sende Commands. Die Fonera leitet diese Befehle an den RP6 über seriell weiter und erhält Antworten, die natürlich zurück zu telnet geschickt werden. commands fangen immer mit einem $ an und schließen mit einem &. alles andere wird vom server verworfen, auch auf dem RP6 schaue ich nicht nach dem absatz als endzeichen, sondern auf das &! Hier ein Beispiel was passiert:

$START& //startet das programm auf dem rp6
Starting RP6 Program! //rückgabe
$Q|ALL& //messwerte werden angefragt
$M|BL:0|BR:0|LDRL:798|LDRR:907|UBAT:830|OL:0|OR:0|ML:0|MR:0& //antwort
$Q|ALL& //messwerte werden angefragt
$M|BL:0|BR:0|LDRL:801|LDRR:909|UBAT:830|OL:0|OR:0|ML:0|MR:0& //antwort
$D|BU& // motorspeed soll um 10 erhöht werden
$M|BL:0|BR:0|LDRL:802|LDRR:910|UBAT:830|OL:0|OR:0|ML:0|MR:0& // messwerte kommen an, aber motorspeed wird trotzdem erhöht!
$D|BU& // noch mal motorspeed erhöhen
$OK& // aknowledge, also speed wurde erhöht
$Q|ALL& // messwerte anfragen
$Q|ALL& // keine antwort, noch mal anfragen
$Q|ALL& // immer noch nix, noch mal anfragen
$M|BL:0|BR:0|LDRL:811|LDRR:912|UBAT:826|OL:0|OR:0|ML:10|MR:10& // antwort
$D|STOP& // alles stoppen
$M|BL:0|BR:0|LDRL:811|LDRR:912|UBAT:826|OL:0|OR:0|ML:10|MR:10& // schon wieder kommen die messwerte an, aber rp6 stoppt trotzdem!

irgendwie scheint der buffer auf dem rp6 komische sachen zu speichern und zu verschicken. die befehle kommen auf jeden fall an, aber die antwort ist falsch.

Ich komme einfach nicht mehr weiter. seit 2-3 Tagen Suche in intensiv nach dem Fehler aber ich kann den Code schon nicht mehr sehen. Ich bin mir ziemlich sicher, das der Fehler irgendwo im Programm auf dem RP6 zu suchen ist, da der Server auf der Fonera eigentlich, meiner Meinung nach, alles richtig macht.

Es wäre toll, wenn ihr mal einen Blick darauf werfen könntet, vielleicht fällt jemanden etwas auf. Den Server habe ich angehängt.

MfG
Andieh

#include "RP6RobotBaseLib.h"


// returns max value from to integer
int max(int number1, int number2)
{
	if ( number1 > number2) return number1;
	else return number2;
}

/*****************************************************************************/

// Main function - The program starts here:



int main(void)

{
	int DIR_LEFT = 0; // 0 nothing, 1 FWD, -1 BWD
	int DIR_RIGHT = 0; // 0 nothing, 1 FWD, -1 BWD
	int M_SPEED_L = 0;
	int M_SPEED_R = 0;
	int SPEED_STEP = 10;
	

	initRobotBase();
	 

	powerON();
	setACSPwrHigh();
	clearReceptionBuffer();
	
	// ---------------------------------------

	// Main loop:
	
	uint8_t buffer_pos = 0;
	int charsToReceive = 50;
	char receiveBuffer[charsToReceive+1];
	int send = 0;

	while(true)

	{
		if(getBufferLength())    // Check if we still have data (means getBufferLength() is not zero)	
		{
			receiveBuffer[buffer_pos] = readChar(); // get next character from reception buffer
			if(receiveBuffer[buffer_pos]=='&') // End of line detected!
			{

				receiveBuffer[buffer_pos+1]='\0'; // Terminate String with a 0, so other routines can determine where it ends!
			
				buffer_pos = -1;
				clearReceptionBuffer();
			}

			else if(buffer_pos >= charsToReceive) // IMPORTANT: We can not receive more characters than "charsToReceive"
			{
				receiveBuffer[charsToReceive]='\0';	// So if we receive more characters, we just stop reception and terminate the String.
				writeString_P("$You entered more characters than possible!%\n");
			}

			buffer_pos++;

			

		}	
					
					
		// try to find something useful information in the 
		// received string
		
		// TODO: check if string starts with $ and ends with &
		
		// read all values
		else if (strncmp("$Q|ALL",receiveBuffer,6) == 0) {
			int BL = getBumperLeft();
			int BR = getBumperRight();
			int OL = isObstacleLeft();
			int OR = isObstacleRight();
			char sendValues[85]; 
			memset(sendValues, '\0', sizeof(sendValues) );
				
			sprintf(sendValues,"$M|BL:%d|BR:%d|LDRL:%d|LDRR:%d|UBAT:%d|OL:%d|OR:%d|ML:%d|MR:%d&",BL,BR,adcLSL,adcLSR,adcBat,OL,OR,M_SPEED_L,M_SPEED_R);
			writeString(sendValues);

			send = 1;
		}
				
		// move forward
		else if (strncmp("$D|FWD", receiveBuffer,6) == 0) {
			if ( DIR_LEFT == -1 || DIR_RIGHT == -1 )
				stop();
				
			setMotorDir(FWD,FWD);
			if ( M_SPEED_L != M_SPEED_R) {
				M_SPEED_L = max(M_SPEED_L,M_SPEED_R);
				M_SPEED_R = max(M_SPEED_L,M_SPEED_R);
			}
			moveAtSpeed(M_SPEED_L,M_SPEED_R);  
			DIR_LEFT = 1; DIR_RIGHT = 1;
			writeString_P("$OK&");
			send = 1;
		}

		// move backwards
		else if (strncmp("$D|BWD", receiveBuffer,6) == 0) {
			if ( DIR_LEFT == 1 || DIR_RIGHT == 1 )
				stop();
				
			setMotorDir(BWD,BWD);
			if ( M_SPEED_L != M_SPEED_R) {
				M_SPEED_L = max(M_SPEED_L,M_SPEED_R);
				M_SPEED_R = max(M_SPEED_L,M_SPEED_R);
			}
			moveAtSpeed(M_SPEED_L,M_SPEED_R);
			DIR_LEFT = -1; DIR_RIGHT = -1;  
			writeString_P("$OK&");
			send = 1;
		}
		
		// turn left
		else if (strncmp("$D|LEFT", receiveBuffer,7) == 0) {
			if ( DIR_LEFT == 1 || DIR_RIGHT == -1 )
				stop();
			setMotorDir(BWD,FWD);
			moveAtSpeed(60,60);
			DIR_LEFT = -1; DIR_RIGHT = 1;  
			writeString_P("$OK&");
			send = 1;
		}
		
		// turn right
		else if (strncmp("$D|RIGHT", receiveBuffer,8) == 0) {
			if ( DIR_LEFT == -1 || DIR_RIGHT == 1 )
				stop();
			setMotorDir(FWD,BWD);
			moveAtSpeed(60,60);
			DIR_LEFT = 1; DIR_RIGHT = -1;  
			writeString_P("$OK&");
			send = 1;
		}
		
		// stop
		else if (strncmp("$D|STOP", receiveBuffer,7) == 0) {
			stop();
			M_SPEED_L = 0;
			M_SPEED_R = 0;
			writeString_P("$OK&");
			send = 1;
		}
		
		// increase motorspeed
		else if (strncmp("$D|BU", receiveBuffer, 5) == 0) {
			if (M_SPEED_L <= (200 - SPEED_STEP) && M_SPEED_R <= (200 - SPEED_STEP)) {
				M_SPEED_L += SPEED_STEP;
				M_SPEED_R += SPEED_STEP;
			}
			moveAtSpeed(M_SPEED_L,M_SPEED_R);
			writeString_P("$OK&");
			send = 1;
		}
			
		// decrease motorspeed
		else if (strncmp("$D|BD", receiveBuffer, 5) == 0) {
			if (M_SPEED_L >= SPEED_STEP && M_SPEED_R >= SPEED_STEP) {
				M_SPEED_L -= SPEED_STEP;
				M_SPEED_R -= SPEED_STEP;
			}
			moveAtSpeed(M_SPEED_L,M_SPEED_R);
			writeString_P("$OK&");
			send = 1;
		}
		
		// increase left motorspeed
		else if (strncmp("$D|LU", receiveBuffer, 5) == 0) {
			if (M_SPEED_L <= (200 - SPEED_STEP)) M_SPEED_L += SPEED_STEP;
			moveAtSpeed(M_SPEED_L, M_SPEED_R);
			writeString_P("$OK&");
			send = 1;
		}
		
		// increase right motorspeed
		else if (strncmp("$D|RU", receiveBuffer, 5) == 0) {
			if (M_SPEED_R <= (200 - SPEED_STEP)) M_SPEED_R += SPEED_STEP;
			moveAtSpeed(M_SPEED_L, M_SPEED_R);
			writeString_P("$OK&");
			send = 1;
		}
		
		// decrease left motorspeed
		else if (strncmp("$D|LD", receiveBuffer, 5) == 0) {
			if (M_SPEED_L >= SPEED_STEP) M_SPEED_L -= SPEED_STEP;
			moveAtSpeed(M_SPEED_L, M_SPEED_R);
			writeString_P("$OK&");
			send = 1;
		}
		
		// decrease right motorspeed
		else if (strncmp("$D|RD", receiveBuffer, 5) == 0) {
			if (M_SPEED_R >= SPEED_STEP) M_SPEED_R -= SPEED_STEP;
			moveAtSpeed(M_SPEED_L, M_SPEED_R);
			writeString_P("$OK&");
			send = 1;
		}
		
		// set motorspeed to default value
		else if (strncmp("$D|DEFAULT", receiveBuffer, 10) == 0) {
			M_SPEED_L = 80;
			M_SPEED_R = 80;
			writeString_P("$OK&");			
			send = 1;
		}
		
		// nothing detected, set send to 0
		else send = 0;
		
		// if something was send, clear receiveBuffer
		if (send) {
			memset(receiveBuffer, '\0', charsToReceive);
			for(uint8_t cnt = 0; cnt < charsToReceive; cnt++)

				receiveBuffer[cnt]=0;
		}
		
	task_RP6System();
		

	}

	// End of main loop!

	// ---------------------------------------



	return 0;

}