RP6 Von Anfänger für Anfänger

[littles]

Hi!
Seit gestern bin ich ein glücklicher Besitzer eines RP6. Da ich die Akkuladezeit irgendwie überbrücken musste dachte ich mir ein kleines Tutorial zu schreiben. In dem tutorial lernt ihr wie der Robby vorwärts fährt und bei Berührung der Tastsensoren (Bumper) anhält, zurücksetzt und sich um 45 grad dreht. Dann nimmt er die Fahrt vorwärts wieder auf. Wenn man die Anleitung gelesen hat und etwas Verständnis für C hat ist das total leicht nachzuvollziehen. Wer sich mit Programmiersprachen gar nicht auskennt sollte vor allem das Handbuch aufmerksam lesen. Viele fragen werden dort sehr gut erläutert. Aber auch mir könnt ihr gerne fragen dazu stellen. Solltet ihr noch fragen dazu haben steht auch meine e-mail adresse mit im tutorial.
Und jetzt viel Spass und Erfolg ;)

[littles]

So, hab mal versucht das Beispiel 5Move5 mit Deutschen kommentaren auszustatten welche leicht verständlich vermitteln sollen was da passiert. Ich hoffe habe da keine Denkfehler mit eingebaut... Ist natürlich noch nicht komplett kommentiert, es sollte aber eigentlich mt etwas nachdenken alles beantwortet werden. Daher Anregungen, Fragen usw.
Hier also der Code mit meinen Kommentaren:

#include "RP6RobotBaseLib.h"
// Leerlauf
#define IDLE 0
//Struktur behaviour_command_t wir an die Funktion move() bzw. rotate() übergeben.
//Alle benötigten werte werden hier abgelegt.
typedef struct {
uint8_t speed_left;
uint8_t speed_right;
unsigned dir:2;
unsigned move:1;
unsigned rotate:1;
uint16_t move_value;
uint8_t state;
} behaviour_command_t;
//wie eine variable(z.B.:uint8_t STOP = 0;) können wir nun die Struktur STOP mit diesen werten erstellen.
//wird dann Stop in der funktion move(STOP) oder rotate(STOP) aufgerufen passiert folgendes :
//In STOP stehen diese Werte:Stop = speed_left, speed_right, dir, move, rotate, move_value, state;
//Diese werte werden an die funktion übergeben und beeinflussen so das fahrverhalten (Wie der name vermuten lässt stoppt
//der Roboter dann ;-)). über den punktoperator können einzelne werte der Struktur abgefragt bzw. geändert werden.
//z.B. Wert ändern: STOP.dir = BWD; (d.h.: Stop.Fahrtrichtung = vorwärts)
//oder abfragen und dann ändern: if(Stop.dir == FWD) [d.h.: wenn Stop.Fahrtrichtung vorwärts ist]
// { Stop.dir = BWD; } [ Stop.Fahrtrichtung = Rückwärts]
//Das Beispiel macht sogar sinn auch wenn es anfangs verwirrend sein kann weshalb die fahrtrichtung beim stop geändert wird...
behaviour_command_t STOP = {0, 0, FWD, false, false, 0, IDLE};

//überall wo CRUISE_SPEED_FWD steht wird der wert ( hier im moment 80) eingetragen. So muss man sich nicht jeden wert merken.
//Einfache aber Sinnvolle namen nehmen (#define CRUISE_SPEED_FWD = #define FAHREN_GESCHWINDIGKEIT_VORWÄRTS)
#define CRUISE_SPEED_FWD 80

#define MOVE_FORWARDS 1
behaviour_command_t cruise = {CRUISE_SPEED_FWD, CRUISE_SPEED_FWD, FWD,
false, false, 0, MOVE_FORWARDS};
//Da wir kein muster abfahren wollen, sondern nur geradeaus bis wir auf ein hinderniss stossen brauchen wir
// in dieser funktion keine einträge. alle wert stehen ja dafür schon in der struktur cruise...
void behaviour_cruise(void)
{
}
//Taster : Standartwerte die über den namen angesprochen werden können. siehe #define oben...
#define ESCAPE_SPEED_BWD 80
#define ESCAPE_SPEED_ROTATE 60

#define ESCAPE_FRONT 1
#define ESCAPE_FRONT_WAIT 2
#define ESCAPE_LEFT 3
#define ESCAPE_LEFT_WAIT 4
#define ESCAPE_RIGHT 5
#define ESCAPE_RIGHT_WAIT 6
#define ESCAPE_WAIT_END 7
behaviour_command_t escape = {0, 0, FWD, false, false, 0, IDLE};
//Bumper Funktion
void behaviour_escape(void)
{ //Variable zum zählen wie oft die taster ausgelöst wurden
static uint8_t bump_count = 0;
//Schalte die Fahreinstellung um, je nachdem welcher Taster getroffen wurde
switch(escape.state)
{
//IDLE ist soviel wie Leer(-lauf)
case IDLE:
break;
//Beide Taster ausgelöst bzw. Hinderniss mittig
case ESCAPE_FRONT:
//einzelne werte der struktur escape ändern um auf das tastereignis zu reagieren
//zurück fahren und ESCAPE_FRONT_WAIT aufrufen
escape.speed_left = ESCAPE_SPEED_BWD;
escape.dir = BWD;
escape.move = true;
//if = wenn...
if(bump_count > 3)
// dann...
escape.move_value = 220;
//else = wenn nicht, dann. Oder auch :sonst...
else
escape.move_value = 160;
escape.state = ESCAPE_FRONT_WAIT;
bump_count+=2;
break;

case ESCAPE_FRONT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
if(bump_count > 3)
{
escape.move_value = 100;
escape.dir = RIGHT;
bump_count = 0;
}
else
{
escape.dir = LEFT;
escape.move_value = 70;
}
escape.rotate = true;
escape.state = ESCAPE_WAIT_END;
}
break;
//Links ausweichen
case ESCAPE_LEFT:
escape.speed_left = ESCAPE_SPEED_BWD;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 190;
else
escape.move_value = 150;
escape.state = ESCAPE_LEFT_WAIT;
bump_count++;
break;
case ESCAPE_LEFT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
escape.dir = RIGHT;
escape.rotate = true;
if(bump_count > 3)
{
escape.move_value = 110;
bump_count = 0;
}
else
escape.move_value = 80;
escape.state = ESCAPE_WAIT_END;
}
break;
//Rechts ausweichen
case ESCAPE_RIGHT:
escape.speed_left = ESCAPE_SPEED_BWD ;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 190;
else
escape.move_value = 150;
escape.state = ESCAPE_RIGHT_WAIT;
bump_count++;
break;
case ESCAPE_RIGHT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
escape.dir = LEFT;
escape.rotate = true;
if(bump_count > 3)
{
escape.move_value = 110;
bump_count = 0;
}
else
escape.move_value = 80;
escape.state = ESCAPE_WAIT_END;
}
break;
//Vorgang beenden
case ESCAPE_WAIT_END:
if(!(escape.move || escape.rotate))
escape.state = IDLE;
break;
}
}
//Bumper Handler
void bumpersStateChanged(void)
{ //beide bumper getroffen
if(bumper_left && bumper_right)
{
escape.state = ESCAPE_FRONT;
}
//linker bumper getroffen
else if(bumper_left)
{
if(escape.state != ESCAPE_FRONT_WAIT)
escape.state = ESCAPE_LEFT;
}
//rechter bumper getroffen
else if(bumper_right)
{
if(escape.state != ESCAPE_FRONT_WAIT)
escape.state = ESCAPE_RIGHT;
}
}


#define AVOID_SPEED_L_ARC_LEFT 30
#define AVOID_SPEED_L_ARC_RIGHT 90
#define AVOID_SPEED_R_ARC_LEFT 90
#define AVOID_SPEED_R_ARC_RIGHT 30
#define AVOID_SPEED_ROTATE 60

#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};
//Anti Kollisions System (IR-Sensoren) Funktion
void behaviour_avoid(void)
{ //letztes hinderniss
static uint8_t last_obstacle = LEFT;
//variable um hindernisse zu zählen
static uint8_t obstacle_counter = 0;
switch(avoid.state)
{
//Idle prüft auf Hindernisse und schaltet zwischen fahrmodis
case IDLE:
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
else if(obstacle_left)
avoid.state = AVOID_OBSTACLE_LEFT;
else if(obstacle_right)
avoid.state = AVOID_OBSTACLE_RIGHT;
break;
//Hinderniss mittig
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))
{
if(obstacle_counter > 3)
{
obstacle_counter = 0;
setStopwatch4(0);
}
else
setStopwatch4(400);
startStopwatch4();
avoid.state = AVOID_END;
}
break;
//Hinderniss Rechts
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;
//Hinderniss Links
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)
{
stopStopwatch4();
setStopwatch4(0);
avoid.state = IDLE;
}
break;
}
}
//ACS Handler (schaltet nur LEDs um)
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();
}
//Funktion erstellt move und rotate befehle mit den übergebenen werten der Struktur behaviour_command_t
//also Wenn ein ereigniss eintritt(Bumper, Ir- Sensoren usw.)
void moveCommand(behaviour_command_t * cmd)
{

if(cmd->move_value > 0)
{
//Überprüfe ob rotate oder move aufgerufen wurde und erstelle den dementsprechenden Fahrbefehl
if(cmd->rotate)
rotate(cmd->speed_left, cmd->dir, cmd->move_value, false);
else if(cmd->move)
move(cmd->speed_left, cmd->dir, DIST_MM(cmd->move_value), false);
cmd->move_value = 0;
}
//weder move noch rotate wurde aufgerufen
else if(!(cmd->move || cmd->rotate))
{
changeDirection(cmd->dir);
moveAtSpeed(cmd->speed_left,cmd->speed_right);
}
//Wenn die bewegung abgeschlossen ist rotate und move auf false setzen um beim nächsten aufruf keine alten werte
//zu nutzen.
else if(isMovementComplete())
{
cmd->rotate = false;
cmd->move = false;
}
}
//Funktion wartet auf Sensorereignisse und ruft die Funktion moveCommand(behaviour_command_t * cmd)
//auf, und übergibt an die Funktion angepasste Fahrbefehle um Hindernissen auszuweichen
void behaviourController(void)
{
// Die funktionen abfragen und bei änderung in cruise.state, avoid.state oder escape.state darauf reagieren
behaviour_cruise();
behaviour_avoid();
behaviour_escape();
//wenn escape.state, avoid.state ,oder cruise.state nicht IDLE sind rufe funktion moveCommand()auf und verweise auf die
//Struktur in der die werte abgespeichert sind um das Fahrverhalten zu ändern.
if(escape.state != IDLE)
moveCommand(&escape);
else if(avoid.state != IDLE)
moveCommand(&avoid);
else if(cruise.state != IDLE)
moveCommand(&cruise);
else
//Wenn alle drei IDLE sind anhalten
moveCommand(&STOP);
}


int main(void)
{
initRobotBase();
setLEDs(0b111111);
mSleep(2500);
setLEDs(0b100100);
//Handler für Taster und Anti Kollisions System
BUMPERS_setStateChangedHandler(bumpersStateChanged );
ACS_setStateChangedHandler(acsStateChanged);

powerON();
setACSPwrMed();
//Endlosschleife
while(true)
{
//Diese Funktionen werden ständigaufgerufen. Bei einer änderung in der Funktion (Wenn etwa eine If bedingung erfüllt wurde
//und ein wert sich somit ändert) laufen dann je nachdem was sich ereignet hat verschiedene funktionen. Die bauen sozusagen
//aufeinander auf(Funktion ruft Funktion auf ruft Funktion auf usw.) Dabei werden Zustände verglichen und vorgegebene Bewegungs-
//befehle erstellt. wenn alle werte bestimmt sind für die Fahrtänderung wird eine Funktion aufgerufen die die Werte in einen
//Befehl umwandelt (hier move() oder rotate()). Der wird dann ausgeführt.Abschließend wechselt er wieder in den normalen
//Fahrmodus. Dann wird wieder kontrolliert, ggf. Funktionen aufgerufen usw...
behaviourController();
task_RP6System();
}
return 0;
}

Bitte verzeiht mir Tippfehler und Groß-/Kleinschreibung aber stehe gerade etwas neben mir. Naja ist ja auch schon spät :)

[TrainMen]

Das hätte ich mir vor 2 Jahren gewünscht. Die Beispielprogramme mit konkreten Erklärung was jetzt passiert und das dann in Deutsch. Ich find das eine gute Idee. Mach weiter so.
MfG TrainMen

[littles]

Danke. Ich wollte erst mal sehen ob da Interesse dran besteht. Werde mich mal die nächsten 2, 3 Wochen mit den Beispielen auseinandersetzen und versuchen sie zu übersetzen. Natürlich bin ich auch nur ein anfänger und deshalb hoffe ich dann bei der Fehlerkorrektur auf euch. Ich will ja auch was lernen.

[SlyD]

> hoffe ich dann bei der Fehlerkorrektur auf euch


OK ;)
Kleiner Hinweis zu den EventHandler Funktionen:
Dort besser nichts blockierendes / Pausen verwenden. Das kann in einigen Fällen funktionieren, muss aber nicht.
Teste einfach mal was passiert wenn Du die Bumper ein paar mal direkt hintereinander drückst wenn Du das so wie bei Dir beschrieben machst mit move(... ,BLOCKING)... ;)
(müsste rekursiv mehrmals hintereinander aufgerufen werden... wenn ich mich recht erinnere - schon lange her)

Habs nur überflogen aber das war mir sofort aufgefallen.

[littles]

Jo, danke für den Tipp. Hatte beim überfliegen von BLOCKING wohl etwas verdreht (um genau zu sein habe ich es komplett andersrum verstanden, hab eben nochmal die Anleitung gelesen. Hätte schwören können das es so dastand wie ich es in Erinnerung hatte) . Na gut war auch aufgeregt wegen dem neuen RP6. Werde dann mal wenn ich wieder zu Hause bin nachbessern. Poste dann den neuen Code mit Erklärung heute abend oder morgen früh. Achso wollte noch was zu rekursiv sagen (SlyD hat den begriff ja im Post genutzt). Eine Rekursion ist u.a. eine Funktion die sich selbst aufruft. Hab irgendwo in C++ noch nen kleines Programm mit ner Doppelt verketteten Liste. Werde das mal umtippen und dann die tage hier posten. Das sollte recht einfach veranschaulichen wie das mit der rekursion funktioniert.

[littles]

Da ich gerade anfange Unterlagen für ein Projekt mit ehemaligen Drogenabhängigen zu erstellen um denen einen Einstieg in Elektronik und Programmierung am beispiel eines Autonomen Roboters zu erläutern (geplant für nächstes Jahr, falls ich ein Konzept zusammenbekomme), habe ich heute mal Die Umrechnung von dezimal in binär und zurück beschrieben. Natürlich nicht um so ständig Werte umzurechnen, sondern um den Zusammenhang etwas besser zu verdeutlichen. Ich dachte erst das wäre in 3, 4 Zeilen zu erklären. Wurde aber von meinen "Beta-Testern" eines besseren belehrt(beide wissen wie man einen PC ein und aus schaltet, aber nicht viel mehr). Am Ende waren es dann viele Zeilen die ich jetzt etwas gekürzt habe. Viel spass beim Lesen

[TrainMen]

mach aber nicht den Fehler bei Deinem Projekt, solche Dinge wie diese Hex/Dez Umwandlungen zu früh zu bringen. Die meisten werden sich langweilen, sich wie in der Schule vorkommen oder Dich darauf hinweisen das es Taschenrechner gibt die diese Funktionen haben. Ich wünsch Dir viel Erfolg mit Deinem Konzept.

[littles]

Danke für den Hinweis. Genau das hatte ich mir auch schon gedacht. Daher habe ich mir vorgenommen daraus ein klein Heft mit solchen "Schulischen langweiligen Kram" zu machen. So hätte jeder ein eigenes kleines Nachschlagewerk und das auch noch umsonst. Da ja gerade bei Sozialen Randgruppen das finanzielle eine große Rolle spielt könnte man damit schon mal arbeiten, falls man sich eben keine Fachliteratur leisten kann. Und da noch die Finanzierung in der Schwebe ist, (Kirchen und Drogenberatungsstellen schmeißen leider nicht mit Geld um sich ) versuche ich den Kurs so günstig wie möglich zu halten. Naja, mein utopisches Ziel wäre es einen wettbewerbsfähigen Roboter von der Gruppe bauen und programmieren zu lassen. Wenn die Gruppe dann auch noch an einem Wettbewerb (Robo Cup, Sumo Roboter oder sowas) teilnehmen könnte wäre ich extremst begeistert. Aber das ist noch geträume und weit weg.
So, muss mal arbeiten.