ESERCITAZIONE

1. ESERCITAZIONE Implementazione di un sistema reattivo per la navigazione sulla base robotica Pioneer I

2. 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

3. Motori Interfaccia UtenteSoftware di alto livelloAmbiente di sviluppo Comandi Controllore di basso livello Dati sensori Sensori(US, Encoder) Architettura del sistema RS232 o connessione radio

4. Architettura del sistema Saphira Colbert Language and Evaluator Software di acquisizione e elaborazione Cavo Seriale RWI-Pioneer I

5. 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

6. Saphira: il simulatore Simulatore

7. Il sistema di coordinate di riferimento

8. I comandi da tastiera

9. I comandi

10. 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.

11. 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.

12. 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.

13. 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); } }

14. 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); } }

15. 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

16. 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.

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

18. Il linguaggio Colbert:Activity Schemas act <aname> [([<type> <param>]*)] { [<type> <local-var>]* [update <stmt>] <stmts> }

19. Il linguaggio Colbert:tipi e variabili C Colbert Dichiarazioni int sfINT int a; float sfFLOAT ptr tonowhere; void sfVOID float **f; string sfSTRING robot *r; act sfACTIVITY void * sfPTR

20. 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>;

21. 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;

22. 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.

23. Saphira API: Robot State struct robot sfRobot

24. 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.

25. 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.

26. 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.

27. 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.

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

29. Il livello 2

30. Sonar void sonar(double *s){ int i; for (i=0;i<7;i++) s[i]= (double) sfSonarRange(i); }

31. 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]; }

32. 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; } }

33. Turn void turn(double angle){ sfSetDHeading((int) angle); }

34. Forward void forward(double distance){ sfSetPosition((int) distance); }

35. Livello_zero void 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;

36. Livello_zero case 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;

37. Livello_zero case sfSUSPEND: sfSMessage("suspended"); break; case sfRESUME: sfSMessage("Level 0 resumed"); process_state=18; break; } }

38. Wander void wander(double *ang_act, double *dis_act){ *dis_act = 250; *ang_act = 15; }

39. 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; } }

40. 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;

41. Livello_Uno case 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;

42. Livello_Uno case 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;

43. Livello_Uno case sfSUSPEND: break; case sfRESUME: process_state=18; break; } }