Programmfehler

**Arjen98** · 02.03.2014, 19:11

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:

Code:

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:

Code:

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