Code:
Und hier mein neues Servogezappel, das noch sehr unausgereift wirkt:
/*****************************************************************************/
// 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 60 // Default speed when no obstacles are detected!
#define MOVE_FORWARDS 1
behaviour_command_t cruise = {CRUISE_SPEED_FWD, CRUISE_SPEED_FWD, FWD,
false, false, 0, MOVE_FORWARDS};
/**
* We don't have anything to do here - this behaviour has only
* a constant value for moving forwards - s. above!
* Of course you can change this and add some random or timed movements
* in here...
*/
void behaviour_cruise(void)
{
}
/*****************************************************************************/
// Escape Behaviour:
#define ESCAPE_SPEED_BWD 80
#define ESCAPE_SPEED_ROTATE 50
#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 = 220;
else
escape.move_value = 160;
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 = 100;
escape.dir = RIGHT;
bump_count = 0;
}
else
{
escape.dir = LEFT;
escape.move_value = 70;
}
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 = 190;
else
escape.move_value = 150;
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 = 110;
bump_count = 0;
}
else
escape.move_value = 80;
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 = 190;
else
escape.move_value = 150;
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 = 110;
bump_count = 0;
}
else
escape.move_value = 80;
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;
}
}
/*****************************************************************************/
// The new Avoid Behaviour:
// Some speed values for different movements:
#define AVOID_SPEED_L_ARC_LEFT 20
#define AVOID_SPEED_L_ARC_RIGHT 65
#define AVOID_SPEED_R_ARC_LEFT 65
#define AVOID_SPEED_R_ARC_RIGHT 20
#define AVOID_SPEED_ROTATE 50
// States for the Avoid FSM:
#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};
/**
* The new avoid behaviour. It uses the two ACS channels to avoid
* collisions with obstacles. It drives arcs or rotates depending
* on the sensor states and also behaves different after some
* detecting cycles to avoid lock up situations.
*/
void behaviour_avoid(void)
{
static uint8_t last_obstacle = LEFT;
static uint8_t obstacle_counter = 0;
switch(avoid.state)
{
case IDLE:
// This is different to the escape Behaviour where
// we used the Event Handler to detect sensor changes...
// Here we do this within the states!
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(600);
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(700);
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(700);
startStopwatch4();
avoid.state = AVOID_END;
}
last_obstacle = LEFT;
obstacle_counter++;
break;
case AVOID_END:
if(getStopwatch4() > 1000) // We used timing based movement here!
{
stopStopwatch4();
setStopwatch4(0);
avoid.state = IDLE;
}
break;
}
}
/**
* ACS Event Handler - ONLY used for LED display!
* This does not affect the behaviour at all!
* The sensor checks are completely done in the Avoid behaviour
* statemachine.
*/
/*****************************************************************************/
// 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 at speed?
{
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_avoid();
behaviour_escape();
// Execute the commands depending on priority levels:
if(escape.state != IDLE) // Highest priority - 3
moveCommand(&escape);
else if(avoid.state != IDLE) // Priority - 2
moveCommand(&avoid);
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.
}
void akku(void)
{
//IF BATT >9V
/* Das alte
if(adcBat > 900)
setLEDs(0b001001);
else if(adcBat < 901 && adcBat > 800)
{
statusLEDs.LED4 = !statusLEDs.LED4;
statusLEDs.LED1 = !statusLEDs.LED1;
updateStatusLEDs();
}
else if(adcBat < 801 && adcBat > 700)
setLEDs(0b000001);
else if(adcBat < 701 && adcBat > 590)
{
statusLEDs.LED1 = !statusLEDs.LED1;
updateStatusLEDs();
}
else if(adcBat < 591 && adcBat > 500)
setLEDs(0b000010);
else if(adcBat < 591 && adcBat > 580)
{ mSleep (5000);
if (adcBat > 591)
{ powerOFF();
mSleep(5000);
powerON();
setACSPwrHigh();
}
else if (adcBat <= 591)
&STOP;
}
*/
if(adcBat > 800)
setLEDs(0b001001);
else if(adcBat < 801 && adcBat > 750)
{
statusLEDs.LED4 = !statusLEDs.LED4;
statusLEDs.LED1 = !statusLEDs.LED1;
updateStatusLEDs();
}
else if(adcBat < 751 && adcBat > 700)
setLEDs(0b000001);
else if(adcBat < 701 && adcBat > 680)
{
statusLEDs.LED1 = !statusLEDs.LED1;
updateStatusLEDs();
}
else if(adcBat < 681 && adcBat > 620)
setLEDs(0b000010);
//könnte ich hier irgendwann eine Variable fahrfunktion
//(langsermer) einrichten...mit angeschlossener
//Ir-empfänger für Palm quelle "suchfunktion"?
else if(adcBat < 621 && adcBat > 601)
{ mSleep (5000) ;
if (adcBat > 621)
{
powerOFF();
mSleep(1000);
powerON();
setACSPwrMed();
}
else if (adcBat < 621)
moveAtSpeed(0,0);
}
else if(adcBat < 601 && adcBat > 590)
{ mSleep (5000) ; //5sek.
if (adcBat > 609)
{
powerOFF();
mSleep(1000);
powerON();
setACSPwrHigh();
}
else if (adcBat <= 609)
moveAtSpeed(0,0);
}
}
/*
{
setLEDs(0b001001);
uint8_t i, pause, servo_stellzeit, servo_delay;
void servo(uint8_t w0, uint8_t w1, uint8_t w2)
{
unsigned int count=0;
do{
PORTA |= E_INT1; // E_INT1 (Pin8)
sleep(w0);
PORTA &= ~E_INT1;
PORTC |= 1; // SCL (Pin10)
sleep(w1);
PORTC &= ~1;
PORTC |= 2; // SDA (Pin12)
sleep(w2);
PORTC &= ~2;
sleep(servo_delay-(w0+w1+w2));
//sleep(127);
} while (count++ < servo_stellzeit);
mSleep(10*pause);
}
i=0;
servo_stellzeit=8;
DDRA |= E_INT1; // E_INT1 als Ausgang
DDRC |= 3; // SCL und SDA als Ausgang
// 5 - 15 - 25 // Min - Mitte - Max
servo(15,15,15); // Grundstellung
while (1)
{
switch (i)
{
case 0: i++; servo_delay= 220; break;
}
servo(10,14,10);
servo(10,13,10);
servo(10,12,10);
servo(10,11,10);
servo(10,10,10);
servo(10,9,10);
servo(10,8,10);
servo(10,7,10);
servo(10,6,10);
servo(10,7,10);
servo(10,8,10);
servo(10,9,10);
servo(10,10,10);
servo(10,11,10);
servo(10,12,10);
servo(10,13,10);
servo(10,14,10);
servo(10,15,10);
servo(10,16,10);
servo(10,17,10);
servo(10,18,10);
servo(10,19,10);
servo(10,20,10);
servo(10,21,10);
servo(10,22,10);
servo(10,23,10);
servo(10,24,10);
servo(10,23,10);
servo(10,22,10);
servo(10,21,10);
servo(10,20,10);
servo(10,19,10);
servo(10,18,10);
servo(10,17,10);
servo(10,16,10);
servo(10,15,10);
powerOFF();
mSleep(1000);
powerON();
}
}
}
*/
//Radar
#define pos1 450
#define pos2 1420
extern uint8_t acs_state;
uint16_t servo_pos, servo_timer, servo_dummy;
void radar (void)
{
DDRC |= 1;
startStopwatch1();
startStopwatch2();
servo_pos = pos1;
{
if ((getStopwatch1() > 20) && (acs_state < 2)) // alle 20ms einen Servoimpuls erzeugen
{
setStopwatch1(0);
servo_timer = servo_pos;
PORTC|=1;
while(servo_timer--) servo_dummy ^= servo_timer;
PORTC&=~1;
}
if (getStopwatch2() > 1000) // alle 1000ms Servo auf andere Seite fahren
{
if (servo_pos == pos1) servo_pos=pos2; else servo_pos=pos1;
setStopwatch2(0);
}
}
}
/*****************************************************************************/
// Main:
int main(void)
{
initRobotBase();
// Set Bumpers state changed event handler:
BUMPERS_setStateChangedHandler(bumpersStateChanged);
powerON(); // Turn on Encoders, Current sensing, ACS and Power LED.
setACSPwrMed();
// Main loop
while(true)
{
//task_RP6System();
radar();
akku();
task_ADC();
task_ACS();
task_Bumpers();
task_motionControl();
radar();
behaviourController();
}
return 0;
}
Zitieren
Lesezeichen