Hallo

Das war ja eigentlich zu erwarten. Das erste große Problem ist das sture Ausweichmanöver. Der RP6 müßte mehr an die jeweilige Situation angepasst reagieren. Hier könnte man im Ausweichmanöver den jeweiligen Schritt der Tour (zufällig sind die move() ungerade und die rotate() gerade Schritte ;) berücksichtigen. Vermutlich treten die meisten Kollisionen aber bei Fahren auf. Dann weiß man aber nicht, wo eigentlich der Fehler aufgetreten war, was weiter zur Verirrung führt.

Das zweite große Problem ist die sture Wiederholung der aktuellen Etappe nach dem Ausweichen, unabhängig vom schon zurückgelegeten Weg. Hier könnte man in den Untiefen der Motion-Controll-Task zwar die aktuellen Zwischenwerte erkunden und als Startparameter für die Weiterfahrt verwenden. Letztlich werden aber auch hier wieder Fehler auftreten die sich zu weiteren Kursabweichungen summieren werden.

Eine Zwischenlösung wäre vielleicht, das Ausweichmanöver direkt im Tourschritt mit anzugeben:
Code:
// Guteres Fahrprogramm mit Überprüfung der Bumper und ACS              13.10.09 mic

// Neu sind nun individuelle Ausweichmanöver bei den einzelnen Touretappen.

#include "RP6RobotBaseLib.h"

#define status_fahren					0 // Vorwärtsfahrt mit Überprüfung der Bumper
#define status_angestossen_links		1
#define status_angestossen_rechts	2
#define status_angestossen_beide		3
#define status_ACS_links  				4
#define status_ACS_rechts   			5
#define status_ACS_beide    			6

uint8_t move_state; // Speicher für die aktuelle Fahrsituation
uint8_t tour; // Einzelschritte der Tour
uint8_t ausweich_winkel; // Drehwinkel bei Kurskorrektur
uint16_t ausweich_weg; // Zurückfahrstrecke bei Kollisition

int main(void)
{
	initRobotBase(); // Sollte man immer machen!
	setLEDs(0b111111);
	mSleep(2500);

	powerON(); // Ab jetzt könnte der RP6 losfahren und sich verletzen!
   setACSPwrMed(); // mittelweites ACS aktivieren
   
	move_state=status_fahren; // Startstatus setzen
	tour=1; // die Tour beginnt bei Schritt 1

	while(true) // Endlos bis Akkus leer
	{
		if(move_state==status_fahren) switch(tour) // nur wenn kein Ausweichen aktiv
		{
			case 1: setLEDs(0b110110); move(75, FWD, DIST_MM(900), false);
			   ausweich_weg=30; ausweich_winkel=20; // Ausweichen: 30 zurück, 20 drehen
            break;
			case 2: setLEDs(0b000111); rotate(50, RIGHT, 90, false);
			   ausweich_weg=30, ausweich_winkel=60;
            break;
			case 3: setLEDs(0b110110); move(75, FWD, DIST_MM(500), false);
				ausweich_weg=600; ausweich_winkel=0; // nur zurück, kein drehen!
            break;
			case 4: setLEDs(0b111000); rotate(50, LEFT, 90, false);
			   ausweich_weg=50, ausweich_winkel=20; // gilt für den Rest der Tour!
            break;
			case 5: setLEDs(0b110110); move(75, FWD, DIST_MM(1400), false);
            break;
			case 6: setLEDs(0b000111); rotate(50, RIGHT, 90, false);
            break;
			case 7: setLEDs(0b110110); move(75, FWD, DIST_MM(1300), false);
            break;
			case 8: setLEDs(0b000111); rotate(50, RIGHT, 90, false);
            break;
			case 9: setLEDs(0b110110); move(75, FWD, DIST_MM(1000), false); //MITTE!
            break;
			case 10: setLEDs(0b111111); rotate(50, RIGHT, 360, false);
            break;
			case 11: setLEDs(0b111011); move(75, BWD, DIST_MM(1000), false);
            break;
			case 12: setLEDs(0b111000); rotate(50, LEFT, 90, false);
            break;
			case 13: setLEDs(0b011011); move(75, BWD, DIST_MM(1300), false);
            break;
			case 14: setLEDs(0b111000); rotate(50, LEFT, 90, false);
            break;
			case 15: setLEDs(0b011011); move(75, BWD, DIST_MM(1400), false);
            break;
			case 16: setLEDs(0b000111); rotate(50, RIGHT, 90, false);
            break;
			case 17: setLEDs(0b011011); move(75, BWD, DIST_MM(500), false);
            break;
			case 18: setLEDs(0b111000); rotate(50, LEFT, 90, false);
            break;
			case 19: setLEDs(0b011011); move(75, BWD, DIST_MM(900), false);
				break;
			case 20: while(1) // Ende der Tour mit blinken
				{setLEDs(0b111000); mSleep(200); setLEDs(0b000111); mSleep(200);}
			   break;
		}

		switch(move_state) // Fahren bis Etappenziel oder Ausweichen bei Bumper oder ACS
		{
 			case status_fahren:
				do
				{
					task_ACS();
					task_Bumpers();
					task_motionControl();
					// erst ACS und dann Bumpers prüfen,
					if(obstacle_left && obstacle_right) move_state=status_ACS_beide;
					else if(obstacle_left) move_state=status_ACS_links;
					else if(obstacle_right) move_state=status_ACS_rechts;
					// weil Bumper eine höhere Priorität haben!
					if(bumper_left && bumper_right) move_state=status_angestossen_beide;
					else if(bumper_left) move_state=status_angestossen_links;
					else if(bumper_right) move_state=status_angestossen_rechts;
				}while((move_state==status_fahren) && !isMovementComplete());

				if(move_state==status_fahren)
				{
					tour++; // Etappenziel erreicht, weiterschalten zu nächstem Ziel
					// if(tour>19) tour=1 // Neustart der Tour (bei 20 ist Ende!)
				}
				else // oder bremsen weil ein Bumper betätigt ist
				{
					setLEDs(0);
					stop(); // sofort anhalten und nochmals Bumpers prüfen
					if(bumper_left && bumper_right) move_state=status_angestossen_beide;

					if(move_state < status_ACS_links) // bei Bumper doppelte Ausweichwerte
					{
					   ausweich_weg = ausweich_weg*2; // (0 bleibt 0 :)
					   ausweich_winkel = ausweich_winkel*2; // Winkel ist nur uint8_t!
					}
					if(ausweich_weg) // nur wenn zurückfahren nötig ist
						move(150,BWD,DIST_MM(ausweich_weg),1); // zurückfahren
				}
				break;

			case status_angestossen_links:  // drehen wenn nötig  (lang)
				setLEDs(0b011000);
			   if(ausweich_winkel) // nur wenn ein Winkel angegeben ist
					rotate(100,RIGHT,ausweich_winkel,1); // wird auch wirklich gedreht
			   move_state=status_fahren;
			   break;

			case status_angestossen_rechts:
				setLEDs(0b000011);
			   if(ausweich_winkel)
			   	rotate(100,LEFT,ausweich_winkel,1);
			   move_state=status_fahren;
			   break;

			case status_angestossen_beide:  // zweimal ausweichen! (lang)
				setLEDs(0b011011);
			   if(ausweich_winkel)
					rotate(100,RIGHT,ausweich_winkel,1);
				if(ausweich_weg) // wenn beide Seiten gedrückt sind flüchten wir
					move(150,BWD,DIST_MM(ausweich_weg),1);
			   if(ausweich_winkel)
					rotate(100,LEFT,ausweich_winkel,1);
			   move_state=status_fahren;
			   break;

			case status_ACS_links:  // drehen wenn nötig (kurz)
				setLEDs(0b010000);
			   if(ausweich_winkel)
			   	rotate(100,RIGHT,ausweich_winkel,1);
			   move_state=status_fahren;
			   break;
			case status_ACS_rechts: // drehen wenn nötig
				setLEDs(0b000010);
			   if(ausweich_winkel)
			   	rotate(100,LEFT,ausweich_winkel,1);
			   move_state=status_fahren;
			   break;
			case status_ACS_beide: // zweimal ausweichen! (kurz)
				setLEDs(0b010010);
			   if(ausweich_winkel)
					rotate(100,RIGHT,ausweich_winkel,1);
				if(ausweich_weg)
					move(150,BWD,DIST_MM(ausweich_weg),1);
			   if(ausweich_winkel)
					rotate(100,LEFT,ausweich_winkel,1);
			   move_state=status_fahren;
			   break;
		}
	}
	return 0;
}
Kommentare sollten möglichst sinnvoll sein, wenn sich die Aktion durch den Befehl selbst erklärt, kann man ihn auch einsparen. Wenn möglich sollten sie aber in der Codebox (Vorschau) nicht über das Zeilenende hinausragen, denn dann kann man den Code nicht mehr richtig kopieren. Deshalb, und weil ich den Code noch besser kenne, habe ich die Änderungen in meiner Programmversion eingefügt.

Gruß

mic

[Edit]
Nachdem nun mein RP6 auch wieder läuft habe ich a: den move()-Befehl beim Zurückstossen korrigiert, b: die etappenabhängigen Ausweichmanöver "weicher" und kleiner gemacht und c: festgestellt, dass es unmöglich ist, ACS oder Bumper auf beiden Seiten gleichzeitig zu betätigen, und letztlich d: die Abfrage von ACS und Bumpers getauscht. Der Code oben wurde deshalb entsprechend angepasst.