Code:
#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 
Zitieren
Lesezeichen