ESERCITAZIONE

Implementazione di un sistema reattivo per la navigazione sulla base

robotica Pioneer I

Il sistema robotico Pioneer IReal Word Interface, USA

Base robotica mobile a 3 ruote dotata di 7 sensori ad ultrasuoni (due laterali e 5 frontali intervallati di 15 gradi) e di due motori dotati di encoder per le ruote anteriori

Velocità max: 90 cm/s 300°/s

Payload: 4.5 Kg Sensori: 10cm-5m Controllore: MC68HC11 Peso: 7.7 Kg

Architettura del sistema

Dati sensoriDati sensori

ComandiComandi ControllorControllore di basso e di basso

livellolivello

SensoriSensori(US, Encoder)(US, Encoder)

MotoriMotoriInterfaccia Interfaccia

UtenteUtenteSoftware di alto Software di alto

livellolivelloAmbiente di Ambiente di

svilupposviluppo

RSRS 232 o connessione 232 o connessione radioradio

Architettura del sistema

Cavo Seriale

SaphiraColbert Language and Evaluator

Software di acquisizione e elaborazione

RWI-Pioneer I

Saphira: l’interfaccia per il controllo del sistema

RobotRobot

Distanza Distanza rilevata dai rilevata dai sensori USsensori US

Comandi in Comandi in Linguaggio Colbert Linguaggio Colbert

e messaggi di e messaggi di sistemasistema

StatoStato

Connessione Connessione con il robotcon il robot

Saphira: il simulatore

SimulatoreSimulatore

Il sistema di coordinate di riferimento

I comandi da tastiera

I comandi

Il linguaggio Colbert Colbert is a C-like language with a

semantics based on finite state machines. It has the standard C constructs such as sequences, iteration, and conditionals, interpreted in a way that makes sense for robot programming.

The main construct of Colbert is the activity schema or act, a procedure that describes a set of coordinated actions that the robot should perform.

Il linguaggio Colbert Colbert is an interpreted language,

which means that you write text files containing Colbert activities, load them into a Saphira client, and then start them up. The Colbert evaluator parses and executes the activities, and reports back results and errors.

Il linguaggio Colbert

The Colbert evaluator has the following capabilities Direct execution of control statements from a running Saphira

client. Tracing of activities: users can see exactly what statements are

being executed. Signaling between activities: activities may start sub-activities, or

interrupt already running activities. Trapping of errors: fatal errors, such as divide by 0, just disable

the offending activity and print an error message. Error correction: buggy activities can be edited with a text editor

and reloaded, without exiting the running Saphira client.

Il linguaggio Colbert: esempio/* Colbert example exercising the direct motion calls */

act patrol(int a) /* go back and forth 'a' times */{

while (a != 0){a = a-1;move(1000);turnto(0);move(1000);turnto(180);}

}

Il linguaggio Colbert: esempio/* Colbert example exercising the direct motion calls */

act square /* move in a square */{

int a;a = 4;while(a){a = a-1;move(1000);urn(90);}

}

Il linguaggio Colbert: scrittura ed esecuzione del codice

Colbert source files may be input from text files, using the load command

Load files can contain definitions of activities, as well as commands to be executed, including any commands that can be typed in the user interaction area. So, for example, it’s possible to load a file that loads other files

Istruzione: load <file> loads file from the current load directory

Colbert source files can have an arbitrary extension (except for .so or .dll), but by convention their extension is .act

Il linguaggio Colbert:Direct Motion Commands

The evaluator provides a set of direct motion commands that can move and rotate the robot.

The direct motion commands are not C functions, and do not return any value. They also have a syntax

for specifying a timeout value and a non-blocking mode. The general form of a direct motion command is:

command(int arg) [timeout n] [noblock]; where timeout n specifies a time in 100 ms increments for the

command to complete, and noblock means that the command will be executed without blocking,

i.e., control will continue with the next statement. Some motion commands are implicitly nonblocking:

speed and rotate.

Il linguaggio Colbert:Direct Motion Commands move(int mm); turn(int deg); turnto(int deg); speed(int mms); rotate(int degs); halt;

Il linguaggio Colbert:Activity Schemas

act <aname> [([<type> <param>]*)]

{

[<type> <local-var>]*

[update <stmt>]

<stmts>

}

Il linguaggio Colbert:tipi e variabili

C ColbertDichiarazioni

int sfINT int a;

float sfFLOAT ptr tonowhere;

void sfVOID float **f;

string sfSTRING robot *r;

act sfACTIVITY

void * sfPTR

Il linguaggio ColbertConditinal Statement:

if (<c_exp>) <stmts> [ else <stmts> ]

waitfor <c_exp> [timeout <n>];

Iteration and Branching Statements

while (<c_exp>) <stmts>

<label>:

goto <label>;

Il linguaggio Colbert: Assignment Statements

point p; b = &a;

int a; *b = 3;

int *b; a = 2;

float *c; p.x = 1.0;

*c = *b; c = &p.x;

Saphira API: State Reflection

The state reflector is a set of data structures in the client that reflects the

sensor and motor state of the robot.

Saphira API: Robot Statestruct robot sfRobot

Saphira API: Sonar bucketsThe current range reading of a sonar sensors is held in an sdata structure, defined below.

sdata *sfSonarBucket(int num)int sfSonarRange(int num)float sfSonarXCoord(int num)float sfSonarYCoord(int num)

The first function returns a pointer to the data structure of the num’th sonar, or NULL if there is no such sonar.The next three functions return the range and x,y coordinates of the sonar reading.

Saphira API: Direct Motion Control

void sfSetVelocity(int vel)void sfSetRVelocity(int rvel)Set the translational and rotational setpoints in the state reflector. Values for translational velocity are in mm/sec; for rotational velocity, degrees/sec.

void sfSetHeading(int head)void sfSetDHeading(int dhead)The first function sets the absolute heading setpoint in the state reflector. The argument is in degrees, from 0 to 359.The second function increments or decrements the heading setpoint. The argument is in degrees, from –180 to +180.

Saphira API: Direct Motion Controlvoid sfSetPosition(int dist)void sfSetMaxVelocity(int vel)The first function sets the distance setpoint in the state reflector. Argument is in mm, either positive (forwards) or negative (backwards). The maximum velocity attained during motion is given by sfSetMaxVelocity, in mm/sec.

int sfDonePosition(int dist)int sfDoneHeading(int ang)Checks whether a previously-issued direct motion command has completed. The argument indicates how close the robot has to get to the commanded position or heading before it is considered completed. Arguments are in mm for position, and degrees for heading.

Saphira API: Direct Motion Control

float sfTargetVel(void)

float sfTargetHead(void)

These functions return the current reflected values for the velocity and heading setpoints, respectively. Values are in mm/sec and degrees.

Implementazione della architettura subsumption di Brooks per il Pioneer nel linguaggio Colbert

Il livello 2

Sonar

void sonar(double *s){int i;for (i=0;i<7;i++)

s[i]= (double) sfSonarRange(i);}

FeelForce

void feelforce(double *s, double *direction, double *magnitude){

int i,imin;

int a[7] = {90, 30, 15, 0, -15, -30, -90};

imin=0;

for (i=1;i<7;i++)

if (s[i] < s[imin]) imin = i;

*direction = a[imin];

*magnitude = s[imin];

}

Runawayvoid runaway(double direction, double magnitude, double

*angle, double *distance){if (magnitude <= SAFE_DIST){

*distance = -(SAFE_DIST - magnitude);*angle=direction;

}else{

*distance = 0;*angle = 0;

}}

Turn

void turn(double angle){

sfSetDHeading((int) angle);

}

Forward

void forward(double distance){

sfSetPosition((int) distance);}

Livello_zerovoid Level0(){

static double s[7];static double direction;static double magnitude;static double distance;static double angle;int i;

switch(process_state){case sfINIT:case 18:

sonar(&s[0]);process_state = 19;break;

case 19:feelforce(&s[0],&direction,&magnitude);process_state = 20;break;

case 20:runaway(direction,magnitude,&angle,&distance);process_state = 21;break;

Livello_zerocase 21:

runaway(direction,magnitude,&angle,&distance);if ( (direction != 0) && (angle != 0)) process_state = 22;else process_state = 18;break;

case 22:turn(angle);process_state = 23;break;

case 23:if (sfDoneHeading(10)) process_state=24;break;

case 24:forward(distance);process_state = 25;break;

case 25:if (sfDonePosition(10)) process_state=18;break;

Livello_zerocase sfSUSPEND:

sfSMessage("suspended");break;

case sfRESUME:sfSMessage("Level 0 resumed");process_state=18;break;

}}

Wandervoid wander(double *ang_act, double *dis_act){

*dis_act = 250; *ang_act = 15;

}

Avoidvoid avoid(double dir_rep, double mag_rep, double dir_act,

double mag_act, double *angle, double *distance){

if (mag_rep <= SAFE_DIST){*angle=dir_rep + dir_act;*distance = mag_act;

}else{

*distance = mag_act;*angle = dir_act;

}}

Livello_Unovoid Level1(){

static double s[7];static double direction;static double magnitude;static double distance;static double angle;static double dist_rnd;static double ang_rnd;static struct _timeb timestart;static struct _timeb timecurr;static sfprocess *proc;int i;char str_s[12];

switch(process_state){

case sfINIT:case 18:

_ftime( &timestart);process_state = 19;break;

Livello_Unocase 19:

_ftime( &timecurr);

if ((timecurr.time - timestart.time)>15){sonar(&s[0]);feelforce(&s[0],&direction,&magnitude);wander(&ang_rnd,&dist_rnd);process_state = 20;

}break;

case 20:sfSMessage("suspending Level 0");proc=sfFindProcess("Level0");sfSetProcessState (proc,sfSUSPEND);process_state = 22;break;

case 22:avoid(direction,magnitude,ang_rnd,dist_rnd,&angle,&distance);process_state = 23;break;

Livello_Unocase 23:

sfSMessage("state 23");turn(angle);process_state = 24;break;

case 24:if (sfDoneHeading(10)) process_state=25;break;

case 25:forward(distance);process_state = 26;break;

case 26:if (sfDonePosition(10)) {

sfSetProcessState (proc,sfRESUME);process_state=18;

}break;

Livello_Unocase sfSUSPEND:

break;

case sfRESUME:process_state=18;break;

}}