Programmfehler

[Arjen98]

Hallo Community,

Ich habe letztens ein Programm geschrieben, dass leider nicht so funktioniert, wie ich es möchte.

Und zwar liegt mein erstes Problem darin, dass das ACS nicht so funktioniert, wie es soll, wenn der Roboter in der Schleife ist reagiert er nicht auf die Hindernisse wie er soll. Auch der Befehl, der Teil mit:

while(!bumper_left && !bumper_right)
{
  moveAtSpeed(70,70)
}
moveAtSpeed(0,0)

... funktioniert nicht.
Was sind meine Fehler.
Ich bitte es zu entschuldigen, dass ich diese warscheinlich Anfängerfehler mache, da ich noch ein Neuling auf dem Gebiet bin.

Ich würde mich sehr über Hinweise, warum das Programm nicht so funktioniert wie es soll, freuen.
Danke schonmal vorab für die Hilfe.
Gruß Arjen

Programm:

#include <asf.h>
#include "RP6RobotBaseLib.h"

#define Leerlauf 0

typedef struct {
	uint8_t Fahren_Links;
	uint8_t Fahren_Rechts;
	unsigned Richtung:2;
	unsigned Bewegung:1;
	unsigned Drehen:1;
	uint16_t Bewegungs_Wert;
	uint8_t Zustand;
} behaviour_command_t;

behaviour_command_t Stop = {0,0,FWD,false,false,0,Leerlauf};

#define Cruise_Fahren_Vorwaerts  70
#define Vorwaerts_Bewegen 1

behaviour_command_t Cruise ={Cruise_Fahren_Vorwaerts,Cruise_Fahren_Vorwaerts,FWD,false,false,0,Vorwaerts_Bewegen};

void behaviour_Cruise(void)
{
	
}

#define Escape_Fahren_Rueckwaerts 70
#define Escape_Fahren_Drehen 50

#define Escape_Frontal			1
#define Escape_Frontal_Warten	2
#define Escape_Links			3
#define Escape_Links_Warten		4
#define Escape_Rechts			5
#define Escape_Rechts_Warten	6
#define Escape_Warten_Ende		7

behaviour_command_t Escape = {0,0,FWD,false,false,0,Leerlauf};

void behaviour_Escape(void)
{
	static uint8_t Taster_Ergebnis = 0;
	
	switch(Escape.Zustand)
	{
		case Leerlauf:
		break;
		case Escape_Frontal:
		Escape.Fahren_Links = Escape_Fahren_Rueckwaerts;
		Escape.Richtung = BWD;
		Escape.Bewegung = true;
		if(Taster_Ergebnis > 3)
		Escape.Bewegungs_Wert = 220;
		else
		Escape.Bewegungs_Wert = 160;
		Escape.Zustand = Escape_Frontal_Warten;
		Taster_Ergebnis+=2;
		break;
		case Escape_Frontal_Warten:
		if(!Escape.Bewegung)
		{
			Escape.Fahren_Links = Escape_Fahren_Drehen;
			if(Taster_Ergebnis > 3)
			{
				Escape.Bewegungs_Wert = 100;
				Escape.Richtung = RIGHT;
				Taster_Ergebnis =0;
			}
			else
			{
				Escape.Richtung = LEFT;
				Escape.Bewegungs_Wert = 70;
			}
			Escape.Drehen = true;
			Escape.Zustand = Escape_Warten_Ende;
		}
		break;
		case Escape_Links:
		Escape.Fahren_Links = Escape_Fahren_Rueckwaerts;
		Escape.Richtung = BWD;
		Escape.Bewegung = true;
		if(Taster_Ergebnis > 3)
		Escape.Bewegungs_Wert = 190;
		else
		Escape.Bewegungs_Wert = 150;
		Escape.Zustand = Escape_Links_Warten;
		Taster_Ergebnis++;
		break;
		case Escape_Links_Warten:
		if(!Escape.Bewegung)
		{
			Escape.Fahren_Links = Escape_Fahren_Drehen;
			Escape.Richtung = RIGHT;
			Escape.Drehen = true;
			if(Taster_Ergebnis > 3)
			{
				Escape.Bewegungs_Wert = 110;
				Taster_Ergebnis = 0;
			}
			else
			Escape.Bewegungs_Wert = 80;
			Escape.Zustand = Escape_Warten_Ende;
		}
		break;
		case Escape_Rechts:
		Escape.Fahren_Links = Escape_Fahren_Rueckwaerts;
		Escape.Richtung = BWD;
		Escape.Bewegung = true;
		if(Taster_Ergebnis > 3)
		Escape.Bewegungs_Wert = 190;
		else
		Escape.Bewegungs_Wert = 150;
		Escape.Zustand = Escape_Rechts_Warten;
		Taster_Ergebnis++;
		break;
		case Escape_Rechts_Warten:
		if(!Escape.Bewegung)
		{
			Escape.Fahren_Links = Escape_Fahren_Drehen;
			Escape.Richtung = LEFT;
			Escape.Drehen = true;
			if(Taster_Ergebnis > 3)
			{
				Escape.Bewegungs_Wert = 110;
				Taster_Ergebnis = 0;
			}
			else
			Escape.Bewegungs_Wert = 80;
			Escape.Zustand = Escape_Warten_Ende;
		}
		break;
		case Escape_Warten_Ende:
		if(!(Escape.Bewegung || Escape.Drehen))
		Escape.Zustand = Leerlauf;
		break;
	}
}

void bumpersStateChanged(void)
{
	if(bumper_left && bumper_right)
	{
		Escape.Zustand = Escape_Frontal;
	}
	else if (bumper_left)
	{
		if(Escape.Zustand != Escape_Frontal_Warten)
		Escape.Zustand = Escape_Links;
	}
	else if(bumper_right)
	{
		if(Escape.Zustand != Escape_Frontal_Warten)
		Escape.Zustand = Escape_Rechts;
	}
}

#define AVOID_SPEED_L_ARC_LEFT  30
#define AVOID_SPEED_L_ARC_RIGHT 40
#define AVOID_SPEED_R_ARC_LEFT  40
#define AVOID_SPEED_R_ARC_RIGHT 30
#define AVOID_SPEED_ROTATE 		30


#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, Leerlauf};

void behaviour_avoid(void)
{
	static uint8_t last_obstacle = LEFT;
	static uint8_t obstacle_counter = 0;
	switch(avoid.Zustand)
	{
		case Leerlauf:
		
		if(obstacle_right && obstacle_left)
		avoid.Zustand = AVOID_OBSTACLE_MIDDLE;
		else if(obstacle_left)
		avoid.Zustand= AVOID_OBSTACLE_LEFT;
		else if(obstacle_right)
		avoid.Zustand = AVOID_OBSTACLE_RIGHT;
		break;
		case AVOID_OBSTACLE_MIDDLE:
		avoid.Richtung = last_obstacle;
		avoid.Fahren_Links = AVOID_SPEED_ROTATE;
		avoid.Fahren_Rechts = AVOID_SPEED_ROTATE;
		if(!(obstacle_left || obstacle_right))
		{
			if(obstacle_counter > 3)
			{
				obstacle_counter = 0;
				setStopwatch4(0);
			}
			else
			setStopwatch4(400);
			startStopwatch4();
			avoid.Zustand = AVOID_END;
		}
		break;
		case AVOID_OBSTACLE_RIGHT:
		avoid.Richtung = FWD;
		avoid.Fahren_Links = AVOID_SPEED_L_ARC_LEFT;
		avoid.Fahren_Rechts = AVOID_SPEED_L_ARC_RIGHT;
		if(obstacle_right && obstacle_left)
		avoid.Zustand = AVOID_OBSTACLE_MIDDLE;
		if(!obstacle_right)
		{
			setStopwatch4(500);
			startStopwatch4();
			avoid.Zustand = AVOID_END;
		}
		last_obstacle = RIGHT;
		obstacle_counter++;
		break;
		case AVOID_OBSTACLE_LEFT:
		avoid.Richtung = FWD;
		avoid.Fahren_Links = AVOID_SPEED_R_ARC_LEFT;
		avoid.Fahren_Rechts = AVOID_SPEED_R_ARC_RIGHT;
		if(obstacle_right && obstacle_left)
		avoid.Zustand = AVOID_OBSTACLE_MIDDLE;
		if(!obstacle_left)
		{
			setStopwatch4(500);
			startStopwatch4();
			avoid.Zustand = AVOID_END;
		}
		last_obstacle = LEFT;
		obstacle_counter++;
		break;
		case AVOID_END:
		if(getStopwatch4() > 1000)
		{
			stopStopwatch4();
			setStopwatch4(0);
			avoid.Zustand = Leerlauf;
		}
		break;
	}
}

void acsStateChanged(void)
{
	if(obstacle_left && obstacle_right)
	statusLEDs.byte = 0b100100;
	else
	statusLEDs.byte = 0b000000;
	statusLEDs.LED5 = obstacle_left;
	statusLEDs.LED4 = (!obstacle_left);
	statusLEDs.LED2 = obstacle_right;
	statusLEDs.LED1 = (!obstacle_right);
	updateStatusLEDs();
}

void moveCommand(behaviour_command_t * cmd)
{
	if(cmd->Bewegungs_Wert > 0)
	{
		
		if(cmd->Drehen)
		rotate(cmd->Fahren_Links,cmd->Richtung,cmd->Bewegungs_Wert,false);
		else if(cmd->Bewegung)
		move(cmd->Fahren_Links,cmd->Richtung, DIST_MM(cmd->Bewegungs_Wert),false);
		cmd->Bewegungs_Wert = 0;
	}
	else if(!(cmd->Bewegung || cmd->Drehen))
	{
		changeDirection(cmd->Richtung);
		moveAtSpeed(cmd->Fahren_Links,cmd->Fahren_Rechts);
	}
	else if(isMovementComplete())
	{
		cmd->Drehen = false;
		cmd->Bewegung = false;
	}
}
void behaviourController(void)
{
	behaviour_Cruise();
	behaviour_avoid();
	behaviour_Escape();
	
	if(Escape.Zustand != Leerlauf)
	moveCommand(&Escape);
	else if(avoid.Zustand != Leerlauf)
	moveCommand(&avoid);
	else if(Cruise.Zustand != Leerlauf)
	moveCommand(&Cruise);
	else
	moveCommand(&Stop);
}

#define SERVO_OUT			SDA


#define LEFT_TOUCH			550
#define RIGHT_TOUCH			254
#define PULSE_ADJUST 		4
#define PULSE_REPETITION	19



void initSERVO(void)
{
	DDRC |= SERVO_OUT;
	PORTC &= ~SERVO_OUT;
	startStopwatch1();
}


void pulseSERVO(uint8_t position)
{
	cli();
	PORTC |= SERVO_OUT;
	delayCycles(LEFT_TOUCH);
	while (position--) {
		delayCycles(PULSE_ADJUST);
	}
	PORTC &= ~SERVO_OUT;
	sei();
}

uint8_t Drehelinks = 0;
uint8_t Dreherechts = 0;
void task_SERVO(void)
{
	static uint8_t pos;
	if (getStopwatch1() > PULSE_REPETITION)
	{
		pulseSERVO(pos);
		if(Drehelinks > 0)
		{
			if (getStopwatch2() > 48)
			{
				pos --;
				if (pos < 1)
				{
					pos = 1;
				}
				setStopwatch2(0);
			}
		}
		if(Dreherechts > 0)
		{
			if(getStopwatch2() > 48)
			{
				pos ++;
				if(pos > RIGHT_TOUCH)
				{
					pos = RIGHT_TOUCH;
				}
				setStopwatch2(0);
			}
			
		}
		setStopwatch1(0);
	}
}


int main (void)
{
	initRobotBase();
	board_init();
	BUMPERS_setStateChangedHandler(bumpersStateChanged);
	ACS_setStateChangedHandler(acsStateChanged);
	powerON();
	setACSPwrLow();
	while(true)
	{
		task_Bumpers();
		if(bumper_left)
		{
			move(70, FWD, DIST_MM(2500), true);
			while(!bumper_left && !bumper_right)
			{
				behaviourController();
				task_RP6System();
			}
			move(70, BWD, DIST_MM(100), true);
			rotate(50, RIGHT, 90, true);
			while(!bumper_left && !bumper_right)
			{
				moveAtSpeed(70,70);
			}
			moveAtSpeed(0,0);
			move(70, BWD, DIST_MM(100), true);
			rotate(50, LEFT, 90, true);
			move(70, FWD, DIST_MM(300), true);
			rotate(50, LEFT, 90, true);
			while(!bumper_left && !bumper_right)
			{
				move(70, FWD, DIST_MM(10), true);
				task_RP6System();
			}
			startStopwatch2();
			startStopwatch3();
			Drehelinks = 1;
			while(getStopwatch3() < 14000)
			{
				task_SERVO();
			}
			move(70, BWD, DIST_MM(200), true);
			rotate(50, LEFT, 90, true);
			while(!bumper_left && !bumper_right)
			{
				moveAtSpeed(70,70);
			}
			moveAtSpeed(0,0);
			move(70, BWD, DIST_MM(100), true);
			rotate(50, RIGHT, 90, true);
			move(70, FWD, DIST_MM(200), true);
			while(!bumper_left && !bumper_right)
			{
				behaviourController();
				task_RP6System();
			}
			move(70, BWD, DIST_MM(100), true);
			rotate(50, RIGHT, 90, true);
			while(!bumper_left && !bumper_right)
			{
				moveAtSpeed(70,70);
			}
			moveAtSpeed(0,0);
			move(70, BWD, DIST_MM(100), true);
			rotate(50, LEFT, 90, true);
			move(70, FWD, DIST_MM(150), true);
			rotate(50, LEFT, 90, true);
			move(70, FWD, DIST_MM(200), true);
			rotate(50, RIGHT, 90, true);
			while(!bumper_left && !bumper_right)
			{
				move(70, FWD, DIST_MM(10), true);
				task_RP6System();
			}
			Drehelinks = 0;
			Dreherechts = 1;
			setStopwatch3(0);
			while(getStopwatch3() < 14000)
			{
				task_SERVO();
			}
			move(70, BWD, DIST_MM(150), true);
			rotate(50, RIGHT, 180, true);
			move(70, FWD, DIST_MM(150), true);
			rotate(50, LEFT, 90, true);
			move(70, FWD, DIST_MM(200), true);
			rotate(50, RIGHT, 90, true);
			move(70, BWD, DIST_MM(200), true);
		}
		else if(bumper_right)
		{
			move(70, FWD, DIST_MM(2500), true);
			while(!bumper_left && !bumper_right)
			{
				behaviourController();
				task_RP6System();
			}
			move(70, BWD, DIST_MM(100), true);
			rotate(50, RIGHT, 90, true);
			while(!bumper_left && !bumper_right)
			{
				moveAtSpeed(70,70);
			}
			moveAtSpeed(0,0);
			move(70, BWD, DIST_MM(100), true);
			rotate(50, LEFT, 90, true);
			move(70, FWD, DIST_MM(450), true);
			rotate(50, LEFT, 90, true);
			while(!bumper_left && !bumper_right)
			{
				move(70, FWD, DIST_MM(10), true);
				task_RP6System();
			}
			startStopwatch2();
			startStopwatch3();
			Drehelinks = 1;
			while(getStopwatch3() < 14000)
			{
				task_SERVO();
			}
			move(70, BWD, DIST_MM(200), true);
			rotate(50, LEFT, 90, true);
			while(!bumper_left && !bumper_right)
			{
				moveAtSpeed(70,70);
			}
			moveAtSpeed(0,0);
			move(70, BWD, DIST_MM(100), true);
			rotate(50, RIGHT, 90, true);
			move(70, FWD, DIST_MM(200), true);
			while(!bumper_left && !bumper_right)
			{
				behaviourController();
				task_RP6System();
			}
			move(70, BWD, DIST_MM(100), true);
			rotate(50, RIGHT, 90, true);
			while(!bumper_left && !bumper_right)
			{
				moveAtSpeed(70,70);
			}
			moveAtSpeed(0,0);
			move(70, BWD, DIST_MM(100), true);
			rotate(50, LEFT, 90, true);
			move(70, FWD, DIST_MM(150), true);
			rotate(50, LEFT, 90, true);
			move(70, FWD, DIST_MM(200), true);
			rotate(50, RIGHT, 90, true);
			while(!bumper_left && !bumper_right)
			{
				move(70, FWD, DIST_MM(10), true);
				task_RP6System();
			}
			Drehelinks = 0;
			Dreherechts = 1;
			setStopwatch3(0);
			while(getStopwatch3() < 14000)
			{
				task_SERVO();
			}
			move(70, BWD, DIST_MM(150), true);
			rotate(50, RIGHT, 180, true);
			move(70, FWD, DIST_MM(150), true);
			rotate(50, LEFT, 90, true);
			move(70, FWD, DIST_MM(200), true);
			rotate(50, RIGHT, 90, true);
			move(70, BWD, DIST_MM(200), true);
		
		}
	}
	return 0;
}

[SlyD]

Hallo,

das ist ein genereller Denkfehler: bei der RP6Lib muss einiges "im Hintergrund" erledigt werden. Aber da hier kein Betriebssystem o.ä. läuft, muss dass explizit in der Hauptschleife aufgerufen werden.
Das sind die task_ Funktionen.

Die wichtigste:

task_RP6System();
muss INNERHALB von SCHLEIFEN die länger als ein paar Millisekunden laufen an einer Stelle aufgerufen werden.

moveAtSpeed hingegen muss nur genau EINMAL aufgerufen werden um die neue Geschwindigkeit zu setzen.
Das braucht nicht in die Schleife (darf, aber bringt nix).

Fang besser mit einem einfacheren kürzeren Programm an in dem Du das separat testen kannst.


MfG,
SlyD

[Arjen98]

Danke für die Antwort.

Ich habe einmal ein anderes, etwas einfacheres Programm zum folgen eines kleinen Parcours geschrieben, und würde dort gerne, dass der Roboter dieses Programm solange ausführt, bis beide Bumper durchgedrückt sind. Dies gelingt ja (meistens) mit der Schleife

while(!bumper_left && !bumper_right){...}

Jedoch funktioniert dies bei mir nicht wie gewollt, und auch wenn ich mit Stopwatches als Schleifenbedingung arbeite gibt es keine Reaktion. Nur in einer Endlosschleife funktioniert das Programm, aber es soll wie schon gesagt auf die Bumper reagieren.

Wo ist mein Denkfehler? und wie kann man dieses Problem umgehen?

Ich wäre euch für Hilfe sehr dankbar.

Grüße Arjen

Das Programm:

#include "RP6RobotBaseLib.h"


int main (void)
{
	initRobotBase();
	board_init();
	powerON();
	setACSPwrLow();
	setLEDs(0b111111);
	mSleep(2500);
	setLEDs(0b000000);
	startStopwatch1();
	while(!bumper_left && !bumper_right)
	{
	
		changeDirection(FWD);
		moveAtSpeed(50,50);
		while(!obstacle_left && !obstacle_right)
		{
			task_RP6System();
		}
		
		
		if(obstacle_left)
		{
			setStopwatch1(0);
			while(getStopwatch1() < 2000)
			{
				task_RP6System();
			}
			moveAtSpeed(0,0);
			setLEDs(0b110000);
			while(obstacle_left)
			{
				rotate(50, RIGHT, 10, true);
			}
			setLEDs(0b000000);
		}
		else if(obstacle_right)
		{
			setStopwatch1(0);
			while(getStopwatch1() < 2000)
			{
				task_RP6System();
			}
			moveAtSpeed(0,0);
			setLEDs(0b000110);
			while(obstacle_right)
			{
				rotate(50, LEFT, 10, true);
			}
			setLEDs(0b000000);
		}
	
	}
	moveAtSpeed(0,0);
	return 0;
}

Ps. Die Stopwatch1 verwende ich dafür, dass der Roboter nicht soweit vor dem Hindernis anhält, sondern noch enger ranfährt, um einen engeren Parcour zu durchqueren.

[shedepe]

Dein Code wird je nach Bumperstatus zu beginn gar nicht ausgeführt und dann springt der Controller in den Reset. Ich würde dir dringend raten dich etwas mehr mit der Programmierung in C zu befassen und m besten auch das beim RP6 mitgelieferte C Tutorial mal komplett durchzuarbeiten.