Bitte erkärt mir diesen Text
Hi Com,
hier ist das Beispiel, in dem der RP6 in Richtung Licht fährt:
Code:
// Includes:
#include "RP6RobotBaseLib.h"
/*****************************************************************************/
// Behaviour command type:
#define IDLE 0
// The behaviour command data type:
typedef struct {
uint8_t speed_left; // left speed (is used for rotation and
// move distance commands - if these commands are
// active, speed_right is ignored!)
uint8_t speed_right; // right speed
unsigned dir:2; // direction (FWD, BWD, LEFT, RIGHT)
unsigned move:1; // move flag
unsigned rotate:1; // rotate flag
uint16_t move_value; // move value is used for distance and angle values
uint8_t state; // state of the behaviour
} behaviour_command_t;
behaviour_command_t STOP = {0, 0, FWD, false, false, 0, IDLE};
/*****************************************************************************/
// Cruise Behaviour:
#define CRUISE_SPEED_FWD 80 // Default speed
#define MOVE_FORWARDS 1
behaviour_command_t cruise = {CRUISE_SPEED_FWD, CRUISE_SPEED_FWD, FWD,
false, false, 0, MOVE_FORWARDS};
/**
* Cruise Behaviour
*/
void behaviour_cruise(void)
{
}
/*****************************************************************************/
// Escape Behaviour:
#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};
/**
* This is the Escape behaviour for the Bumpers.
*/
void behaviour_escape(void)
{
static uint8_t bump_count = 0;
switch(escape.state)
{
case IDLE:
break;
case ESCAPE_FRONT:
escape.speed_left = ESCAPE_SPEED_BWD;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 200;
else
escape.move_value = 140;
escape.state = ESCAPE_FRONT_WAIT;
bump_count+=2;
break;
case ESCAPE_FRONT_WAIT:
if(!escape.move) // Wait for movement to be completed
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
if(bump_count > 3)
{
escape.move_value = 110;
escape.dir = RIGHT;
bump_count = 0;
}
else
{
escape.dir = LEFT;
escape.move_value = 75;
}
escape.rotate = true;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_LEFT:
escape.speed_left = ESCAPE_SPEED_BWD;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 160;
else
escape.move_value = 100;
escape.state = ESCAPE_LEFT_WAIT;
bump_count++;
break;
case ESCAPE_LEFT_WAIT:
if(!escape.move) // Wait for movement to be completed
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
escape.dir = RIGHT;
escape.rotate = true;
if(bump_count > 3)
{
escape.move_value = 100;
bump_count = 0;
}
else
escape.move_value = 65;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_RIGHT:
escape.speed_left = ESCAPE_SPEED_BWD ;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 160;
else
escape.move_value = 100;
escape.state = ESCAPE_RIGHT_WAIT;
bump_count++;
break;
case ESCAPE_RIGHT_WAIT:
if(!escape.move) // Wait for movement to be completed
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
escape.dir = LEFT;
escape.rotate = true;
if(bump_count > 3)
{
escape.move_value = 100;
bump_count = 0;
}
else
escape.move_value = 65;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_WAIT_END:
if(!(escape.move || escape.rotate)) // Wait for movement/rotation to be completed
escape.state = IDLE;
break;
}
}
/**
* Bumpers Event handler
*/
void bumpersStateChanged(void)
{
if(bumper_left && bumper_right) // Both Bumpers were hit
{
escape.state = ESCAPE_FRONT;
}
else if(bumper_left) // Left Bumper was hit
{
if(escape.state != ESCAPE_FRONT_WAIT)
escape.state = ESCAPE_LEFT;
}
else if(bumper_right) // Right Bumper was hit
{
if(escape.state != ESCAPE_FRONT_WAIT)
escape.state = ESCAPE_RIGHT;
}
}
/*****************************************************************************/
// Avoid Behaviour:
#define AVOID_SPEED_L_ARC_LEFT 20
#define AVOID_SPEED_L_ARC_RIGHT 80
#define AVOID_SPEED_R_ARC_LEFT 80
#define AVOID_SPEED_R_ARC_RIGHT 20
#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};
/**
* Avoid behaviour with ACS IR Sensors.
*/
void behaviour_avoid(void)
{
static uint8_t last_obstacle = LEFT;
static uint8_t obstacle_counter = 0;
switch(avoid.state)
{
case IDLE:
if(obstacle_right && obstacle_left) // left AND right sensor have detected something...
avoid.state = AVOID_OBSTACLE_MIDDLE;
else if(obstacle_left) // Left "sensor" has detected something
avoid.state = AVOID_OBSTACLE_LEFT;
else if(obstacle_right) // Right "sensor" has detected something
avoid.state = AVOID_OBSTACLE_RIGHT;
break;
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;
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;
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 Event Handler
*/
void acsStateChanged(void)
{
if(avoid.state != IDLE)
{
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();
}
}
/*****************************************************************************/
// Follow Behaviour:
#define FOLLOW 3
#define LIGHT_MIN 100
behaviour_command_t follow = {0, 0, FWD, false, false, 0, IDLE};
/**
* The new Light following behaviour.
*/
void behaviour_follow(void)
{
switch(follow.state)
{
case IDLE:
if(adcLSL >= LIGHT_MIN || adcLSR >= LIGHT_MIN) // Is the light bright enough?
{
setStopwatch6(0);
startStopwatch6();
follow.state = FOLLOW;
}
break;
case FOLLOW:
if(getStopwatch6() > 100)
{
if(adcLSL >= LIGHT_MIN || adcLSR >= LIGHT_MIN) // Is the light bright enough?
{
int16_t dif = ((int16_t)(adcLSL - adcLSR))>>1;
if(dif > 40) dif = 40;
if(dif < -40) dif = -40;
follow.speed_left = 60 - dif;
follow.speed_right = 60 + dif;
// Set LEDs - a small bargraph display that shows which sensor has detected
// brighter light - and how much brighter it is:
if(dif > 30)
setLEDs(0b111000);
else if(dif > 25)
setLEDs(0b011000);
else if(dif > 5)
setLEDs(0b001000);
else if(dif > -5)
setLEDs(0b001001);
else if(dif > -25)
setLEDs(0b000001);
else if(dif > -30)
setLEDs(0b000011);
else
setLEDs(0b000111);
}
else // The light is not bright enough - it is very dark!
{
stopStopwatch6();
follow.state = IDLE;
}
if((avoid.state || escape.state)) // Update LEDs with ACS values if there is an
{ // Obstacle...
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();
}
setStopwatch6(0);
}
break;
}
}
/*****************************************************************************/
// Behaviour control:
/**
* This function processes the movement commands that the behaviours generate.
* Depending on the values in the behaviour_command_t struct, it sets motor
* speed, moves a given distance or rotates.
*/
void moveCommand(behaviour_command_t * cmd)
{
if(cmd->move_value > 0) // move or rotate?
{
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; // clear move value - the move commands are only
// given once and then runs in background.
}
else if(!(cmd->move || cmd->rotate)) // just move forwards?
{
changeDirection(cmd->dir);
moveAtSpeed(cmd->speed_left,cmd->speed_right);
}
else if(isMovementComplete()) // movement complete? --> clear flags!
{
cmd->rotate = false;
cmd->move = false;
}
}
/**
* The behaviourController task controls the subsumption architechture.
* It implements the priority levels of the different behaviours.
*/
void behaviourController(void)
{
// Call all the behaviour tasks:
behaviour_cruise();
behaviour_follow();
behaviour_avoid();
behaviour_escape();
// Execute the commands depending on priority levels:
if(escape.state != IDLE) // Highest priority - 4
moveCommand(&escape);
else if(avoid.state != IDLE) // Priority - 3
moveCommand(&avoid);
else if(follow.state != IDLE) // Priority - 2
moveCommand(&follow);
else if(cruise.state != IDLE) // Priority - 1
moveCommand(&cruise);
else // Lowest priority - 0
moveCommand(&STOP); // Default command - do nothing!
// In the current implementation this never
// happens.
}
/*****************************************************************************/
// Main:
int main(void)
{
initRobotBase();
setLEDs(0b111111);
mSleep(2500);
setLEDs(0b001001);
// Set Bumpers state changed event handler:
BUMPERS_setStateChangedHandler(bumpersStateChanged);
// Set ACS state changed event handler:
ACS_setStateChangedHandler(acsStateChanged);
powerON(); // Turn on Encoders, Current sensing, ACS and Power LED.
setACSPwrMed();
// Main loop
while(true)
{
behaviourController();
task_RP6System();
}
return 0;
}
Mein Englisch ist nicht perfekt, deshalb verstehe ich einige Sachen nicht.
Wäre einfach toll, wenn mir jemand der Reihe nach sagt, wozu man was schreiben muss bzw. wofür das ist.
Und erklärt mir bitte auch mal jemand wie man diese Kästen für den Programmiertext hier macht.
Edit von radbruch: Die Kästen macht man mit (code) ... (/code), die runden Klammern () aber mit Eckigen [] ersetzen.
