esercitazione implementazione di un sistema reattivo per la navigazione sulla base robotica pioneer...
TRANSCRIPT
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( ×tart);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;
}}