ESERCITAZIONE
Implementazione di un sistema
reattivo per la navigazione sulla
base robotica Pioneer I
Il sistema robotico Pioneer I
Real 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
Interfaccia Utente
Software di alto
livello
Ambiente di
sviluppo
Motori
Comandi
Dati sensori
Controllore
di basso
livello
Sensori
(US, Encoder)
RS 232 o connessione radio
Architettura del sistema
Saphira
Colbert
Language and
Evaluator
Software di
acquisizione e
elaborazione
Cavo Seriale
RWI-Pioneer I
Saphira: l’interfaccia per il
controllo del sistema
Connessione con il
robot
Robot
Distanza rilevata
dai sensori US
Stato
Comandi in Linguaggio
Colbert e messaggi di
sistema
Saphira: il simulatore
Simulatore
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
int
float
void
string
act
void *
Colbert
sfINT
sfFLOAT
sfVOID
sfSTRING
sfACTIVITY
sfPTR
Dichiarazioni
int a;
ptr tonowhere;
float **f;
robot *r;
Il linguaggio Colbert
Conditinal 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;
int a;
int *b;
float *c;
*c = *b;
b = &a;
*b = 3;
a = 2;
p.x = 1.0;
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 State
struct robot sfRobot
Saphira API: Sonar buckets
The 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 Control
void 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];
}
Runaway
void 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_zero
void Level0(){
static double
static double
static double
static double
static double
int i;
s[7];
direction;
magnitude;
distance;
angle;
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_zero
case 21:
case 22:
case 23:
case 24:
case 25:
runaway(direction,magnitude,&angle,&distance);
if ( (direction != 0) && (angle != 0)) process_state = 22;
else process_state = 18;
break;
turn(angle);
process_state = 23;
break;
if (sfDoneHeading(10)) process_state=24;
break;
forward(distance);
process_state = 25;
break;
if (sfDonePosition(10)) process_state=18;
break;
Livello_zero
}
}
case sfSUSPEND:
sfSMessage("suspended");
break;
case sfRESUME:
sfSMessage("Level 0 resumed");
process_state=18;
break;
Wander
void wander(double *ang_act, double *dis_act){
*dis_act = 250;
*ang_act = 15;
}
Avoid
void 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_Uno
void 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_Uno
case 19:
case 20:
case 22:
_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;
sfSMessage("suspending Level 0");
proc=sfFindProcess("Level0");
sfSetProcessState (proc,sfSUSPEND);
process_state = 22;
break;
avoid(direction,magnitude,ang_rnd,dist_rnd,&angle,&distance);
process_state = 23;
break;
Livello_Uno
case 23:
case 24:
case 25:
case 26:
sfSMessage("state 23");
turn(angle);
process_state = 24;
break;
if (sfDoneHeading(10)) process_state=25;
break;
forward(distance);
process_state = 26;
break;
if (sfDonePosition(10)) {
sfSetProcessState (proc,sfRESUME);
process_state=18;
}
break;
Livello_Uno
case sfSUSPEND:
break;
case sfRESUME:
process_state=18;
break;
}
}