RP6 Servo-Programm

[Arjen98]

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.

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

}

[Dirk]

Hi,
das sind nur zwei teilweise zusammen kopierte Einzelprogramme.
Dein Ziel, bestimmte Bewegungen mit dem RP6 auszuführen, kannst du z.B. durch Änderung des RP6Base_Move_02 Beispiels schaffen.
Die Funktionen zur Servo-Ansteuerung brauchst du dazu nicht.

[Arjen98]

Erstmal Danke für die Antwort.
Doch eine Frage habe ich noch. Es ist doch möglich, mithilfe einer Schleife, dass der Robby immer geradeaus fährt, bis beide Bumper angeschlagen sind oder?
Dies hab ich schon versucht, doch irgendwie bereitet diese Stelle im Programm dem Robby Schwierigkeiten, da Frage ich mich, wo ich den Denkfehler mache, oder ob man es überhaupt so schreiben kann.
Der Roboter führt die ersten Fahrmanöver aus, und dann überspringt er den Schleifenpart. Ich habe daran schon lange rumprobiert, und im Internet fand ich auch nichts.
Also das Programm lautet so:

#include "RP6RobotBaseLib.h"

int main (void)
{
	initRobotBase();

	mSleep(2500);
	
	powerON();
	
	move(70, FWD, DIST_MM(200), true);
	
	rotate(60, LEFT, 90, true);
	
	task_Bumpers();
	
	while(bumper_left && bumper_right == false)
	{
		move(70,FWD, DIST_MM(10), true);
	}
	
	move(70,BWD, DIST_MM(100), true);
	
	while(true)
	{
		task_RP6System();
	}
	return 0;

}

Was muss ich anders machen?

Danke im Voraus.

[Dirk]

1. Du must das ganze Fahren und Stoppen bei einem Hindernis in die Hauptschleife:
while(true)
{
task_RP6System();
}
... packen.

2. Die Abfrage while(bumper_left && bumper_right == false) macht nicht das, was du willst.
So sollte es gehen: while(!bumper_left && !bumper_right).

3. Wenn du ein Hindernis gemerkt hast (mit einem Bumper), must du den RP6 auch noch Stoppen.

[Arjen98]

Vielen vielen Dank Dirk, es hat funktioniert. Danke das du einem so schnell bei seinen Problemen hilfst.

ich hab die while Schleife jetzt so geschrieben:

while(!bumper_left && !bumper_right != false)

und es funktioniert gut jetzt muss ich noch den Servo einbringen und hab die Funktion fertig Danke

[Dirk]

Hi,

while(!bumper_left && !bumper_right != false)

Erklärung zu deinem Code:
Deine while-Schleife läuft, solange ...
- der linke Bumper nicht gedrückt ist (!bumper_left) UND (&&) ...
- der Wert für den nicht gedrückten rechten Bumper (!bumper_right) UNGLEICH (!=) false
... ist.
Der letzte Teil (!bumper_right != false) ist identisch mit: (bumper_right == false) wegen der doppelten Verneinung und das ist identisch mit: (!bumper_right).

Also: Dein Code macht zwar alles richtig, ist aber so nicht nötig,- das: while(!bumper_left && !bumper_right) reicht.

Übrigens:
(!bumper_left && !bumper_right) kann man auch so schreiben: (bumper_left == false && bumper_right == false)

Verwirrung komplett?