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
Architettura del sistema
RS 232 o connessione radio
4
Architettura del sistema
Saphira
Colbert Language and Evaluator
Software di acquisizione e elaborazione
Cavo Seriale
RWI-Pioneer I
5
Saphira linterfaccia 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, its possible
  to load a file that loads other files
• Istruzione load ltfilegt 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 ColbertDirect 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 ColbertDirect Motion Commands

• move(int mm)
• turn(int deg)
• turnto(int deg)
• speed(int mms)
• rotate(int degs)
• halt

18
Il linguaggio ColbertActivity Schemas

• act ltanamegt (lttypegt ltparamgt)
• lttypegt ltlocal-vargt
• update ltstmtgt
• ltstmtsgt

19
Il linguaggio Colberttipi 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 (ltc_expgt) ltstmtsgt else ltstmtsgt
• waitfor ltc_expgt timeout ltngt
• Iteration and Branching Statements
• while (ltc_expgt) ltstmtsgt
• ltlabelgt
• goto ltlabelgt

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 numth 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 (i0ilt7i)
• si (double) sfSonarRange(i)

31
FeelForce

• void feelforce(double s, double direction,
  double magnitude)
• int i,imin
• int a7 90, 30, 15, 0, -15, -30, -90
• imin0
• for (i1ilt7i)
• if (si lt simin) imin i
• direction aimin
• magnitude simin

32
Runaway

• void runaway(double direction, double magnitude,
  double angle, double distance)
• if (magnitude lt SAFE_DIST)
• distance -(SAFE_DIST - magnitude)
• angledirection
• 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 s7
• static double direction
• static double magnitude
• static double distance
• static double angle
• int i
• switch(process_state)
• case sfINIT
• case 18
• sonar(s0)
• process_state 19
• break
• case 19
• feelforce(s0,direction,magnitude)
• process_state 20
• break
• case 20

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_state24
• break
• case 24
• forward(distance)
• process_state 25
• break
• case 25
• if (sfDonePosition(10)) process_state18
• break

37
Livello_zero

• case sfSUSPEND
• sfSMessage("suspended")
• break
• case sfRESUME
• sfSMessage("Level 0 resumed")
• process_state18
• 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 lt SAFE_DIST)
• angledir_rep dir_act
• distance mag_act
• else
• distance mag_act
• angle dir_act

40
Livello_Uno

• void Level1()
• static double s7
• 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_s12
• switch(process_state)
• case sfINIT
• case 18

41
Livello_Uno

• case 19
• _ftime( timecurr)
• if ((timecurr.time - timestart.time)gt15)
• sonar(s0)
• feelforce(s0,direction,magnitude)
• wander(ang_rnd,dist_rnd)
• process_state 20
• break
• case 20
• sfSMessage("suspending Level 0")
• procsfFindProcess("Level0")
• sfSetProcessState (proc,sfSUSPEND)
• process_state 22
• break
• case 22
• avoid(direction,magnitude,ang_rnd,dist_rnd,ang
  le,distance)
• process_state 23

42
Livello_Uno

• case 23
• sfSMessage("state 23")
• turn(angle)
• process_state 24
• break
• case 24
• if (sfDoneHeading(10)) process_state25
• break
• case 25
• forward(distance)
• process_state 26
• break
• case 26
• if (sfDonePosition(10))
• sfSetProcessState (proc,sfRESUME)
• process_state18

43
Livello_Uno

• case sfSUSPEND
• break
• case sfRESUME
• process_state18
• break