Stopwatch wird ignoriert/ falsch interpretiert

**Mac80** · 20.04.2012, 21:49

Hallo, ich bin wieder auf ein neues Problem gestoßen.
Der Roboter soll bei der ACS links+rechts Erkennung eine kurze Pause einlegen (ca 5000ms) und danach weiterfahren, laut Programm. Ich habe hier den nur den Base als Master.

Mein Code-Ausschnitt (aus Examples Base Move 5):

Code:

#define AVOID_SPEED_L_ARC_LEFT  30
#define AVOID_SPEED_L_ARC_RIGHT 40 // 90
#define AVOID_SPEED_R_ARC_LEFT  40 // 90
#define AVOID_SPEED_R_ARC_RIGHT 30
#define AVOID_SPEED_ROTATE 	30     // 60
#define AVOID_SPEED_WAIT 0   //Motoren halt

// States for the Avoid FSM:
#define AVOID_OBSTACLE_RIGHT 		1
#define AVOID_OBSTACLE_LEFT 		2
#define AVOID_OBSTACLE_MIDDLE	    3
#define AVOID_OBSTACLE_MIDDLE_WAIT 	4
#define AVOID_END 					5
behaviour_command_t avoid = {0, 0, FWD, false, false, 0, IDLE};

void behaviour_avoid(void)
{
	static uint8_t last_obstacle = LEFT;
	static uint8_t obstacle_counter = 0;
	switch(avoid.state)
	{
		case IDLE: 
			if(obstacle_right && obstacle_left) // left AND right sensor have detected something...
				avoid.state = AVOID_OBSTACLE_MIDDLE;
			else if(obstacle_left)  // Left "sensor" has detected something
				avoid.state = AVOID_OBSTACLE_LEFT;
			else if(obstacle_right) // Right "sensor" has detected something
				avoid.state = AVOID_OBSTACLE_RIGHT;
		break;
		case AVOID_OBSTACLE_MIDDLE:
			avoid.dir = last_obstacle;
			avoid.speed_left = AVOID_SPEED_WAIT;
			avoid.speed_right = AVOID_SPEED_WAIT;
			setStopwatch4(0);
			startStopwatch4();
						
			while(getStopwatch4() >= 4990){		// Die while Schleife verursacht nur Probleme....
			if (getStopwatch4() >=5000)
			{
			setStopwatch4(0);
			}}
			avoid.speed_left = AVOID_SPEED_ROTATE;
			avoid.speed_right = AVOID_SPEED_ROTATE;
			//else
			
			
					
			
						
			if(!(obstacle_left || obstacle_right))
			{
				if(obstacle_counter > 3)
				{
					obstacle_counter = 0;
					setStopwatch4(0);
				}
				else
					setStopwatch4(400);
				startStopwatch4();
				avoid.state = AVOID_END;
			}
		break;
		case AVOID_OBSTACLE_RIGHT:
			avoid.dir = FWD;
			
			avoid.speed_left = AVOID_SPEED_L_ARC_LEFT;
			avoid.speed_right = AVOID_SPEED_L_ARC_RIGHT;
			if(obstacle_right && obstacle_left)
				avoid.state = AVOID_OBSTACLE_MIDDLE;
			if(!obstacle_right)
			{
				setStopwatch4(500);
				startStopwatch4();
				avoid.state = AVOID_END;
			}
			last_obstacle = RIGHT;
			obstacle_counter++;
		break;
		case AVOID_OBSTACLE_LEFT:
			avoid.dir = FWD;
			avoid.speed_left = AVOID_SPEED_R_ARC_LEFT;
			avoid.speed_right = AVOID_SPEED_R_ARC_RIGHT;
			if(obstacle_right && obstacle_left)
				avoid.state = AVOID_OBSTACLE_MIDDLE;
			if(!obstacle_left)
			{
				setStopwatch4(500); 
				startStopwatch4();
				avoid.state = AVOID_END;
			}
			last_obstacle = LEFT;
			obstacle_counter++;
		break;
		case AVOID_END:
			if(getStopwatch4() > 1000) // We used timing based movement here!
			{	stopStopwatch4();
				setStopwatch4(0);
				avoid.state = IDLE;
			}
		break;
	}
}

Die Wartezeit soll nur einmal aufgerufen werden pro Ausweichmanöver.
Ich habe da verschieden Schleifenvariationen ausprobiert und der RP6 ignoriert die Erkennung oder bleibt stehen. Ohne Schleife wird immer ein Teil übersprungen.
Wäre es sinnvoller den Timer in einem weiteren case z.B. AVOID_OBSTACLE_MIDDLE_WAIT einzurichten und mit einem "avoid.state = AVOID_OBSTACLE_MIDDLE;" überzuleiten? Oder läuft es auf das selbe Ergebnis heraus?

Wer sieht meinen Fehler?

**Mac80** · 21.04.2012, 12:21

Habs!

Nur der Befehl "move(0,FWD,0,BLOCKING);" löst das Problem (warum auch immer...

)
Der neue Code ist:

Code:

case AVOID_OBSTACLE_MIDDLE_WAIT:
			
			move(0,FWD,0,BLOCKING); //RP6 Motoren halt
			setStopwatch4(0);
			startStopwatch4();			
			while(getStopwatch4() <5001)    //5001 stopwatch Ungenauigkeit
                        {
			if(getStopwatch4() >=5000)
			{
			avoid.state = AVOID_OBSTACLE_MIDDLE;
			}
			}			
			setStopwatch4(0);

**SlyD** · 22.04.2012, 10:01

Hallo,

eigentlich sind diese Verhalten so gedacht, dass da nirgendwo eine blockierende Funktion oder Schleife eingefügt wird (abgesehen von ganz kurzen Pausen).
Verzögerungen kannst Du implementieren indem Du einfach einen neuen Zustand einführst und dort dann die Stopwatch Abfrage ist.

s. Escape Verhalten als Beispiel.

Denn wenn Du das nicht so machst, kann ein Verhalten das höhere Priorität hat (also z.B. Escape oder Akku leer) das AVOID Verhalten nicht
überstimmen und auch andere Sensorwerte für das aktive Verhalten haben keinen Einfluss...

MfG,
SlyD

**Mac80** · 09.05.2012, 13:31

Hallo SlyD,

wegen der Schleifen und der Blocking Funktion zum Thema: Stopwatch wird ignoriert/ falsch interpretiert:
Ich habe einen neuen Zustand definiert, welcher den Stopwatch bis 5000ms höchzählen enthält. Dieser wird ohne Blocking Funktion übersprungen.

Code:

void behaviour_avoid(void)
{
	static uint8_t last_obstacle = LEFT;
	static uint8_t obstacle_counter = 0;
	switch(avoid.state)
	{
		case IDLE: 
			if((obstacle_right && obstacle_left) || (srf_middle_avoid) || (srf_middle_avoid && srf_left_avoid && srf_right_avoid)) // Wenn Sensoren ACS und SRF08
				{
				avoid.state = AVOID_OBSTACLE_MIDDLE_WAIT;}
			else if((obstacle_left) || (srf_left_avoid))  // Sensor Links ACS und SRF08 
				avoid.state = AVOID_OBSTACLE_LEFT;
			else if((obstacle_right) || (srf_right_avoid)) // Sensor Rechts ACS und SRF08
				avoid.state = AVOID_OBSTACLE_RIGHT;
		break;
		case AVOID_OBSTACLE_MIDDLE_WAIT:
			moveAtSpeed(0,0);
			
			{
			setStopwatch4(400);
			startStopwatch4();
			avoid.state = AVOID_OBSTACLE_MIDDLE_MIDDLE;
			}						
			setStopwatch5(0);				
		case AVOID_OBSTACLE_MIDDLE_MIDDLE:
			
			startStopwatch5();			
			if((getStopwatch5() >=5000))
			{
			setStopwatch4(400);
			startStopwatch4();
			avoid.state = AVOID_OBSTACLE_MIDDLE;
			}
		break;
		case AVOID_OBSTACLE_MIDDLE:
			avoid.dir = last_obstacle;
			avoid.speed_left = AVOID_SPEED_ROTATE;
			avoid.speed_right = AVOID_SPEED_ROTATE;			
			if(!(obstacle_left || obstacle_right || srf_middle_avoid))
			{
				if(obstacle_counter > 3)
				{
					obstacle_counter = 0;
					setStopwatch4(0);
				}
				else
					setStopwatch4(400);
				startStopwatch4();
				avoid.state = AVOID_END;
			}
		break;		
		case AVOID_OBSTACLE_RIGHT:
			avoid.dir = FWD;
			avoid.speed_left = AVOID_SPEED_L_ARC_LEFT;
			avoid.speed_right = AVOID_SPEED_L_ARC_RIGHT;
			if((obstacle_right && obstacle_left) || (srf_middle_avoid))
				avoid.state = AVOID_OBSTACLE_MIDDLE_WAIT;
			if(!obstacle_right)
			{
				setStopwatch4(500);
				startStopwatch4();
				avoid.state = AVOID_END;
			}
			last_obstacle = RIGHT;
			obstacle_counter++;
		break;
		case AVOID_OBSTACLE_LEFT:
			avoid.dir = FWD;
			avoid.speed_left = AVOID_SPEED_R_ARC_LEFT;
			avoid.speed_right = AVOID_SPEED_R_ARC_RIGHT;
			if((obstacle_right && obstacle_left) || (srf_middle_avoid))
				avoid.state = AVOID_OBSTACLE_MIDDLE_WAIT;
			if(!obstacle_left)
			{
				setStopwatch4(500); 
				startStopwatch4();
				avoid.state = AVOID_END;
			}
			last_obstacle = LEFT;
			obstacle_counter++;
		break;
		case AVOID_END:
			if(getStopwatch4() > 1000)
			{	stopStopwatch4();
				setStopwatch4(0);
				avoid.state = IDLE;
			}
		break;
	}
}

Übersehe ich da etwas?

Gruß
Mac80