Wie die Bumper richtig ins Programm einbringen?

**radbruch** · 12.10.2009, 13:35

Hallo

Keine Erfahrung hat nichts mit "doof" zu tun. Ich bin auch nicht so das Programmierass:

Code:

// Einfaches Fahrprogramm mit Überprüfung der Bumper                 12.10.09 mic

// Wegen der Belastung durch den Aufprall ist die Verwendung
// des ACS natürlich besser!

#include "RP6RobotBaseLib.h"

#define test 1 // 0 bedeutet Simulation ohne Motorenansteuerung

#define status_start              0
#define status_fahren             1 // Vorwärtsfahrt mit Überprüfung der Bumpers
#define status_angestossen_links  2
#define status_angestossen_rechts 3
#define status_angestossen_beide  4

uint8_t move_state; // Speicher für die aktuelle Fahrsituation

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

	if(test) powerON(); // Ab jetzt könnte der RP6 losfahren und sich verletzen!
	
	move_state=status_start; // Startstatus setzen

	while(true) // Endlos bis Akkus leer
	{
		switch(move_state)
		{
			case status_start:
				setLEDs(0b011011);
			   setMotorDir(FWD, FWD);
    			if(test) moveAtSpeed(150,150);
    			move_state=status_fahren;
				break;

			case status_fahren:
				setLEDs(0b001001);
				do
				{
					task_Bumpers();
					task_motionControl();
					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);
				setLEDs(0);
				if(test) move(150,100,BWD,1); // Geregelt bremsen und 100 zurückfahren
				   else mSleep(500); // Simmulation
				break;

			case status_angestossen_links:
				setLEDs(0b010000);
			   if(test) rotate(150,RIGHT,90,1); // Geregelt drehen
			      else mSleep(2000);
			   move_state=status_start;
			   break;

			case status_angestossen_rechts:
				setLEDs(0b000010);
			   if(test) rotate(150,LEFT,90,1);
			      else mSleep(2000);
			   move_state=status_start;
			   break;

			case status_angestossen_beide:
				setLEDs(0b010010);
			   if(test) rotate(150,LEFT,180,1);
			      else mSleep(2000);
			   move_state=status_start;
			   break;
		}
	}
	return 0;
}

Ach, jetzt habe das mit der Tour völlig vergessen. Das dauert noch etwas...

Gruß

mic

[Edit]
Jetzt mit Tour, allerdings ist das Ausweichmanöver eher sinnfrei:

Code:

// Besseres Fahrprogramm mit Überprüfung der Bumper                     12.10.09 mic

// Wegen der Belastung durch den Aufprall ist die Verwendung des ACS
// natürlich besser!

// Die jetzt nichtblockiernden Fahrfunktionen müssen "von Hand" mit der
// task_motionControl()-Funktion solange angestossen werden bis isMovementComplete()
// das Erreichen des Etappenziels signalisiert. Dann wird die Tour weitergeschaltet.

// Sollte während der Fahrt ein Bumper betätigt werden, weicht der RP6 aus.
// Danach setzt setzt er die Fahrt mit dem aktellen Etappenziel fort.

// Allerdings kann ich das alles im Moment nicht testen und weiß deshalb nicht,
// wie die Tour überhaupt aussieht und wohin er sinnvollerweise ausweichen sollte ;)

#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

uint8_t move_state; // Speicher für die aktuelle Fahrsituation
uint8_t tour; // Einzelschritte der Tour

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

	powerON(); // Ab jetzt könnte der RP6 losfahren und sich verletzen!
	
	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);
            break;
			case 2: setLEDs(0b000111); rotate(50, RIGHT, 90, false);
            break;
			case 3: setLEDs(0b110110); move(75, FWD, DIST_MM(500), false);
            break;
			case 4: setLEDs(0b111000); rotate(50, LEFT, 90, false);
            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
		{
 			case status_fahren:
				do
				{
					task_Bumpers();
					task_motionControl();
					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);
					move(150,100,BWD,1); // mit Rampen bremsen und 100 zurückfahren
				}
				break;

			case status_angestossen_links:
				setLEDs(0b010000);
			   rotate(150,RIGHT,90,1); // mit Rampen blockierend drehen
			   move_state=status_fahren;
			   break;

			case status_angestossen_rechts:
				setLEDs(0b000010);
			   rotate(150,LEFT,90,1);  // mit Rampen blockierend drehen
			   move_state=status_fahren;
			   break;

			case status_angestossen_beide:
				setLEDs(0b010010);
				rotate(150,LEFT,180,1); // mit Rampen blockierend wenden
			   move_state=status_fahren;
			   break;
		}
	}
	return 0;
}

[Edit2]
Mit ACS-Vorbereitung:

Code:

// Gutes Fahrprogramm mit Überprüfung der Bumper und ACS               12.10.09 mic

// Die nichtblockiernden Fahrfunktionen müssen "von Hand" mit der
// task_motionControl()-Funktion solange angestossen werden bis isMovementComplete()
// das Erreichen des Etappenziels signalisiert. Dann wird die Tour weitergeschaltet.

// Sollte während der Fahrt ein Bumper betätigt werden oder das ACS ansprechen,
// weicht der RP6 aus.
// Danach setzt setzt er die Fahrt mit dem aktellen Etappenziel fort.

// Allerdings kann ich das alles im Moment nicht testen und weiß deshalb nicht,
// wie die Tour überhaupt aussieht und wohin er sinnvollerweise ausweichen sollte ;)

#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

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);
            break;
			case 2: setLEDs(0b000111); rotate(50, RIGHT, 90, false);
            break;
			case 3: setLEDs(0b110110); move(75, FWD, DIST_MM(500), false);
            break;
			case 4: setLEDs(0b111000); rotate(50, LEFT, 90, false);
            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();

					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;

					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;
				}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);
					move(150,100,BWD,1); // mit Rampen bremsen und 100 zurückfahren
				}
				break;

			case status_angestossen_links:
				setLEDs(0b010000);
			   rotate(150,RIGHT,90,1); // mit Rampen blockierend drehen
			   move_state=status_fahren;
			   break;

			case status_angestossen_rechts:
				setLEDs(0b000010);
			   rotate(150,LEFT,90,1);  // mit Rampen blockierend drehen
			   move_state=status_fahren;
			   break;

			case status_angestossen_beide:
				setLEDs(0b010010);
				rotate(150,LEFT,180,1); // mit Rampen blockierend wenden
			   move_state=status_fahren;
			   break;

			case status_ACS_links:
			   break;
			case status_ACS_rechts:
			   break;
			case status_ACS_beide:
			   break;
		}
	}
	return 0;
}

Macht Spass :)

Thema: Wie die Bumper richtig ins Programm einbringen?

Themen-Optionen

Thema durchsuchen

Anzeige

Baum-Darstellung

Lesezeichen

Lesezeichen

Berechtigungen