Hey Leute,

ich bin es nochmal und benötige nochmal eure Hilfe.

Ich hab ein Programm für den RP6 geschrieben, indem er erst 20 cm geradeaus fahren soll, anschließend sich um 90° nach Links drehen, dann geradeaus fahren, solange bis beide Bumper gedrückt sind und dann 10 cm Rückwärts fahren, wieder um 90° nach Links fahren und zum Schluss wieder 20 cm geradeaus fahren soll.

Jedoch scheint mein Programm nicht zu funktionieren. Der RP6 tut zwar etwas, aber nicht das was er soll.

Ich hoffe ihr könnt mir ein bisschen auf die Sprünge helfen. Wenn es geht mir auch Verbesserungsvorschläge machen, damit ich es auch lerne.
Ich habe mich zwar an Beispielprogrammen orientiert, aber so richtig klappts noch nicht.

Ich bedanke mich schon einmal im voraus, für eure Hilfe und Anmerkungen.

Code:
#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;

#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 Reaktion_Cruise(void)
{
	
}

#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();
}
	
void task_SERVO(void)
{static uint8_t pos;
	if (getStopwatch1() > PULSE_REPETITION) 
	{	pulseSERVO(pos);			
			pos++;
			setStopwatch1(0);
	}
}
behaviour_command_t Entnehmen = {0, 0, FWD, false, false, 0, Leerlauf};

void Reaktion_Entnehmen(void)
{
	if (bumper_left && bumper_right)
	{
		task_SERVO();
	}
	
	move(70, BWD, DIST_MM(100), true);
	rotate(60, LEFT, 90, true);
	move(70, FWD, DIST_MM(200), true);
	mSleep(20000);
}

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

behaviour_command_t Stop = {0,0,FWD,false,false,0,Leerlauf};
	
	void behaviourController(void)
	{
		Reaktion_Cruise();
		Reaktion_Entnehmen();

		if(Entnehmen.Zustand != Leerlauf) 
		moveCommand(&Entnehmen);
		else if(Cruise.Zustand != Leerlauf) 
		moveCommand(&Cruise);
		else                    
		moveCommand(&Stop); 
	}

int main (void)
{
	initRobotBase();
	board_init();

	mSleep(2500);
	initSERVO();
	
	powerON();
	
	move(70, FWD, DIST_MM(200), true);
	rotate(60, LEFT, 90, true);
	behaviourController();
	
	while(true)
	{
		task_RP6System();
	}
	return 0;

}