Documentation of the Smart Car Demonstrator€¦ · Title: Documentation of the Smart Car...
59
Documentation of the Smart Car Demonstrator Albert Dias, Jörg Irran Research Report 45/2002 Institut für Technische Informatik Technische Universität Wien, Vienna, Austria July, 2004
Transcript of Documentation of the Smart Car Demonstrator€¦ · Title: Documentation of the Smart Car...
Documentation of the Smart Car Demonstrator
Albert Dias, Jörg Irran
Research Report 45/2002 Institut für Technische Informatik
Technische Universität Wien, Vienna, Austria
July, 2004
Research Report 45/2002
2
Research Report 45/2002
Table of Contents
1 Introduction ..............................................................................................................................................................5
2 Theory........................................................................................................................................................................7
2.1 Time Triggered Protocol for SAE Class A applications (TTP/A) ..........................................................7 2.1.1 UART data........................................................................................................................................7 2.1.2 Rounds and Round Descriptor Lists (RODL)..............................................................................7 2.1.3 Round Sequences (ROSE)...............................................................................................................9
2.2 Modes of operation.......................................................................................................................................9 2.2.1 Rabbit mode .....................................................................................................................................9 2.2.2 Turtle mode ......................................................................................................................................9
2.3 Grid generation .............................................................................................................................................9 2.3.1 Line evaluation ..............................................................................................................................10 2.3.2 Grid entry algorithms ...................................................................................................................13 2.3.3 Object recognition..........................................................................................................................13 2.3.4 Grid Ownership.............................................................................................................................14 2.3.5 Grid updating ................................................................................................................................14
2.4 Navigation decisions ..................................................................................................................................15 2.4.1 Path planning.................................................................................................................................15 2.4.2 Path assessment .............................................................................................................................17 2.4.3 Decision making ............................................................................................................................18 2.4.4 Aborting navigation......................................................................................................................18
2.5 Calibration and Filtering............................................................................................................................19 2.5.1 Sharp GP2D02 Infrared sensor calibration.................................................................................19 2.5.2 Sharp GP2D02 Infrared sensor: Infinity detection filtering.....................................................20 2.5.3 Polaroid 6500 series Ultrasonic ranging sensor calibration .....................................................21 2.5.4 Futaba S3003 Servo motor calibration ........................................................................................22
3 Smart Car Hardware architecture........................................................................................................................23
3.1 Mechanical ...................................................................................................................................................23 3.1.1 Smart Car body..............................................................................................................................23 3.1.2 Steering and drive .........................................................................................................................23
3.2 Electrical .......................................................................................................................................................23 3.2.1 Power supply system....................................................................................................................23 3.2.2 TTP/A bus......................................................................................................................................24 3.2.3 Sharp GP2D02 Infrared sensors...................................................................................................24 3.2.4 Futaba S3003 Analog Servo..........................................................................................................25 3.2.5 Polaroid 6500 series Ultrasonic ranging .....................................................................................25 3.2.6 Euroturn Shaft Position Encoder.................................................................................................26
3.3 Microprocessors ..........................................................................................................................................27 3.3.1 Master Node...................................................................................................................................27 3.3.2 ATMega128 Slave Node ...............................................................................................................27 3.3.3 AT90S4433 Slave Node .................................................................................................................28
3
Research Report 45/2002
4 Smart Car Software architecture ......................................................................................................................... 29
4.1 General software architecture of the Smart Car..................................................................................... 29 4.1.1 Distributed processing ................................................................................................................. 29 4.1.2 Communication ............................................................................................................................ 29
4.2 General software architecture of a TTP/A node.................................................................................... 29 4.3 Specific software architecture of TTP/A nodes in the Smart Car ....................................................... 30
4.3.1 TTP/A Master: Master node (Master) ....................................................................................... 30 4.3.2 TTP/A Slave: Navigation node (Nav) ....................................................................................... 35 4.3.3 TTP/A Slave: Position node (Pos) .............................................................................................. 45 4.3.4 TTP/A Slave: Infrared sensor nodes (Infra_)............................................................................ 46 4.3.5 TTP/A Slave: Ultrasonic sensor nodes (Ultra_) ....................................................................... 48 4.3.6 TTP/A Slave: Servo nodes (Servo_ and Steering).................................................................... 49 4.3.7 TTP/A Slave: Digital control node (Speed) .............................................................................. 50
5 Smart Car Monitoring utilities............................................................................................................................. 52
5.1 Monitor ........................................................................................................................................................ 52 5.2 Gridiculous.................................................................................................................................................. 52
6 Performance evaluation........................................................................................................................................ 54
6.1 Hardware performance evaluation.......................................................................................................... 54 6.1.1 Sharp GP2D02 Infrared Sensors: Accuracy............................................................................... 54 6.1.2 Polaroid 6500 series Ultrasonic ranging Sensors: High voltage interference ....................... 54
6.2 Software performance evaluation............................................................................................................ 54 6.2.1 Grid generation ............................................................................................................................. 54
7 Summary................................................................................................................................................................. 55
8 Bibliography........................................................................................................................................................... 56
9 Appendix ................................................................................................................................................................ 57
9.1 Sharp GP2D02 Infrared sensor calibration: Raw data........................................................................... 57 9.2 Polaroid 6500 series Ultrasonic ranging sensor calibration: Raw data ............................................... 57 9.3 Sharp GP2D02 Infrared sensor Infinity detection: Raw data ............................................................... 58
4
Research Report 45/2002
1 Introduction
The Time Triggered Protocol for SAE Class A applications (TTP/A) [5] is the newest member of the family of protocols for the Time Triggered Architecture (TTA) [7] developed at the Institut für Technische Informatik at the Technische Universität in Vienna, Austria. The following report will describe the design and implementation of an autonomous mobile robot that serves as a case study for TTA and TTP/A. The Smart Car is a demonstrator for the TTP/A protocol, with the aim of determining the real-life feasibility of implementing a TTP/A cluster [5]. The Smart Car’s primary task is to navigate through a static obstacle course by perceiving its immediate environment with the help of a combination of infrared and ultrasonic distance sensors. The car uses a series of grid generation and navigational path planning algorithms to choose the best path. Additionally, the car is also fault tolerant, in the sense that if it makes a poor navigational path decision and encounters a dead end, the car can retrace its path backwards and take an alternative path.
Figure 1: Smart Car Schematic
The Smart Car’s operation can be categorized into 4 main fields – Software, Electrical hardware, Electro-mechanical hardware and Mechanical hardware. The first layer in the hierarchy – the software layer, refers to software code for the TTP/A nodes compiled using the AVR GCC compiler. The second layer - the electrical hardware layer, consists of a fieldbus network, complete with TTP/A nodes and the car’s TTP/A communication bus. The third layer – electrical/electromechanical hardware layer refers to the sensors, power supplies, servos, LED indicators and other components such as additional power supply busses. The fourth layer – the mechanical layer, consists if the main chassis of the Smart Car, which is an off-the-shelf four-wheeled model car fitted with a wooden mounting board.
5
Research Report 45/2002
TTP/A
MechanicalHardware
Electrical/Electro-
MechanicalHardware
ElectricalHardware
Software
Non-TTP/A
Figure 2: Smart Car categorization and scope of TTP/A
6
Research Report 45/2002
2 Theory
2.1 Time Triggered Protocol for SAE Class A applications (TTP/A)
The Time Triggered Protocol for SAE Class A applications (TTP/A) is the newest member of the family of protocols for the Time Triggered Architecture (TTA) developed at the Institut für Technische Informatik at the Technische Universität in Vienna, Austria.
2.1.1 UART data
At the lowest hierarchical layer in TTP/A bus communication, data is transferred using the Universal Asynchronous Receiver Transmitter (UART) protocol. Each byte of data from a TTP/A node is represented on the bus by 1 start bit, 8 data bits, 1 parity bit and 2 stop bits. It should be noted that TTP/A control bytes and regular data bytes use different parity conventions, in order to ensure that commands are not misinterpreted as data bytes and vice versa.
Figure 3: Schematic diagram of UART data on the bus
2.1.2 Rounds and Round Descriptor Lists (RODL)
The Round Descriptor List is a local file stored in each node, which tells the node what action to perform at different slots of a round on the TTP/A bus. It is imperative that all nodes in a TTP/A cluster share a mutual understanding of the operation of other nodes in the network during a RODL, so that there is only one node writing to the TTP/A bus in any given slot. In order to ease this process, the Smart Car uses a centralized RODL table to ensure that all nodes in the network are programmed with the correct RODL and that there are no clashes on the TTP/A bus. Data is represented in a tabular format exactly as it appears in the respective node’s RODL header file. The most common abbreviations used in the table are R (1-byte Unprotected Valid Read from local IFS to TTP/A bus), W (1-byte Unprotected Valid Write from TTP/A bus to local IFS), E (Execute TTP/A task), WS (1-byte Unprotected Valid Write from TTP/A bus to local IFS, with timing synchronization) and RI (1-byte Unprotected Invalid Read to signify end of RODL). All numerical data in the RODL configuration tables are in the hex format.
00 01 02 03 04 05 06 07 08 09 0A 0B 0C 0D
FireworksByte
MasterSlaveSync
Figure 4: Schematic diagram of slots in RODL0 (slot numbers in boxes)
00 01 02 03 04 05 06 07 08 09 0A 0B 0C 0D
FireworksByte
MasterSlaveSync
0E 0F 10 11 12 13
MasterSlaveSync
Figure 5: Schematic diagram of slots in RODL2 (slot numbers in boxes)
7
Research Report 45/2002
Figure 6: Smart Car RODL Configuration Table
8
Research Report 45/2002 2.1.3 Round Sequences (ROSE)
A round sequence is a fixed pattern of interleaving RODLs and master slave rounds. Round sequences are always determined by the master (or Pyromaniac – a master solely responsible for fireworks bytes) in the TTP/A cluster. In the last current version of the Smart Car the master was running the following round sequences:
RODL 0 MSA Figure 7: ROSE0 round sequence
RODL 0 MSA RODL 0 MSA RODL 0 MSA RODL2 Figure 8: ROSE 1 round sequence
2.2 Modes of operation
The Smart Car has two principal modes of operation – Rabbit mode and Turtle mode. Each mode is characterized by its own specific ROSE.
2.2.1 Rabbit mode
The Smart Car is set into Rabbit mode whenever it is determined that the region in front of the car is clear enough to allow faster movement. Unlike the Turtle mode where all the sensors are used, the Rabbit mode only uses the Ultrasonic ranging sensors and Infrared sensors 2 and 3 (mounted on the sides). Consequently, the Smart Car only uses RODL0 in the Rabbit mode with interleaving master slave address and data rounds (corresponding to ROSE 0).
2.2.2 Turtle mode
The Smart Car is set into Turtle mode whenever there is an obstacle in the car’s field of view, which requires navigational decision making. In the Turtle mode, all available sensors on the Smart Car (Ultrasonic ranging sensors and Infrared sensors 1, 2 and 3) are used to make navigational decisions and the car moves at a slower speed. The Smart Car uses an optimal combination of RODL0 and RODL2 in the Turtle mode with interleaving master slave address and data rounds (corresponding to ROSE 1).
2.3 Grid generation
The grid seen by the Smart Car is a 17x10 cell array corresponding to a 170x100cm region around the car. It is generated by a series of 3 sensors involved in a 13-direction sweep of the area in front of the car. Four key elements of grid generation are line evaluation, object recognition, grid ownership and grid updating.
9
Research Report 45/2002
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
170 cm Figure 9: Smart Car Grid array representation corresponding to actual view
2.3.1 Line evaluation
By convention, the Smart Car uses an 8-bit data value to represent the status of each cell in the grid. Cell values can range from 0x00 (certainly empty) through 0x80 (uncertain) to 0xFF (certainly full/blocked). During the Infrared sensor sweep, the sensors can only return distance values for the direction in which they are pointed. It is the responsibility of the navigation node to map the measured distance for a particular direction onto the appropriate grid cells. A simple solution that takes up the least processing resources would be to pre-program the navigation node with pre-processed information on which cells to fill for each sensor direction and measured distance. However, due to the large memory space requirements of this approach, the Smart Car uses an alternative and more scalable technique, whereby points along the line are generated during the sweeps themselves. The line evaluation algorithm uses 5 inputs (2 source coordinates, 2 end target coordinates and the measured target distance) to generate a line between the source and measured target. Due to the resolution limitation of the existing grid, all input measurements must be scaled down by a factor of 10. The algorithm runs a loop to generate dtarget (after scaling by 10) points on the grid, using Equations 1, 2 and 3 in the code segment shown below. void addtogrid (uint8_t s, uint8_t d, uint8_t dist_) io = sensor[s].origin.i; //CARTESIAN POSITION OF SENSOR ALONG i jo = sensor[s].origin.j; //CARTESIAN POSITION OF SENSOR ALONG j it = sensor[s].target[d].i; //CARTESIAN POSITION OF LINE TARGET ALONG i jt = sensor[s].target[d].j; //CARTESIAN POSITION OF LINE TARGET ALONG j maxdist = sensor[s].target[d].length/10; //MAXIMUM DISTANCE FOR A SPECIFIED DIRECTION dist_ = dist_/10; if (dist_>maxdist) dist_=maxdist+1; if (it>io) di = it-io; //+ DISTANCE BETWEEN ORIGIN/TARGET ALONG i else di = io-it; //- DISTANCE BETWEEN ORIGIN/TARGET ALONG i if (jt>jo) dj = jt-jo; //+ DISTANCE BETWEEN ORIGIN/TARGET ALONG j else dj = jo-jt; //- DISTANCE BETWEEN ORIGIN/TARGET ALONG j data=0x00; for (dist=1;dist<=dist_;dist++) //FILL CELLS WITH WHITE TILL DISTANCE
10
Research Report 45/2002 if (dist<=maxdist) //CHECKS THAT CELL IS WITHIN GRID if (dist==dist_) data=0xFF; cell=io; //START OF EQUATION BREAKDOWN temp=di; temp=((temp*dist)/maxdist); if (it>io) cell=cell+temp; else cell=cell-temp; cell=(cell*17)+jo; temp=dj; temp=((temp*dist)/maxdist); if (jt>jo) cell=cell+temp; else cell=cell-temp; //END OF EQUATION BREAKDOWN ifs_write_byte(&data,0,IFS_ADDR(18,((cell>>2)+1),(cell&3)));
(po intpo int origin t arg et origin
t arg et
di i i i
d⎛ ⎞
= + −⎜⎜⎝ ⎠
)⎟⎟ Equation 1: Generating ipoint
(pointpo int origin t arg et origin
t arg et
dj j j j
d⎛ ⎞
= + −⎜⎜⎝ ⎠
)⎟⎟ Equation 2: Generating jpoint
( )grid point po intcell w i j= + Equation 3: Generating the cell address The run-time complexity of the line evaluation algorithm is directly proportional to the measured target distance. Eventually, in the manner described above, once all 3 sensors have completed 13 sweeps, the grid should be a reasonable representation of the region in front of the car.
11
Research Report 45/2002
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1000
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1001
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1002
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1003
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1004
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1005
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1006
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1007
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1008
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1009
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1010
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1011
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1012
Figure 10: 13 line sweeps evaluated from software simulation of grid
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
FIELDVIEW
Figure 11: Total coverage area of the Infrared Sensor array
12
Research Report 45/2002 2.3.2 Grid entry algorithms
2.3.2.1 Robust certainty grid algorithm
The robust certainty grid algorithm [12] is a fault tolerant sensor mapping algorithm that was developed by Wilfried Elmenreich, Lukas Schneider and Raimund Kirner at the Institut für Technische Informatik, Vienna [3]. It works with at least 3 sensors and can tolerate repeated sensor failure, with the ability to reintegrate recovered sensors. void AddToGrid (uint8_t sensor, uint8_t cell) if ((cell.owner == UNKNOWN) || (cell.owner == sensor)) cell.occ = sensor.measurement; cell.owner = sensor; else comparison := 4*(cell.occ-0.5)*(sensor.measurement-0.5); weight1 := abs(cell.occ-0.5)*cell.owner.conf; weight2 := abs(sensor.measurement-0.5)*sensor.conf; cell.occ := (cell.occ*weight1+sensor.measurement*weight2)/ (weight1 + weight2); if comparison > THRESHOLD then inc(cell.owner.conf); inc(sensor.conf); if comparison < –THRESHOLD then dec(cell.owner.conf); dec(sensor.conf); contribution := 4*(cell.occ-0.5)*(sensor.measurement-0.5); if contribution > THRESHOLD then cell.owner := sensor; else cell.owner := unknown;
2.3.2.2 Simplified grid algorithm
One of the key problems with effectively implementing the robust certainty grid algorithm in the Smart Car, is that there is only one case where all 3 sensors are able to validate the presence or absence of an object in a grid cell (sensor 1 direction 6, sensor 2 direction 12 and sensor 3 direction 12, all pointing to grid cell[8][0]). In such circumstances, where most of the time, only 2 sensors can validate call, it is very difficult to gauge the actual certainty of a sensor based on the technique used in the robust certainty grid algorithm. Hence, it is much simpler to use a modified version of the robust certainty grid algorithm, which uses alternative techniques to calculate sensor certainty, such as the variance of sensor readings.
2.3.3 Object recognition
Once the navigation node is able to generate points along the line of sight of each sensor for a complete sensor sweep, it is time to put together the collected data and generate the complete grid. It is only possible to recognize objects if they have a surface or plane facing the Smart Car’s sensors. Such surfaces are represented by values corresponding to ‘certainly blocked’ (dark areas on the grid). Other planes or surfaces which cannot be seen the Smart Car’s sensors will be represented by a shadow of values corresponding to ‘uncertain’.
13
Research Report 45/2002
80 80 80 80 80 00 00 00 00 00 00 00 80 80 80 80 80
80 80 80 80 80 80 00 00 00 00 00 80 80 80 80 80 80
80 80 80 80 80 FF 00 00 00 00 00 FF 80 80 80 80 00
00 FF 80 80 FF 00 00 00 00 00 00 00 FF FF FF 00 00
00 00 FF FF FF 00 00 00 00 00 00 00 00 00 00 00 00
80 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 80
80 80 80 00 00 00 00 00 00 00 00 00 00 00 80 80 80
80 80 80 80 80 00 00 00 00 00 00 00 80 80 80 80 80
80 80 80 80 80 80 80 00 00 00 80 80 80 80 80 80 80
80 80 80 80 80 80 80 80 80 80 80 80 80 80 80 80 80
80 80 80 80 80 80 80 80 80 80 80 80 80 80 80 80 80
Figure 12: Actual objects, numerical grid interpretation and schematic grid interpretation
2.3.4 Grid Ownership
A value is stored for each cell in the grid that allows keeping track of which sensor last updated the cell. This value is called cell ownership. In this way it is possible to compare the readings of different sensors on a single cell. Each sensor has a confidence value that is dynamically changed during the mapmaking process, and this confidence level affects the sensor’s ability to change the occupancy probability of the cells of the grid.
Figure 13: Cell owner value (cell ownership) for each grid cell [3]
At the beginning of the mapmaking process all grid cells are initialized with occupancy = 0.5 (uncertain) and cell ownership = unknown. The new occupancy value of a cell is calculated as a weighted average between the old value and the current measurement. The weights are derived from the confidence value of the sensor and the significance of the measurement. A measurement is more significant if it has a greater absolute distance to the uncertain state (0.5). The next step is to update the ownership value of the cell. Therefore, a contribution value is derived. The contribution is a measurement of the consistency of the sensor measurement with the new occupancy value. If the contribution is above a certain threshold, the contributing sensor becomes the new owner of the cell. Otherwise the ownership value will be reset to unknown.
2.3.5 Grid updating
A critical limiting factor in the Smart Car demonstrator is the response time of the Servo motors. Being the slowest components on the network, they reduce the car’s ability to generate the grid quickly. As a result, after a full sweep, sensor readings taken at the beginning of the sweep will be considerably more outdated than readings taken at the end of the sweep. An effective solution for this would be to set all grid cells which haven’t been updated for a given duration to uncertain (0x80). However, due to the nature of the grid’s sweep and the interleaving paths, this can become quite a complicated task. Furthermore, we are limited by the processing speed of the nodes. A simple solution would be to employ grid fading. The procedure basically involves progressively reducing the certainty of the grid by making all cells converge towards 0x80. (0xFF/0x00 corresponds to high certainty, while 0x80 corresponds to low certainty). Ideally, the factor by which the grid cells converge to 0x80 should be small enough so as to avoid noticeable difference in a very short period of time, but to produce a noticeable difference in longer periods of time if a cell is not updated. An additional advantage of grid fading is that it ensures robustness against certain kinds of sensor failure, such as when a sensor fails to initialize and is unable to communicate data on the bus.
14
Research Report 45/2002 2.4 Navigation decisions
One of the Smart Car’s most critical tasks is effective navigation around obstacles. The decision-making process takes up a significant amount of processor time. The navigational decision-making process can be roughly explained in four areas – Path planning, Path assessment, Decision making and Aborting.
2.4.1 Path planning
The first step in navigational decision-making is to plan out the paths that the car can take, given a fixed set of steering constants. The steering slave node can currently handle up to 85 distinct constants. However, since most of them do not make a significant directional difference for short distances, the Smart Car only uses 13 evenly-spaced directional paths out of the possible 85. In order to confirm the actual paths taken by the car, a 170cm x 100 cm paper grid was set up in the laboratory. The smart car was then placed at its ‘grid origin’ (the position indicated by the black areas in figure 12) and driven with the different steering constants at minimum speed. After a series of tests, the paths taken by the car for the 13 steering directions were plotted and manually converted to grid representations, as shown in the following figure.
15
Research Report 45/2002
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1000
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1001
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1002
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1003
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1004
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1005
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1006
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1007
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1008
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1009
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1010
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1011
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
1012
Figure 14: The 13 directional paths evaluated from a tested 1:1 scale grid
A significant amount of trial-and-error testing was performed to come up with a simple formula to generate paths reasonably similar to the grid representations. However, since no simple formula could be derived, the Smart Car currently uses a simple memory storage format with 8 data structures (for the first 8 horizontal lines) for each grid representation. Each of these data structures holds the vertical origin and horizontal length of a linear cross-section corresponding to each of the previously mentioned horizontal lines. The following code segment shows how the path for steering direction 0 can be stored efficiently using this technique.
16
Research Report 45/2002 typedef struct smartline__ uint8_t start; uint8_t length; smartline_; smartline_ direction0[8] = 0, 0, //SMARTLINE 0 0, 0, //SMARTLINE 1 0, 0, //SMARTLINE 2 0, 6, //SMARTLINE 3 0, 8, //SMARTLINE 4 0, 9, //SMARTLINE 5 5, 4, //SMARTLINE 6 7, 3 //SMARTLINE 7 ;
2.4.2 Path assessment
A key factor in sensible navigation decision-making lies in being able to understand the relative risk of different obstacles in one’s path. It needn’t be explicitly stated that closer obstacles pose greater risks than farther obstacles. The Smart Car uses a simple risk distribution scheme wherein the visible region around the smart car is divided into concentric rings of width 20cm, centered on the center of the Smart Car. Each of these rings is then given a unique risk weight for all the grid cells in that ring, starting with the least risk for the ring containing the farthest visible point. In the Smart Car, the radial risk distribution is stored in a single array as shown in the following code segment.
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
0
1
2
3
4
5
6
7
8
9
10
RISK 1
RISK 2
RISK 3
RISK 4
RISK 5
Figure 15: Radial risk distribution in the Smart Car
uint8_t gridweight [187] = 0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0, 0,1,1,1,1,2,2,2,2,2,2,2,1,1,1,1,0, 1,1,1,2,2,2,2,2,2,2,2,2,2,2,1,1,1, 1,1,2,2,2,2,3,3,3,3,3,2,2,2,2,1,1, 1,1,2,2,3,3,3,3,3,3,3,3,3,2,2,1,1, 0,2,2,2,3,3,4,4,4,4,4,3,3,2,2,2,0, 0,0,0,3,3,4,4,4,4,4,4,4,3,3,0,0,0, 0,0,0,0,0,4,4,5,5,5,4,4,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ;
17
Research Report 45/2002 2.4.3 Decision making
Using the information explained in the previous sections on Path planning and Path assessment, the Smart Car now has the required raw data to begin the process of making a navigational decision or path selection. The first step in this process is to calculate the average risk for each of the 13 possible steering direction paths using equation 4. In Equation 4, the summation i=[0, n) refers to the n points defined by the 8 smart lines for a respective path.
n
i 0
gridweight[i].grid[i]risk[i]
n==∑
Equation 4: Calculating average risk of a path
Once the average risk for each of the 13 paths is evaluated, a simple algorithm is used to search for the path with the least risk. In the event of identically low risk factors for two or more paths, the path closest to direction 6 (straight) is preferentially chosen (In the following figure, higher numerical values indicate higher preference). If two steering directions with identically low risk factors have the same deviation from direction 6, then either one of the directions is randomly chosen. However, if the path eventually chosen has a risk factor higher than a pre-determined threshold value, the car aborts further action and retraces its path until a decision to go forward can be made, which does not repeat the path it just retraced. In the following example scenario, the least risk factor is found to be that of directions 06 and 02. However, direction 06 is chosen over direction 02, because the former has a higher preference factor. Furthermore, as a side comment, directions 03, 04, 05, 08, 09, 10, 11 and 12 can never be chosen because they are above the risk threshold. If by any chance, either of these directions turns out to have the lowest risk factor, then the car will simply abort and retrace its path.
00 01 02 03 04 05 06 07 08 09 10 11 12
0 1 2 3 4 5 6 5 4 3 2 1 0DIRECTION
PREFERENCE
AVERAGE RISK
THRESHOLD
Figure 16: Example scenario for navigation decision-making
2.4.4 Aborting navigation
One of the main aspects of a good navigation algorithm is predictable behavior in unexpected scenarios. If the Smart Car finds that the minimum risk factor for a particular sweep is above the threshold value, then it switches into abort mode. In abort mode, the Smart Car assumes that all available paths ahead are either completely blocked or just too narrow for the car to pass through. The car then retraces its path backwards using an in-built history list of the last ‘n’ movements. For every step backward, the car re-generates the grid and looks for alternative navigational paths. If it finds an alternative path different from the last chosen path at that point, then the Smart Car takes the new path. If it finds an alternative path similar to the last chosen path at that point, then the Smart Car continues the above procedure until the history list is empty. The key limiting factor in this algorithm is the size of the history list, which can be increased or decreased, depending on the complexity of the area the car is in.
18
Research Report 45/2002
Figure 17: Illustration of the Smart Car aborting navigation
2.5 Calibration and Filtering
One of the most time consuming, yet vital tasks in any embedded system that interfaces with external devices is calibration. In the Smart Car, the Sharp GP2D02 Infrared sensors, the Polaroid 6500 series Ultrasonic ranging sensors and the Futaba S3003 servo motors are 3 key elements that require proper calibration, owing to their inconsistent input/output behavior.
2.5.1 Sharp GP2D02 Infrared sensor calibration
The Sharp GP2D02 Infrared sensors proved to be the most problematic component in the Smart Car, exhibiting a variety of problems from highly varying outputs for a consistent input to being unable to detect infinity (see appendix 8.3). In order to calibrate the Sharp GP2D02 Infrared sensors, the first step was to generate an accurate representation of the performance of each of the 3 sensors used in the Smart Car. This was accomplished using a mounted 80gsm white A4-matte sheet of paper, which is representative of most environments that the Smart Car will encounter within the range of the grid. For each sheet position, a set of readings from the sensor were taken and averaged after filtering out irregular values. The values obtained from each sensor were then tested with equation 5 with different sets of constants, to determine the set that would yield the least average error after conversion. The average percentage error after conversion for sensors 1, 2 and 3 were 6%, 5% and 2% respectively (see appendix 8.1). The following figure shows the results of average distance measurements from each of the 3 Sharp GP2D02 Infrared sensors used in the version of the Smart Car at the time of this report.
greal
sensor o
kd
d k=
− Equation 5: Converting infrared sensor readings to real distances
19
Research Report 45/2002
Sensor 1
Sensor 2
Sensor 3
0
50
100
150
200
250
0 20 40 60 80 100 120
Act ual Di st ance ( cm)
Figure 18: Sharp GP2D02 raw sensor data with 80gsm white A4 matte sheet
2.5.2 Sharp GP2D02 Infrared sensor: Infinity detection filtering
A serious hardware failure with the GP2D02 Infrared sensor that Sharp has failed to document is the sensor’s inability to distinguish between objects in its range and objects at infinity (objects out of its range). In a series of tests, conducted with the Smart Car, each of the 3 Sharp GP2D02 Infrared sensors were pointed in a direction free of objects for at least 3m (equivalent to infinity for the sensor) and sampled for approximately 10 minutes (5000 readings). Following this, a random infrared sensor was pointed at an object that returned an 8-bit un-calibrated sensor reading of 100, for approximately 10 minutes (5000 readings). The frequency distribution curves shown in the following figures clearly indicate failure on the part of the Sharp GP2D02 to distinguish between objects in the sensor’s range and objects at infinity. However, a clearly noticeable difference between the set of values taken at infinity and for an object within range is the standard deviation of the values around the mean. For infinity readings, the standard deviation around the mean is quite high compared to that for an object within range. It is this difference between the two cases that is used in the Infinity detection algorithm. The algorithm is quite simple, and uses a linear last-in-first-out (LIFO) queue of size ‘ntotal’ to cache the last ‘ntotal’ infrared sensor readings (ri). If a minimum of ‘nmin’ readings (nmin ≤ ntotal) fall within a pre-specified tolerance range ‘∆rmax’, then the mean of the nmin cached readings that satisfy the tolerance range is assumed to be a valid, non-infinity reading (for smaller values of ∆rmax, any value from the cached readings can be randomly picked). However, if more than nmin readings fall out of the tolerance range, then it is assumed that the reading is infinity. Owing to several memory and stack restrictions on the AT90S4433, the infrared nodes use a highly simplified version of the infinity filtering algorithm. In a series of tests (raw data documented in appendix 8.3) //NOTE THAT UNCALIBRATED 8-BIT SENSOR READINGS ARE ROUGHLY INVERSELY PROPORTIONAL TO DISTANCE #define LISTSIZE 4 //CACHE SIZE #define MINRANGE 2 //TOLERANCE RANGE #define MAX 0 //INFINITY READING uint8_t data_cache [LISTSIZE]; uint8_t i; uint8_t inifinity_filter (uint8_t data) for (i=0;i<(LISTSIZE-1);i++) data_cache[i]=data_cache[i+1];
20
Research Report 45/2002 data_cache[LISTSIZE-1]=data; for (i=0;i<(LISTSIZE-1);i++) if (data_cache[i]>data_cache[i+1]) if ((data_cache[i]-data_cache[i+1])>MINRANGE) return(MAX); else if ((data_cache[i+1]-data_cache[i])>MINRANGE) return(MAX); return(data);
0
5 0
10 0
15 0
2 0 0
2 5 0
3 0 0
3 5 0
4 0 0
0 5 0 10 0 15 0 2 0 0 2 5 0
8 - b it s e n s o r r e a d in g
0
10 0
2 0 0
3 0 0
4 0 0
5 0 0
6 0 0
7 0 0
0 5 0 10 0 15 0 2 0 0 2 5 0
8 - b it s e n s o r r e a d in g
0
10 0
2 0 0
3 0 0
4 0 0
5 0 0
6 0 0
7 0 0
0 5 0 10 0 15 0 2 0 0 2 5 0
8 - b it s e n s o r r e a d in g
0
10 0 0
2 0 0 0
3 0 0 0
4 0 0 0
5 0 0 0
6 0 0 0
0 5 0 10 0 15 0 2 0 0 2 5 0
8 - b it s e n s o r r e a d in g
Figure 19: Frequency distribution curves for Unfiltered Infrared sensor 1 pointing to Infinity (top-left),
Unfiltered Infrared sensor 2 pointing to Infinity (top-right), Unfiltered Infrared sensor 3 pointing to Infinity (bottom-left) and an Unfiltered Infrared sensor pointing to an object at 8-bit un-calibrated sensor reading 100
2.5.3 Polaroid 6500 series Ultrasonic ranging sensor calibration
The Polaroid 6500 Ultrasonic ranging sensors were relatively easier to calibrate than the infrared sensors. The first step was to generate an accurate representation of the performance of each of the 2 sensors used in the Smart Car, using a mounted 80gsm A4-matte sheet of paper. For each mounted sheet position, a set of readings from the sensor were taken and averaged. Unlike the infrared sensors, the ultrasonic ranging sensors didn’t require value filtering as the values were quite stable. The values obtained from each sensor were then tested with equation 6 with different sets of constants, to determine the set that would yield the least average error after conversion. The average percentage error after conversion for sensors 1 and 2 were 8% each (see appendix 8.2). The following figure shows the results
21
Research Report 45/2002 of average distance measurements from each of the Polaroid 6500 series Ultrasonic ranging sensors used in the version of the Smart Car at the time of this report.
sensorreal 2
1
dd
C= − C Equation 6: Converting ultrasound sensor readings to real distances
0
20
40
60
80
100
120
0.0 5.0 10.0 15.0 20.0 25.0 30.0
Act ual Di st ance ( dm)
Figure 20: Polaroid 6500 series raw sensor data with 80gsm A4 matte sheet
2.5.4 Futaba S3003 Servo motor calibration
Contrary to the information contained in the datasheets, the Futaba S3003 servo motors did not align to approximately 90 degrees when given a pulse-width modulated (PWM) signal of width 1.5ms. In a series of tests conducted on the 3 servo motors in the Smart Car, it was found that the angles were quite varied among the 3 servos, for a consistent PWM signal.
0° 90° 180°
Figure 21: Servo angle convention used in the Smart Car
The servos were calibrated by experimentally determining the pulse width required to generate angles of 0 degrees, 90 degrees and 180 degrees for each of the 3 servos. A trial-and-error method was then used to determine the best calibration formula to map the experimentally determined pulse width software constants to software constants that would theoretically generate pulse widths of 1.0ms, 1.5ms and 2.0ms respectively.
22
Research Report 45/2002
3 Smart Car Hardware architecture
The hardware architecture of the Smart Car can be subdivided into two main groups – the mechanical component architecture and electrical component architecture. The mechanical component architecture discusses several aspects of the Smart Car’s physical construction such as the car’s body, node and sensor mounting. The electrical component architecture discusses the design and implementation of the chipsets on the Smart Car.
3.1 Mechanical
3.1.1 Smart Car body
The mechanical basis of the Smart Car – its body – is a model radio controlled car from TAMIYA called “Wild Dagger”, available at Conrad Electronics1. Designed for off-road usage, the car has great ground clearance und all-terrain tires. A wooden plate has been mounted on the chassis to support mounting of nodes and sensors.
3.1.2 Steering and drive
The drive unit has been realized through an electronic speed controller used for model radio-controlled cars. The motor is attached to the electronic speed controller and driven by sending a pulse-width modulated (PWM) signal. The PWM-signal is identical to the signal used to control the servos, therefore it is possible to replace a servo with an electronic drive speed controller without changing the control method. See figure 21 for a comparison of a standard servo and an electronic speed controller.
PWM signal [ms] standard servo [degrees]
electronic speed controller
0,6 0 max. reverse 1,5 45 neutral 2,4 90 max. forward
Figure 22: Controlling the motor with the electronic speed controller
Steering is performed by a servo connected to the front wheels via a steering linkage. Both, the steering servo and the electronic speed controller are instrumented by a microcontroller, more precisely each by a single pin of a microcontroller port which serves the pulse-width modulated signal. The movement range of the steering servo was divided into 13 parts to limit the amount of possible driving directions (see 2.4.1). For further information about the drive and steering model for the Smart car see the work of Mathias Csisinko on “Smart car simulation environment” [11].
3.2 Electrical
3.2.1 Power supply system
The power supply system of the smart car is quite simple in principle, yet it required a significant amount of experimentation before the current configuration. Owing to high instantaneous current demands from several components such as the Polaroid 6500 series ultrasonic ranging sensors (up to 5A) and the Futaba S3003 servos, the TTP/A nodes require the use of capacitor arrays to overcome voltage drops from weaker battery packs that cannot meet the high instantaneous power demands. 1 http://www.conrad.at - item number 215651
23
Research Report 45/2002 In its current configuration, the system uses a single power supply which is distributed to various components through the Smart Car along two independent buses V1 and V2. The main input is buffered by a 1mF capacitor. V1, which operates at approximately 6V (LC7806CV voltage regulator), is used to power devices such as the servos and ultrasonic sensors which have irregular current requirements with high instantaneous current bursts. V2 which operates at approximately the same voltage as the input voltage (Vin), is used to power the TTP/A nodes which have regular current requirements. V2 is buffered by a 4mF capacitor to protect the TTP/A system from failure in the event that the battery pack cannot supply enough power during instantaneous current bursts on V1, which could pull V2 below TTP/A operation levels.
VIN
GND
L7806CV
1mF 4mF
V1
GND
V2
GND Figure 23: Power supply system for the Smart Car
3.2.2 TTP/A bus
The TTP/A communication bus used in the Smart Car uses a pull-up resistor on the bus’s signal line. A low signal is therefore dominant and high signal is recessive. If two or more devices try to communicate simultaneously, the device(s) sending a logical zero will dominate. Therefore the output of all simultaneously sent data will be a logical AND of all data.
3.2.3 Sharp GP2D02 Infrared sensors
The Sharp GP2D02 requires additional circuitry on its interface lines since the Vin line operates a 3.3V level, and not the Atmel microcontrollers’ 5V level in the Smart Car.
3 kΩ
Figure 24: Connection diagram for the Sharp GP2D02 Infrared sensor
24
Research Report 45/2002
MSB LSB
1.6 ms ~ 2.0 ms
0.1 ms
Sensor ready & delivers reading Post reading sensor delaySensor not ready
~ 70.0 ms
Vin
Vout
Figure 25: Sharp GP2D02 timing diagram for Vin and Vout
3.2.4 Futaba S3003 Analog Servo
The Futaba S3003 analog servo doesn’t require any external circuitry on its interface lines. The servo motors respond to pulse width modulated (PWM) signals in the range from 1ms to 2ms, with 1.5ms corresponding to roughly 90 degrees.
1.0 ~ 2.0 ms ~ 2.0 ms
Servo active Inter pulse gapServo inactive
CONTROL
Figure 26: Futaba S3003 Analog servo timing diagram for Control line
3.2.5 Polaroid 6500 series Ultrasonic ranging
The echo line of the 6500 series Ultrasonic ranging sensor requires a 47kΩ pull-up resistor in order that the output signal range can be detected by the TTP/A slave node.
25
Research Report 45/2002
4.7kΩ
Figure 27: Connection diagram for the Polaroid 6500 series Ultrasonic sensor
≤ 40.0 ms ~ 2.0 ms
Sensor active Post-reading delaySensor inactive
INIT
ECHO
Object echo time
≥ 0 ms
Figure 28: Polaroid 6500 series timing diagram for Init and Echo
3.2.6 Euroturn Shaft Position Encoder
Clockwise(A leads B)
No rotation
A
B
Counter-clockwise(B leads A)
Figure 29:Euroturn shaft position encoder timing diagram for channels A and B
26
Research Report 45/2002 3.3 Microprocessors
3.3.1 Master Node
The Master node is realized by an Atmel AT90S8515 microcontroller running at a clock rate of 7,3728MHz [12]. The Atmel AT90S8515 is based on the AVR RISC architecture with 8KB in-system programmable flash-memory [8], 512 Bytes of internal SRAM and 512 Bytes of in-system programmable EEPROM. It further has a programmable serial UART, one 8-bit counter and one 16-bit counter. The Master node with its RS232 interface is also able to operate as a gateway node. It is possible to connect a PC that acts as a gateway providing a CORBA interface that can be used by monitoring and configuration tools.
Figure 30: Atmel AT90S8515 Node size comparison
3.3.2 ATMega128 Slave Node
The ATMega128 Slave node runs on the Atmel ATMega128 microprocessor, which is a high performance, low power 8-bit RISC microcontroller that can be clocked at speeds up to 16MHz. The node features 128 KB of in-system reprogrammable flash memory, 4KB of internal SRAM, 4 KB of EEPROM on the ATMega128, 4KB of ATMega128 RAM and 32KB of external memory. A key advantage of the ATMega128 is that it is fully backward compatible with the ATMega103 (in ATMega103 compatibility mode supported on fuse M103C), which is well supported by the AVR-GCC compiler and TTP/A configurations.
Figure 31: ATMega128 Slave Node size comparison
27
Research Report 45/2002 Nevertheless, despite most efforts made by Atmel to ensure backward compatibility, there exist several difficulties in programming the ATMega128 over a Serial interface, especially since most common serial interface chipsets such as the STK-200 aren’t well supported by software ISPs. Several tests conducted with Atmel’s standalone AVR-ISP and LancOS’s PonyProg failed to program the ATMega128. However, more advanced serial interface chipsets such as the Atmel STK-500 proved to be quite convenient for programming the ATMega128, when used with and Atmel AVR Studio 4. The STK-500 supports firmware upgrades by serial interface to accommodate newer microcontrollers. A key aspect of the ATMega128 microcontroller is that unlike most previous Atmel microcontrollers, it features an on-chip RC oscillator capable of supporting up to 8MHz (factory-preset to 1MHz). As a result, the ATMega128 requires fuse reprogramming when driven by any external oscillators or RCs. The ATMega128’s fuses can be easily reprogrammed using a serial interface with Atmel’s STK-500 and Atmel’s AVR Studio 4. Care must be taken to ensure that the correct oscillator frequency fuses are selected, failing which, the chip may not respond to serial interface programming.
Figure 32: Programming the ATMega128’s fuses using Atmel AVR Studio 4
3.3.3 AT90S4433 Slave Node
The ATMEL AT90S4433 Slave Node is a member of the high-performance and low-power AVR 8-bit RISC Architecture and able to reach up to 8 MIPS throughput at 8 MHz [9]. The AT90S4433 Slave Nodes on the Smart Car are running at 7.3728MHz [12]. The AT90S4433 features 4 KB of in-system reprogrammable flash memory, 128 Bytes of internal SRAM and 256 Bytes of EEPROM. It also features a programmable serial UART, one 8-bit counter and one 16-bit counter.
Figure 33: AT90S4433 Slave Node size comparison
28
Research Report 45/2002
4 Smart Car Software architecture
4.1 General software architecture of the Smart Car
4.1.1 Distributed processing
The Smart Car works on the principle of distributed processing, whereby several independent computing elements (TTP/A master and slave nodes) process data locally and exchange it with other nodes over a time-triggered TTP/A bus.
TTP/A bus
TTP/ASlave 2
TTP/ASlave 4
TTP/ASlave 6
TTP/ASlave 1
TTP/ASlave 3
TTP/ASlave 5
TTP/AMaster
Figure 34: Distributed processing on the TTP/A bus
4.1.2 Communication
Most of the communication occurring in the Smart Car is done in real-time. Sensor nodes (Ultrasonic, Infrared and Odometric) send their perceived values in real-time. The navigation node reads real-time data from and sends real-time data to the bus. Actuator nodes (Servo and Speed) receive their commands in real-time. The master node only participates in the communication in master/slave rounds to fulfill its gateway function serving external monitoring and configuration processes.
Node sending receiving real-time master/slave rounds
Servo X X Speed X X Ultrasonic X X Infrared X X Odometric X X Navigation X X X Master X X X
Figure 35: Communication scheme of the Smart Car
4.2 General software architecture of a TTP/A node
The software controlling a TTP/A node consists of protocol code and application code [10]. The protocol code is entirely implemented in Assembler. A node application can be implemented by using three key features - TTP/A tasks, background tasks and interrupt handler. Time-critical tasks are implemented in TTP/A tasks, which are executed synchronously in execute slots of a RODL (see also section 2.1.2). Non time-critical tasks can be implemented in the background task. Interrupt handlers can also be defined. The
29
Research Report 45/2002 protocol code is executed with the highest priority, followed by time-triggered tasks with lower priority and the background task with the lowest priority. A more detailed description can be found in [10].
Figure 36: Software architecture of a TTP/A node [10]
4.3 Specific software architecture of TTP/A nodes in the Smart Car
In the following section there is a listing of program codes of the nodes which are used on the Smart Car.
4.3.1 TTP/A Master: Master node (Master)
#define GW_INPUT 17 #define GW_OUTPUT 18 #define GW_INPUT_HEX 0x0E #define GW_OUTPUT_HEX 0x0F #define PARITY #include <io.h> #include <interrupt.h> #include "depend.h" #include "ttpa.h" #include "avr_macros.h" #include "constants.h" #include <sig-avr.h> #include "master.h" /* prototypes */ void comm_task(void); /* variables */ uint8_t *ifs_F20; uint8_t *ifs_FROSE; int deb_y=0; int deb_x=0; /* variables for monitoring */ volatile uint8_t comm_mode = RECEIVE_MSR; uint8_t data_msd[5]; uint8_t data_msr[5]; uint8_t counter = 0; uint8_t timeout_counter = 0;
30
Research Report 45/2002 volatile uint8_t timeout = 0; volatile uint8_t overrun = 0; uint8_t nodeid = 0xFF; uint8_t error_code = 0x00; #if 1 /*-----------------------------*/ /* Count Bits for Parity Check */ /*-----------------------------*/ uint8_t countbits(uint8_t count_data) uint8_t counter = 0; while (count_data > 0) counter += count_data & 0x01; count_data = count_data >> 1; return counter; /*-----------------------*/ /* Send Data to the UART */ /*-----------------------*/ void UART_SendByte(uint8_t Data) volatile uint8_t Reg_Temp; /* Read register */ Reg_Temp = inp(UART_CONTROL_REGISTER); /* Set Parity Bit --> only DATA_FRAME Mode */ /* TXB80 auf 0/1 setzen */ if ((countbits(Data) % 2) == 0) Reg_Temp &= 0xFE; else Reg_Temp |= 0x01; /* Write Bit 9 - Parity Bit*/ outp(Reg_Temp,UART_CONTROL_REGISTER); /* Send character */ outp(Data,UDR); #endif void init_user(void) map_ifsbyte(ifs_F20, file20,1,0); map_ifsbyte(ifs_FROSE, rose,1,0); #if 1 /* init Timer 0 for timeout */ /* enable Overflow Interrupt for TC0 */ set_timerirq(T0_OVF_MSK); /* clear TC0 */ outp(0x00,TCNT0); /* prescale CK/64 for TC0 */ #if defined (__AVR_AT90S4433__) outp(BV(CS01)|BV(CS00),TCCR0); #elif defined (__AVR_AT90S8515__) outp(BV(CS01)|BV(CS00),TCCR0); #elif defined (__AVR_ATmega103__) outp(BV(CS02),TCCR0); #endif #endif #if 0 /* init Timer 0 for timeout */ /* enable Overflow Interrupt for TC0 */ set_timerirq(T0_OVF_MSK); /* clear TC0 */ outp(0x00,TCNT0); /* prescale CK/1024 for TC0 */ outp(BV(CS02)|BV(CS00),TCCR0); #endif /* set data direction for RTS and CTS */ outp(inp(RTS_DDR) | BV(RTS_PIN),RTS_DDR); /* set bit -> output */ outp(inp(CTS_DDR) & (0xFF ^ BV(CTS_PIN)),CTS_DDR); /* clear bit -> input */
31
Research Report 45/2002 outp(0xFF,DDRB); // set all pins of port B as output sbi(PORTB,PB0); // turn yellow led off sbi(PORTB,PB1); // turn red led off outp(0xFF,DDRA); // set all pins of port A as output outp(0x00,PORTA); // turn all leds of port A off /* enable RxD/TxD for hardware UART */ outp(BV(RXEN)|BV(TXEN)|BV(CHR9),UART_CONTROL_REGISTER); /* RXEN: Receiver Enable */ /* TXEN: Transmitter Enable */ /* CHR9: 9 Bit - Parity Bit (RXB80 or TXB80) */ /* set baud rate: 9600 BAUD, 7.3728MHz -> 47 */ /* set baud rate: 19200 BAUD, 7.3728MHz -> 23 */ /* set baud rate: 115200 BAUD, 7.3728MHz -> 3 */ outp(UART_CONSTANT,UBRR); //ifs_write_word(&ms_counter,0,IFS_ADDR(0x12,2,3)); RESTART; void ttpa_bgtask(void) if (deb_y++ & 0x050) cbi(PORTB,PB0); else sbi(PORTB,PB0); // turn yellow led on //nodeid = 0x66; ifs_read_byte(&nodeid,0,IFS_ADDR(8,1,3)); //Config File 8 / Record 1 / Byte 3 -> NODEID /* Communication - Task */ comm_task(); /* Communication Task for Monitoring */ void comm_task(void) uint8_t err_code; switch(comm_mode) case RECEIVE_MSR: /* set RTS line */ SET_RTS; /* receive four data bytes and one checkbyte */ counter = 0; while ((counter < 5) && (timeout == 0) && (overrun == 0)) if (UART_RECEIVE_COMPLETE) data_msr[counter] = inp(UDR); /* check if first byte has odd parity */ if ((counter == 0) && (((countbits(data_msr[0]) + parity_bit) & 0x01) != 1)) timeout = 1; ++counter; if (UART_OVERRUN) overrun = 1; /* clear RTS line */ CLEAR_RTS; if (timeout) timeout = 0; RESTART;
32
Research Report 45/2002 else if (overrun) overrun = 0; comm_mode = SEND_MSD; /* prepare data */ data_msd[0] = 0xFF; data_msd[1] = 0xFF; data_msd[2] = 0xFF; data_msd[3] = 0xFF; data_msd[4] = IFS_ERR_COMM; else /* verify checkbyte and nodeid */ if (check_msr && check_alias) if (opcode == READ) /* Master reads -> Slave writes */ comm_mode = SEND_MSD; /* prepare data */ err_code = ifs_read_byte(data_msd, 3, IFS_ADDR(file, record, 0)); if (err_code == 0x00) /* no error occurred */ data_msd[4] = check(data_msd); else /* an error occurred */ data_msd[0] = 0xFF; data_msd[1] = 0xFF; data_msd[2] = 0xFF; data_msd[3] = 0xFF; data_msd[4] = err_code; else if (opcode == WRITE) comm_mode = RECEIVE_MSD; else /* checkbyte error or wrong nodeid */ RESTART; break; case RECEIVE_MSD: /* set RTS line */ SET_RTS; /* reset timeout */ RESET_TIMEOUT; /* receive four data bytes and one checkbyte */ counter = 0; while ((counter < 5) && (timeout==0)) if (UART_RECEIVE_COMPLETE) //RESET_TIMEOUT; data_msd[counter] = inp(UDR); ++counter; if (UART_OVERRUN) timeout = 1; /* clear RTS line */ CLEAR_RTS; if (timeout)
33
Research Report 45/2002 timeout = 0; else /* verify checkbyte */ if (check_msd) /* write received data to IFS */ ifs_write_byte(data_msd, 3, IFS_ADDR(file, record, 0)); RESTART; break; case SEND_MSD: RESET_TIMEOUT; /* send four data bytes and one check byte */ counter = 0; while ((counter < 5) && (timeout==0)) if (CTS_SET) /* wait until previous byte was sent */ if ((counter == 0) || (UART_TRANSMIT_COMPLETE)) //RESET_TIMEOUT; CLEAR_TXC; UART_SendByte(data_msd[counter]); ++counter; if (timeout) timeout = 0; else while ((!UART_TRANSMIT_COMPLETE))// || (!CTS_CLEAR)) /* wait until last byte was sent and CTS line is clear */ CLEAR_TXC; RESTART; break; INTERRUPT(SIG_OVERFLOW0) if (timeout_counter < TIMEOUT_CONST-1) ++timeout_counter; else timeout = 1; INTERRUPT(ttpa_task00) outp(inp(PORTB)^0x02,PORTB); // toggle red led if(ifs_F20[0x10] > 12) ifs_FROSE[0] = 0; else ifs_FROSE[0] = 1;
34
Research Report 45/2002 4.3.2 TTP/A Slave: Navigation node (Nav)
//#define COMM //ENABLES COMMUNICATION WITH MONITOR.EXE ON A PC //#define GRIDICULOUS //ENABLES LINK TO GRIDICULOUS.EXE ON THE PC //#define ABORT_ROUTINE //ENABLES ABORT ROUTINE TO BACK OUT OF DEAD ENDS //#define ROBUST_GRID //ENABLES ROBUST CERTAINTY GRID ALGORITHM #define DUMP_MODE //ENABLES DUMP MODE #ifdef DUMP_MODE #define DUMP_GRID //DUMPS GRID PARAMETERS #define VERBOSE //OUTPUTS INFORMATION ABOUT STATE #define DUMP_NAVIGATION //DUMPS NAVIGATION PARAMETERS // #define DUMP_SENSOR_MEASUREMENTS //DUMPS SENSOR MEASUREMENTS TO COM PORT #endif #define BUFFERED_COMMUNICATION #define SENSOR_DELAY 2 #define THRESHOLD 0x40 #define HISTORY_SIZE 10 //SIZE OF ARRAY TO STORE LAST MOVEMENTS #define MASTERLINK //ENABLES SELECTION OF ROSE ON DUMMY MASTER //#define ALBERT_GRID //ENABLES SIMPLER GRID ALGORITHM #define MOVE_DISTANCE 20 //DISTANCE CAR MOVES EVERY RUN ///////////////////////////////////////////////////////////////////////////// // HEADER FILES // ///////////////////////////////////////////////////////////////////////////// #include <io.h> #include <interrupt.h> #include <sig-avr.h> #include "ttpa.h" #include "avr_macros.h" #include "constants.h" #include "navigation.h" //CO-REQUISITE FOR NAVIGATION.C #ifdef COMM #include "monitor.c" //CO-REQUISITE FOR COMMUNICATION WITH MONITOR.EXE ON A PC #endif #ifdef GRIDICULOUS #include "gridiculous.c" #endif ///////////////////////////////////////////////////////////////////////////// // GLOBAL VARIABLES // ///////////////////////////////////////////////////////////////////////////// uint8_t recnr; uint8_t ubrr1; //UART BAUD RATE VARIABLE uint8_t dir_ = 0; //PREPROCESS CURRENT DIRECTION OF SENSORS uint8_t dir1 = 0; //CURRENT DIRECTION OF SENSORS (0-12) uint8_t dir2 = 0; //CURRENT DIRECTION OF SENSORS (0-12) uint8_t dir3 = 0; //CURRENT DIRECTION OF SENSORS (0-12) uint8_t usercounter = 0x00; //USER COUNTER uint8_t dir_pref[13] = 6, 5, 7, 4, 8, 3, 9, 2,10, 1,11, 0,12; //PRIORITY OF DIRECTIONAL PREFERENCES //uint8_t sequence1[13] = 6, 5, 7, 4, 8, 3, 9, 2,10, 1,11, 0,12; //SEQUENCE OF SENSOR1 DIRECTIONS //uint8_t sequence2[13] = 0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10,11,12; //SEQUENCE OF SENSOR2 DIRECTIONS //uint8_t sequence3[13] = 0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10,11,12; //SEQUENCE OF SENSOR3 DIRECTIONS uint8_t sequence1[13] = 0, 2, 4, 6, 8,10,12,11, 9, 7, 5, 3, 1; //SEQUENCE OF SENSOR1 DIRECTIONS uint8_t sequence2[13] = 0, 2, 4, 6, 8,10,12,11, 9, 7, 5, 3, 1; //SEQUENCE OF SENSOR1 DIRECTIONS uint8_t sequence3[13] = 0, 2, 4, 6, 8,10,12,11, 9, 7, 5, 3, 1; //SEQUENCE OF SENSOR1 DIRECTIONS uint8_t position_end; //END POSITION uint8_t maxdist; //MAXIMUM DISTANCE FOR A SPECIFIED SENSOR/DIRECTION uint8_t mindir; uint8_t dist; //USER COUNT VARIABLE uint8_t dist1; //DISTANCE READING FROM SENSOR1
35
Research Report 45/2002 uint8_t dist2; //DISTANCE READING FROM SENSOR2 uint8_t dist3; //DISTANCE READING FROM SENSOR3 uint8_t dist1_; //DISTANCE READING FROM SENSOR1 uint8_t dist2_; //DISTANCE READING FROM SENSOR2 uint8_t dist3_; //DISTANCE READING FROM SENSOR3 uint8_t sensor_conf[3] = 0x80, 0x80, 0x80; //CONFIDENCE OF SENSORS uint8_t cell; //STORES POSITION IN GRID ARRAY uint8_t cell2; //STORES POSITION IN GRID ARRAY uint8_t io; //CARTESIAN POSITION OF SENSOR ALONG i uint8_t jo; //CARTESIAN POSITION OF SENSOR ALONG j uint8_t it; //CARTESIAN POSITION OF LINE TARGET ALONG i uint8_t jt; //CARTESIAN POSITION OF LINE TARGET ALONG j uint8_t di; //CARTESIAN DISTANCE BETWEEN ORIGIN/TARGET ALONG i uint8_t dj; //CARTESIAN DISTANCE BETWEEN ORIGIN/TARGET ALONG j uint8_t d; uint8_t d_; uint8_t i; uint8_t j; uint8_t k; uint8_t l; uint8_t data; //STORES DATA READ FROM OR WRITTEN TO A CELL uint8_t data2; //STORES DATA READ FROM OR WRITTEN TO A CELL uint8_t status=SWEEP; //STATUS OF SMARTCAR uint8_t move_history[HISTORY_SIZE]; //ARRAY TO STORE HISTORY OF LAST MOVEMENTS uint8_t backingout=0; //FLAG TO INDICATE WHETHER CAR IS BACKING OUT (REVERSING) uint16_t temp; uint16_t histogram[13]; //ARRAY FOR COMPARING 13 DIRECTIONS uint16_t weightsum[13]; //ARRAY FOR COMPARING 13 DIRECTIONS uint32_t dummy = 0x80808080; //SET RECORD TO UNCERTAIN (0x80) uint8_t cell_gridiculous=0; //CELL COUNTER FOR GRIDICULOUS uint8_t carmode=TURTLE_MODE; //SPECIFIES THE MODE THE CAR IS OPERATING ON #ifdef DUMP_MODE const uint8_t hexconvert[16]=48,49,50,51,52,53,54,55,56,57,65,66,67,68,69,70;//ASCII CHARACTERS FOR HEX VALUES uint8_t temp_test; //DISTANCE READING FROM SENSOR3 #endif ///////////////////////////////////////////////////////////////////////////// // MACROS // ///////////////////////////////////////////////////////////////////////////// #ifdef BUFFERED_COMMUNICATION #define RINGBUFFERSIZE 1024 //uint8_t uart_buffer[RINGBUFFERSIZE] __attribute__((section(".external"))); uint8_t uart_buffer[RINGBUFFERSIZE]; volatile uint16_t buffer_ptr=0; uint16_t send_ptr=0; #define send_byte(z) uart_buffer[buffer_ptr++ & (RINGBUFFERSIZE-1)]=z; #define print_hexbyte(z) if ((z)>=16) send_byte(hexconvert[(z) >> 4]); send_byte(hexconvert[(z) & 15]); void print_string(uint8_t *buf) /* send null-terminated buffer <buf> to uart */ while(*buf != 0) send_byte(*buf); buf++; #else #define send_byte(z) while((inp(USR)&0x20)!=0x20); outp(z,UDR); #endif #define send_EOL send_byte(13); send_byte(10); //send End Of Line #define send_immediately(z) while((inp(USR)&0x20)!=0x20); outp(z,UDR); #define print_hexbyte_immediately(z) if ((z)>=16) send_immediately(hexconvert[(z) >> 4]); send_immediately(hexconvert[(z) & 15]); ///////////////////////////////////////////////////////////////////////////// // IFS POINTERS // ///////////////////////////////////////////////////////////////////////////// uint8_t *ifs_position; //CURRENT POSITION
36
Research Report 45/2002 uint8_t *ifs_speed; //CURRENT SPEED uint8_t *ifs_steering; uint8_t *ifs_grid; uint8_t *ifs_servo1; uint8_t *ifs_servo2; uint8_t *ifs_servo3; uint8_t *ifs_infrared1; uint8_t *ifs_infrared2; uint8_t *ifs_infrared3; uint8_t *ifs_ultra1; uint8_t *ifs_ultra2; uint8_t *dir_risk; ///////////////////////////////////////////////////////////////////////////// // FUNCTIONS // ///////////////////////////////////////////////////////////////////////////// void map_ifs_pointers () map_ifsbyte (ifs_ultra1, file10,1,0); map_ifsbyte (ifs_ultra2, file10,1,2); map_ifsbyte (ifs_infrared1, file10,2,0); map_ifsbyte (ifs_servo1, file10,2,2); map_ifsbyte (ifs_infrared2, file10,3,0); map_ifsbyte (ifs_servo2, file10,3,2); map_ifsbyte (ifs_infrared3, file10,4,0); map_ifsbyte (ifs_servo3, file10,4,2); map_ifsbyte (ifs_speed, file10,5,0); map_ifsbyte (ifs_steering, file10,5,1); map_ifsbyte (ifs_position, file10,6,0); map_ifsbyte (ifs_grid, file12,1,0); map_ifsbyte (dir_risk, file10,7,0); #ifdef BUFFERED_COMMUNICATION //map_ifsbyte (uart_buffer, file25,1,0); #endif void showGrid(void) // Only a debug function, writes grid to RS232 #define GRIDWIDTH 17 #define GRIDHEIGHT 11 int x, y, n; print_string("Current state of certainty grid:\r\n"); for (y=0 ; y < GRIDHEIGHT; y++) for (x = 0; x < GRIDWIDTH; x++) n = y * GRIDWIDTH + x; print_hexbyte(ifs_grid[n] >> 4); send_byte(' '); send_EOL; void init_user(void) int i; outp(0xC0,MCUCR); //ENABLE EXTERNAL RAM - NO WRITESTATE & NO SLEEP #ifdef MASTERLINK outp(0xFF,DDRB); outp(ROSE1,PORTB); //REQUESTS MASTER TO TRIGGER ROSE1 #endif map_ifs_pointers(); #ifdef COMM init_monitor(); #endif #ifdef GRIDICULOUS init_gridiculous(); #endif dir_=0; ifs_steering[0]=direction[6].angle; //INITIALIZE STEERING TO FORWARD ifs_speed[0]=STOP; //INITIALIZE SPEED TO STOP #ifdef DUMP_MODE //outp(95,UBRR); // 9600 Baud @ 14745600 MHz
37
Research Report 45/2002 outp(47,UBRR); // 19200 Baud @ 14745600 MHz //outp(23,UBRR); // 38400 Baud @ 14745600 MHz //outp(7,UBRR); // 115200 Baud @ 14745600 MHz outp(0x08,UCR); asm(";hier!"); for( i=0; i<1000; i++ ) send_immediately(46); //2000 dots send_immediately(32); //2000 spaces send_immediately(13); //carriage return send_immediately(10); //linefeed #ifdef VERBOSE print_string("Smart Car navigation program running\r\n"); //print_string("Initialization complete\r\n"); showGrid(); #endif #endif void addtogrid (uint8_t s, uint8_t d, uint8_t dist_,uint8_t conf) //ADDTOGRID CURRENTLY DOWNSCALES RESOLUTION BY 10 io = sensor[s].origin.i; //CARTESIAN POSITION OF SENSOR ALONG i jo = sensor[s].origin.j; //CARTESIAN POSITION OF SENSOR ALONG j it = sensor[s].target[d].i; //CARTESIAN POSITION OF LINE TARGET ALONG i jt = sensor[s].target[d].j; //CARTESIAN POSITION OF LINE TARGET ALONG j maxdist = sensor[s].target[d].length; //MAXIMUM DISTANCE FOR A SPECIFIED SENSOR/DIRECTION #ifdef DUMP_GRID send_byte('1'+s); //sensor index send_byte(32); //space print_hexbyte(dist_); //distance send_byte(32); //space print_hexbyte(io); //io send_byte(32); //space print_hexbyte(jo); //jo send_byte(32); //space print_hexbyte(it); //it send_byte(32); //space print_hexbyte(jt); //jt send_byte(32); //space print_hexbyte(maxdist); //maxdist send_byte(32); //space print_hexbyte(conf); //confidence send_EOL; #endif if (conf==0) return; if (dist_>maxdist) dist_=maxdist+10; maxdist = (maxdist+5)/10; dist_ = (dist_+5)/10; if (it>io) di = it-io; //+CARTESIAN DISTANCE BETWEEN ORIGIN/TARGET ALONG i else di = io-it; //-CARTESIAN DISTANCE BETWEEN ORIGIN/TARGET ALONG i if (jt>jo) dj = jt-jo; //+CARTESIAN DISTANCE BETWEEN ORIGIN/TARGET ALONG j else dj = jo-jt; //-CARTESIAN DISTANCE BETWEEN ORIGIN/TARGET ALONG j
38
Research Report 45/2002 for (dist=1;dist<=maxdist;dist++) if (dist==dist_) data=0xFF; else if (dist>dist_) data=0x80; else data=0x00; //START OF EQUATION BREAKDOWN: cell=(17*(io+(di*dist/maxdist)))+(jo+(dj*dist/maxdist)); cell=io; temp=di; temp=((temp*dist)/maxdist); if (it>io) cell=cell+temp; else cell=cell-temp; cell=(cell*17)+jo; temp=dj; temp=((temp*dist)/maxdist); if (jt>jo) cell=cell+temp; else cell=cell-temp; //END OF EQUATION BREAKDOWN #ifdef ALBERT_GRID if (gridowner[cell]==0) ifs_grid[cell]=data; else ifs_grid[cell]=((ifs_grid[cell]/(gridowner[cell]+1))*gridowner[cell])+(data/(gridowner[cell]+1)); gridowner[cell]++; #else if (gridowner[cell]==0) ifs_grid[cell]=data; else ifs_grid[cell]=((ifs_grid[cell]/(gridowner[cell]+1))*gridowner[cell])+(data/(gridowner[cell]+1)); gridowner[cell]++; #endif uint8_t directions () mindir=dir_pref[0]; for (d_=0;d_<13;d_++) d=dir_pref[d_]; histogram[d]=0; weightsum[d]=0; for (i=0;i<8;i++)
39
Research Report 45/2002 for (j=direction[d].smartline[i].start;j<(direction[d].smartline[i].start+direction[d].smartline[i].length);j++) cell2=(17*i)+j; histogram[d]+=(gridweight[cell2]*ifs_grid[cell2]); weightsum[d]+=gridweight[cell2]; dir_risk[d]=histogram[d]/weightsum[d]; #ifdef DUMP_NAVIGATION print_string(" D:"); print_hexbyte(d); //direction print_string(" Risk:"); print_hexbyte(dir_risk[d]); //calculated risk #endif if (dir_risk[d]<dir_risk[mindir]) mindir=d; #ifdef DUMP_NAVIGATION send_EOL; #endif #ifdef ABORT_ROUTINE //threshold searching is poor ... redo to ensure that frontal path is also clear for swing radius if (backingout) if ((dir_risk[mindir]<THRESHOLD)&&(mindir!=move_history[0])) backingout=0; move_history[0]=mindir; status=MOVE_FORWARD; else for (k=0;k<(HISTORY_SIZE-2);k++) move_history[k]=move_history[k+1]; move_history[HISTORY_SIZE-1]=0xFF; //BOGEY VALUE mindir=move_history[0]; status=MOVE_REVERSE; else if (dir_risk[mindir]<THRESHOLD) for (k=(HISTORY_SIZE-2);k>0;k--) move_history[k+1]=move_history[k]; move_history[0]=mindir; status=MOVE_FORWARD; else backingout=1; mindir=move_history[0]; status=MOVE_REVERSE; #endif #ifndef ABORT_ROUTINE if (dir_risk[mindir]<THRESHOLD) status=MOVE_FORWARD; else status=ABORT_STOP; #endif return(mindir);
40
Research Report 45/2002 void fadegrid () for (cell=0;cell<136;cell++) //FADE GRID if (ifs_grid[cell]>0x80) ifs_grid[cell] -= FADESTEP; //LOW VALUES CONVERGE TO 0x80 if (ifs_grid[cell]<0x80) ifs_grid[cell] += FADESTEP; //HIGH VALUES CONVERGE TO 0x80 void resetgrid () for (recnr=1; recnr<48; recnr++) ifs_write_long(&dummy,0,IFS_ADDR(GRID_FILE,recnr,0)); ///////////////////////////////////////////////////////////////////////////// // BACKGROUND TASK // ///////////////////////////////////////////////////////////////////////////// void ttpa_bgtask(void) #ifdef BUFFERED_COMMUNICATION while(send_ptr!=buffer_ptr) int8_t h; //h=uart_buffer[send_ptr++ & (RINGBUFFERSIZE - 1)]; send_immediately(uart_buffer[send_ptr++ & (RINGBUFFERSIZE - 1)]); //send_immediately('.'); //if (h==32) // send_immediately(' ') //else // send_immediately('.'); #if 0 send_immediately('<'); print_hexbyte_immediately(buffer_ptr >> 8); print_hexbyte_immediately(buffer_ptr & 0xFF); send_immediately(':'); print_hexbyte_immediately(send_ptr >> 8); print_hexbyte_immediately(send_ptr & 0xFF); send_immediately('>'); #endif #endif #ifdef COMM monitor_bgtask(); #endif #ifdef GRIDICULOUS if ((inp(USR)&0x20)==0x20) if (cell_gridiculous==187) outp(0x7E,UDR); cell_gridiculous=0x00; else outp(0xFF-ifs_grid[cell_gridiculous],UDR); cell_gridiculous++; #endif ///////////////////////////////////////////////////////////////////////////// // TIME-TRIGGERED TASKS // /////////////////////////////////////////////////////////////////////////////
41
Research Report 45/2002 INTERRUPT(ttpa_task00) switch (carmode) case TURTLE_MODE : if ((ifs_ultra1[0]>20)&&(ifs_ultra2[0]>20)) //IF CLEAR FOR AT LEAST A 200cm RANGE carmode==RABBIT_MODE; #ifdef MASTERLINK outp(ROSE0,PORTB); //REQUESTS MASTER TO TRIGGER ROSE0 #endif break; case RABBIT_MODE : ifs_steering[0]=direction[6].angle; //SET DIRECTION TO STRAIGHT ifs_speed[0]=FORWARD; //MOVE FORWARD AT FULL SPEED if ((ifs_ultra1[0]<20)||(ifs_ultra2[0]<20)) //IF BLOCKED WITHIN A 200cm RANGE ifs_speed[0]=STOP; //STOP THE CAR carmode==TURTLE_MODE; #ifdef MASTERLINK outp(ROSE1,PORTB); //REQUESTS MASTER TO TRIGGER ROSE1 #endif dir_=0; status=SWEEP; resetgrid(); break; INTERRUPT(ttpa_task01) uint8_t chosen_dir; static uint8_t dist2cover_new,dist2cover_old; uint16_t ultra_dist; dir_risk[1]=status; // IFS File 10 record 7 byte 1 contains status switch (status) case SWEEP: usercounter++; if (usercounter==SENSOR_DELAY) #ifdef DUMP_SENSOR_MEASUREMENTS print_hexbyte(ifs_ultra1[0]); //ultrasonic sensor 1 send_byte(32); //space print_hexbyte(ifs_ultra2[0]); //ultrasonic sensor 2 send_byte(32); //space print_hexbyte(ifs_infrared1[0]); //infrared sensor 1 send_byte(32); //space print_hexbyte(ifs_infrared2[0]); //infrared sensor 2 send_byte(32); //space print_hexbyte(ifs_infrared3[0]); //infrared sensor 3 send_EOL; //End Of Line ifs_ultra1[0]=0; ifs_ultra2[0]=0; ifs_infrared1[0]=0; ifs_infrared2[0]=0; ifs_infrared3[0]=0; usercounter=SENSOR_DELAY-1; break; #endif usercounter=0x00; dir_++; dist1=ifs_infrared1[0]; //VALUES ARE BUFFERED FIRST TO AVOID DISCREPANCY dist2=ifs_infrared2[0]; dist3=ifs_infrared3[0]; //fadegrid(); #ifdef ALBERT_GRID if (dist1 != 0x00) addtogrid(0,dir1,dist1,ifs_infrared1[1]); //ADD SENSOR1 READING TO GRID ifs_infrared1[0]=0x00; if (dist2 != 0x00)
42
Research Report 45/2002 addtogrid(1,dir2,dist2,ifs_infrared1[1]); //ADD SENSOR2 READING TO GRID ifs_infrared2[0]=0x00; if (dist3 != 0x00) addtogrid(2,dir3,dist3,ifs_infrared1[1]); //ADD SENSOR3 READING TO GRID ifs_infrared3[0]=0x00; #else addtogrid(0,dir1,dist1,ifs_infrared1[1]); //ADD SENSOR1 READING TO GRID addtogrid(1,dir2,dist2,ifs_infrared1[1]); //ADD SENSOR2 READING TO GRID addtogrid(2,dir3,dist3,ifs_infrared1[1]); //ADD SENSOR3 READING TO GRID #endif if (dir_==13) //IF SWEEP COMPLETE, CHANGE STATUS dir_=0; for (l=0;l<187;l++) gridowner[l]=0; status=THINK; dir1=sequence1[dir_]; dir2=sequence2[dir_]; dir3=sequence3[dir_]; ifs_servo1[0]=sensor[0].angle[dir1]; ifs_servo2[0]=sensor[1].angle[dir2]; ifs_servo3[0]=sensor[2].angle[dir3]; #ifdef ALBERT_GRID if (usercounter==(SENSOR_DELAY-2)) ifs_infrared1[1]=0x00; ifs_infrared2[1]=0x00; ifs_infrared3[1]=0x00; dist1_=ifs_infrared1[0]; //VALUES ARE PREBUFFERED TO AVOID DISCREPANCY dist2_=ifs_infrared2[0]; dist3_=ifs_infrared3[0]; #endif break; case THINK: #ifdef VERBOSE showGrid(); print_string("THINK!\r\n"); #endif chosen_dir = directions(); //status IS CHANGED BY directions() //EITHER TO FORWARD, MOVE_REVERSE OR ABORT_STOP ifs_steering[0]=direction[directions()].angle; //CALCULATE STEERING DECISION position_end=(ifs_position[0]+MOVE_DISTANCE) % 0xFF; //CALCULATE TARGET POSITION ultra_dist=(ifs_ultra1[0]+ifs_ultra2[0]) >> 1; #ifdef VERBOSE print_string("Fused ultrasonic distance:"); print_hexbyte(ultra_dist); send_EOL; print_string("Chosen dir:"); print_hexbyte(chosen_dir); send_EOL; #endif if (chosen_dir == 6) // straight ahead if (ultra_dist > 10) status=RABBIT; ifs_speed[0]=FORWARD_SLOW; #ifdef VERBOSE print_string("Straight ahead at RABBIT mode!!!\r\n"); #endif
43
Research Report 45/2002 break; #ifdef VERBOSE print_string("Dir:"); print_hexbyte(ifs_steering[0]); print_string("\tTarget_distance:"); print_hexbyte(position_end); send_EOL; #endif break; case MOVE_FORWARD: ifs_speed[0]=FORWARD_SLOW; //MOVE FORWARD SLOWLY if (((position_end - ifs_position[0]) & 0xff) < 0x80) //if (ifs_position[0]<position_end) ; dist2cover_new=(position_end - ifs_position[0]) % 0xFF; if (dist2cover_new != dist2cover_old) dist2cover_old = dist2cover_new; #ifdef VERBOSE print_string("Distance to cover:"); print_hexbyte((position_end - ifs_position[0]) % 0xFF); send_EOL; #endif else print_string("Position reached!"); ifs_speed[0]=STOP; //STOP resetgrid(); status=SWEEP; break; case RABBIT: ultra_dist=(ifs_ultra1[0]+ifs_ultra2[0]) >> 1; #ifdef VERBOSE print_string("Fused ultrasonic distance:"); print_hexbyte(ultra_dist); #endif if (ultra_dist > 10) ifs_speed[0]=FORWARD_SLOW; #ifdef VERBOSE print_string("Straight ahead at RABBIT mode!!!\r\n"); #endif break; else ifs_speed[0]=STOP; resetgrid(); status=SWEEP; break; case MOVE_REVERSE: #ifdef VERBOSE print_string("Reversing..."); send_EOL; #endif ifs_speed[0]=REVERSE_SLOW; //MOVE BACKWARDS SLOWLY if (ifs_position[0]>position_end) ifs_speed[0]=STOP; //STOP resetgrid(); status=SWEEP; break; case ABORT_STOP: #ifdef VERBOSE print_string("Abort Stop!"); send_EOL; #endif ifs_speed[0]=STOP; //STOP
44
Research Report 45/2002 break;
4.3.3 TTP/A Slave: Position node (Pos)
#define GRADIENT 26 // 9.75TCKs = 1cm #define SHIFT 8 // => [cm] = (TCKs * 26) >> 8; ///////////////////////////////////////////////////////////////////////////// // HEADER FILES // ///////////////////////////////////////////////////////////////////////////// #include <io.h> #include <sig-avr.h> #include <interrupt.h> #include "ttpa.h" #include "avr_macros.h" #include "constants.h" ///////////////////////////////////////////////////////////////////////////// // GLOBAL VARIABLES // ///////////////////////////////////////////////////////////////////////////// extern uint8_t file10[]; ///////////////////////////////////////////////////////////////////////////// // INTERRUPT HANDLERS // ///////////////////////////////////////////////////////////////////////////// INTERRUPT(ttpa_task00) file10[2] = inp(TCNT0); file10[0] = (*((uint16_t*)(file10+2)) * GRADIENT + 0x80) >> SHIFT; INTERRUPT(SIG_OVERFLOW0) file10[3]++; ///////////////////////////////////////////////////////////////////////////// // FUNCTIONS // ///////////////////////////////////////////////////////////////////////////// void init_user() cbi(DDRD,4); //CONFIGURE T0 AS INPUT outp(0x00,TCNT0); //RESET COUNTER0 set_timerirq(BV(TOIE0)); //ENABLE COUNTER0 OVFL INT outp(0x07,TCCR0); //START COUNTER0; EXT. PIN T0 RISING cbi(PORTB,4); //LED OFF sbi(DDRB,4); //CONFIGURE LED PIN AS OUTPUT void ttpa_bgtask() uint32_t i; /* heart beat */ for(i=0; i<0x3FFFF; i++) sbi(PORTB,4); //LED ON for(i=0; i<0x1FFFF; i++) cbi(PORTB,4); //LED OFF for(i=0; i<0x1FFFF; i++) sbi(PORTB,4); //LED ON for(i=0; i<0x1FFFF; i++)
45
Research Report 45/2002 cbi(PORTB,4); //LED OFF
4.3.4 TTP/A Slave: Infrared sensor nodes (Infra_)
#define K_0 45 #define K_G 1575 #define LISTSIZE 4 #define MINRANGE 2 #define PREWAIT 0 //WAITING TO RECEIVE DATA FROM SENSOR #define RECEIVE_H 1 //RECEIVING DATA ... CLOCK HIGH #define RECEIVE_L 2 //RECEIVING DATA ... CLOCK LOW #define POSTWAIT 3 //WAIT AFTER RECEIVING DATA #define SDATA_LENGTH 8 //BIT LENGTH OF SENSOR DATA #define SDATA_POSTDELAY 20 //POST MEASUREMENT DELAY x0.1ms #define MS_01 0xA3 //TCNT0 COUNTS UPWARDS ... WE //NEED 0xFF-0x5C = 0xA3 #define MIN 10 //MINMUM POSSIBLE READING #define MAX 120 //MAXIMUM POSSIBLE READING uint8_t state_pinC0 = PREWAIT; //STATE OF THE SENSOR AND CLOCK uint8_t count_pinC0 = 0x00; //NUMBER OF BITS uint8_t sdata_pinC0 = 0x00; //SERIAL DATA RECEIVED uint8_t confidence = 0x00; //CONFIDENCE OF DATA uint16_t temp; //TEMPORARY REGISTER uint8_t *ifs_data; uint8_t *ifs_confidence; uint8_t data[LISTSIZE]; uint8_t i; void init_user (void) sei(); //ENABLE INTERRUPTS outp(0xFE,DDRC); //PORTC = oooooooi <o=out,i=in> outp(0xFD&inp(PORTC),PORTC); //PORTC = ------0- (CLOCKING) register uint8_t temp; //TEMPORARY REGISTER #if (TOIE0 == TOV0) // AT90LS4433 SPECIFIC ROUTINE asm volatile ( "ldi %0, %1" "\n\t" "or r15,%0" "\n\t" "out %2, r15" "\n\t" "out %3, r15" "\n\t" ::"d"(temp),"M"(BV(TOIE0)),"M"(TIFR),"M"(TIMSK)); #endif map_ifsbyte (ifs_data,file10,1,0); map_ifsbyte (ifs_confidence,file10,1,1); INTERRUPT (ttpa_task00) //INTERRUPT TO ENABLE TCNT0 if (state_pinC0==PREWAIT) outp(MS_01,TCNT0); //ADJUST COUNTER0 FOR 1ms INTERRUPT outp(BV(CS01),TCCR0); //RE-ENABLE TCNT0 INTERRUPT (SIG_OVERFLOW0) //TCNT0 OVERFLOW INTERRUPT HANDLER outp(MS_01,TCNT0); //ADJUST COUNTER0 FOR 1ms INTERRUPT switch (state_pinC0) case PREWAIT: if (bit_is_set(PINC,PINC0)) //SENSOR READY TO DELIVER MEASUREMENT outp(0x02|inp(PORTC),PORTC); //PORTC = ------1- (CLOCKING) START state_pinC0=RECEIVE_H; sdata_pinC0=0x00; count_pinC0=0x00; break; case RECEIVE_H: if (count_pinC0 == SDATA_LENGTH) //8 BITS READ FROM SENSOR state_pinC0=POSTWAIT;
46
Research Report 45/2002 temp = K_G / (sdata_pinC0 - K_0); sdata_pinC0 = temp & 0x00FF; confidence = 0x0F; if (sdata_pinC0 < MIN) sdata_pinC0=MIN; confidence = 0x00; if (sdata_pinC0 > MAX) sdata_pinC0=MAX; confidence = 0x00; for (i=0;i<(LISTSIZE-1);i++) data[i]=data[i+1]; data[LISTSIZE-1]=sdata_pinC0; for (i=0;i<(LISTSIZE-1);i++) if (data[i]>data[i+1]) if ((data[i]-data[i+1])>MINRANGE) sdata_pinC0=MAX; else if ((data[i+1]-data[i])>MINRANGE) sdata_pinC0=MAX; ifs_data[0]=sdata_pinC0; ifs_confidence[0]=confidence; else //MORE BITS TO READ FROM SENSOR outp(0xFD&inp(PORTC),PORTC); //PORTC = ------0- (CLOCKING) LOW state_pinC0=RECEIVE_L; break; case RECEIVE_L: outp(0x02|inp(PORTC),PORTC); //PORTC = ------1- (CLOCKING) HIGH state_pinC0=RECEIVE_H; sdata_pinC0 <<= 1; if (bit_is_set(PINC,PINC0)) sdata_pinC0 |= 0x01; count_pinC0++; break; case POSTWAIT: count_pinC0++; if (count_pinC0 > SDATA_POSTDELAY) //AFTER RECEIVING, WAIT 2ms outp(0xFD&inp(PORTC),PORTC); //PORTC = ------0- (CLOCKING) WAITING state_pinC0=PREWAIT; outp(0x00,TCCR0); //DISABLE TCNT0 break;
47
Research Report 45/2002
4.3.5 TTP/A Slave: Ultrasonic sensor nodes (Ultra_)
#define WAIT 0x00 #define OVER 0xFF #define CALIB_1 4 #define CALIB_2 1 uint8_t status=OVER; uint8_t echotime; uint8_t distance=0x00; uint8_t confidence; uint8_t usercounter=0x00; uint8_t *ifs_data; uint8_t *ifs_confidence; void init_user () outp(0xFF,DDRC); //SET PORTC TO OUTPUT outp(0x00,DDRD); //SET PORTD TO INPUT outp(0xFF,PORTD); //ENABLE INTERNAL PULL-UP outp(0x40,GIFR); outp(0x03|inp(MCUCR),MCUCR); //SET INT0 TO RISING EDGE DETECTION outp(0x40|inp(GIMSK),GIMSK); //ENABLE EXTERNAL INTERRUPT INT0 outp(0x05,TCCR0); //SET PRESCALER register uint8_t temp; //TEMPORARY REGISTER #if (TOIE0 == TOV0) asm volatile ( "ldi %0, %1" "\n\t" "or r15,%0" "\n\t" "out %2, r15" "\n\t" "out %3, r15" "\n\t" ::"d"(temp),"M"(BV(TOIE0)),"M"(TIFR),"M"(TIMSK)); #endif sei(); map_ifsbyte (ifs_data,file10,1,0); map_ifsbyte (ifs_confidence,file10,1,1); INTERRUPT (SIG_INTERRUPT0) if ((status==WAIT)&&(inp(TCNT0)>0x10)) echotime=inp(TCNT0); status=OVER; outp(0x00,PORTC); //DISABLE INIT distance=(echotime/CALIB_1)-CALIB_2; //CALIBRATE DISTANCE READING INTERRUPT (SIG_OVERFLOW0) if (status==WAIT) echotime=0xFF; status=OVER; outp(0x00,PORTC); //DISABLE INIT distance=0xFF; //SET DISTANCE TO INFINITY INTERRUPT (ttpa_task00) if (status==OVER) if (++usercounter==2) usercounter=0; confidence = 0x0F; ifs_data[0]=distance; ifs_confidence[0]=confidence; outp(0xFF,PORTC); //ENABLE INIT outp(0x00,TCNT0); //RESET THE COUNTER status=WAIT;
48
Research Report 45/2002 4.3.6 TTP/A Slave: Servo nodes (Servo_ and Steering)
#define SERVOPORT PORTC //ATLS4433 #define DDR_SERVOPORT DDRC #define HIGH 0xFF //SERVO PIN STATES #define LOW 0x00 #define CALIB_OFFSET -0x0A //ANGLE OFFSET CALIBRATION #define CALIB_GRADATION 0x78/0x78 //ANGLE GRADATION CALIBRATION #define MIN_ANGLE 48 //MINIMUM ANGLE #define MAX_ANGLE 133 //MAXIMUM ANGLE uint8_t pinstate = LOW; //SEMAPHORE TO INDICATE TASK EXECUTION uint8_t timeout = 0; //TCNT0 TIMEOUT uint8_t direction = 0x00; //DIRECTION OF THE SERVO uint8_t usercounter = 0x00; //USER COUNTER FOR FURTHER PRESCALING void init_user (void) sei(); //ENABLE INTERRUPTS register uint8_t temp; #if (TOIE0 == TOV0) asm volatile ( "ldi %0, %1" "\n\t" "or r15,%0" "\n\t" "out %2, r15" "\n\t" "out %3, r15" "\n\t" ::"d"(temp),"M"(BV(TOIE0)),"M"(TIFR),"M"(TIMSK)); #endif outp(0x03,TCCR0); //PRESCALE CLOCK/64 = 115.2 clocks/ms outp(0x00,TCNT0); //RESET COUNTER outp(0xFF,DDR_SERVOPORT); //SET SERVOPORT TO OUTPUT INTERRUPT (ttpa_task00) ifs_read_byte(&direction,0,IFS_ADDR(16,1,0)); //READ DIRECTION FROM IFS if (direction<MIN_ANGLE) direction=MIN_ANGLE; if (direction>MAX_ANGLE) direction=MAX_ANGLE; if (pinstate==LOW) //CONFIRMING PULSE READY STATUS timeout=CALIB_OFFSET+(direction*CALIB_GRADATION); //CALCULATE PULSE WIDTH TIMEOUT outp(timeout,TCNT0); //WRITE TIMEOUT outp(0x03,TCCR0); //PRESCALE CLOCK/64 outp(HIGH,SERVOPORT); //SET SERVOPORT TO HIGH pinstate=HIGH; //SET FLAG TO HIGH INTERRUPT (SIG_OVERFLOW0) switch (pinstate) case HIGH: outp(0x8C,TCNT0); //INITIATE 1ms TIMER outp(LOW,SERVOPORT); //SET SERVOPORT TO LOW pinstate=LOW; //SET FLAG TO LOW break; case LOW: if (++usercounter>9) //USERCOUNTER FOR N x 1ms outp(0x00,TCCR0); //STOP TIMER usercounter=0; //RESET USER COUNTER else outp(0x8C,TCNT0); //CONTINUE 1ms TIMER break;
49
Research Report 45/2002 4.3.7 TTP/A Slave: Digital control node (Speed)
#define FORWARD 0x93 #define FORWARD_SLOW 0x39 #define NEUTRAL 0xCA #define STOP 0x0F #define REVERSE_SLOW 0xC5 #define REVERSE 0x36 #define FORWARD_TIMEOUT 0x8C #define FORWARD_SLOW_TIMEOUT 0x5A//TESTED #define NEUTRAL_TIMEOUT 0x52 #define STOP_TIMEOUT 0x3B #define REVERSE_SLOW_TIMEOUT 0x2A//TESTED #define REVERSE_TIMEOUT 0x19 #define MOTORPORT PORTC//ATLS4433 #define DDR_MOTORPORT DDRC ///////////////////////////////////////////////////////////////////////////// // HEADER FILES // ///////////////////////////////////////////////////////////////////////////// #include <io.h> #include <sig-avr.h> #include <interrupt.h> #include "ttpa.h" #include "avr_macros.h" #include "constants.h" ///////////////////////////////////////////////////////////////////////////// // GLOBAL VARIABLES // ///////////////////////////////////////////////////////////////////////////// extern uint8_t file10[]; ///////////////////////////////////////////////////////////////////////////// // FUNCTIONS // ///////////////////////////////////////////////////////////////////////////// void init_user (void) outp(0x00,TCCR0); //STOP TIMER0 set_timerirq(BV(TOIE0)); //ENABLE TIMER0 OVFL INT outp(0xFF,DDR_MOTORPORT); //CONFIGURE MOTORPORT AS OUTPUT outp(0x00,MOTORPORT); //SET MOTORPORT TO LOW cbi(PORTB,4); //LED OFF sbi(DDRB,4); //CONFIGURE LED PIN AS OUTPUT ///////////////////////////////////////////////////////////////////////////// // INTERRUPT HANDLERS // ///////////////////////////////////////////////////////////////////////////// INTERRUPT (ttpa_task00) register uint8_t speed; speed = file10[0]; switch(speed) case FORWARD: outp(FORWARD_TIMEOUT,TCNT0); //SET TIMEOUT break; case FORWARD_SLOW: outp(FORWARD_SLOW_TIMEOUT,TCNT0); //SET TIMEOUT break; case REVERSE_SLOW: outp(REVERSE_SLOW_TIMEOUT,TCNT0); //SET TIMEOUT break; case REVERSE: outp(REVERSE_TIMEOUT,TCNT0); //SET TIMEOUT
50
Research Report 45/2002 break; case NEUTRAL: outp(NEUTRAL_TIMEOUT,TCNT0); //SET TIMEOUT break; case STOP: default: outp(STOP_TIMEOUT,TCNT0); //SET TIMEOUT break; outp(0xFF,MOTORPORT); //SET MOTORPORT TO HIGH outp(0x03,TCCR0); //START TCNT0; CLK/64 INTERRUPT (SIG_OVERFLOW0) outp(0x00,MOTORPORT); //SET MOTORPORT TO LOW outp(0x00,TCCR0); //STOP TCNT0 void ttpa_bgtask(void) uint32_t i; /* heart beat */ for(i=0; i<0x3FFFF; i++) sbi(PORTB,4); //LED ON for(i=0; i<0x1FFFF; i++) cbi(PORTB,4); //LED OFF for(i=0; i<0x1FFFF; i++) sbi(PORTB,4); //LED ON for(i=0; i<0x1FFFF; i++) cbi(PORTB,4); //LED OFF
51
Research Report 45/2002
5 Smart Car Monitoring utilities
5.1 Monitor
It is difficult to debug programs for embedded systems. The lack of a screen and keyboard makes it difficult to monitor such systems. Therefore, a debugging tool with a graphical interface the “IFS Monitor” [10] and a communication routine for the TTP/A nodes have been developed to gain access to their Interface File System (IFS). If the monitoring tool is connected to a master node, the user has furthermore full read/write access to all nodes on the network via the maser/slave rounds. As described in section 3.3.1 the master node is able to act as a gateway to the Smart Car’s TTP/A network via its RS232-interface. So it is possible to gain access and monitor all nodes on the Smart Car. The PC-sided software acts itself also as a gateway providing a CORBA interface that can be used by other monitoring and configuration tools.
Figure 37: Screenshot of the Monitoring-Tool [10]
5.2 Gridiculous
One of the key limitations of the full-fledged monitoring utility described above is that it can be quite slow due to the bi-directional communication whereby the monitoring utility has to request any data it requires. In view of these limitations, a much simpler grid monitoring utility was developed, where the navigation node sends a continuous uni-directional datastream to the PC. The datastream is a row-major sequence of bytes from the grid starting from the top-left corner through to the bottom-right corner. For simplicity, the navigation node transmits a tilde character (‘~’ = 0x7E) to indicate the beginning of the grid. Though not foolproof, it was found that 0x7E occurred quite rarely in the current implementation of the Smart Car.
52
Research Report 45/2002 Furthermore, even if it does occur, this application is not data critical and can hence tolerate infrequent errors with minimal impact on the grid’s interpretation.
Figure 38: Gridiculous grid monitoring utility
53
Research Report 45/2002
6 Performance evaluation
6.1 Hardware performance evaluation
6.1.1 Sharp GP2D02 Infrared Sensors: Accuracy
The TTP/A Smart Car uses Sharp GP2D02 infrared sensors which evaluate relative distance from the sensor to an object by measuring the angle of reflection of a fixed-frequency infrared emission. Two key limitations of these sensors arise from the very design principle on which they are based. Firstly, the fact that they measure the angle of reflection to evaluate distance means that the sensors have factory-limited range. Secondly, the fact that the infrared emissions are of a fixed frequency implies that they are quite susceptible to stray emissions from neighboring sensors operating at the same frequency.
6.1.2 Polaroid 6500 series Ultrasonic ranging Sensors: High voltage interference
The Polaroid 6500 series Ultrasonic ranging sensors’ connection cables transmit signals in the order of 200V at 50kHz bursts. In a series of tests, it was found that for improperly grounded lines connected to devices with internal pull-up resistors (such as the Futaba S3003 servo motor control signal line), the current induced from the ultrasound lines caused these devices to behave as if they were receiving real signals at 50kHz. In the case of the Futaba S3003, the servo motor would swing to the extreme clockwise position, as if it were receiving a pulse-width modulated signal of duration longer than 2.0ms.
6.2 Software performance evaluation
6.2.1 Grid generation
In its preliminary version, the smart car was tested with a series of simple obstacles and made to trace paths through an obstacle course. Here is a sample of such an instance and the grid that was generated as a result.
Figure 39: Sample Grid setup in the laboratory and corresponding Smart Car grid evaluation
54
Research Report 45/2002
7 Summary
The Smart Car introduced in this report has been developed and built at the Institut für Technische Informatik at the Technische Universität in Vienna to serve the purpose to evaluate and demonstrate the feasibility of using a TTP/A cluster [7] in a real-life application. On the basis of an off-the-shelf model radio controlled car and by adding sensors, actuators and microcontrollers connected to a TTP/A network of smart transducers [12], a mobile autonomous robot has been developed, which is able to perceive its surroundings through different types of distance sensors, can move through its environment by controlling an electric motor and a steering servo, and is able to recognize its own movement with a shaft encoder mounted on a small wheel. Additional to the sensor and actuator nodes a control node for navigation is connected to the network. It collects sensor data sent by infrared and ultrasonic sensor nodes and generates a map of the immediate environment of the Smart Car. There are three infrared sensors mounted on servos at the front of the robot. With the help of the servos each of the infrared sensors can be moved in a scan range. The scan ranges of the three infrared sensors overlap each other which results in redundant sensor data in these areas allowing the use of fault-tolerant algorithms - particularly the robust certainty grid algorithm [12] developed at the institute. The navigation node uses the map for path planning and sends accordingly commands to the drive and steering nodes. The TTP/A master node switches between two Round Sequences (ROSE). One of them is the Rabbit mode which is active as long as there are no obstacles detected in front of the Smart car and, therefore, the car can drive faster. The second mode is the Turtle-mode which is used in case of obstacles in front of the car which requires the use of all sensors for navigation decisions. Another task of the master is to work as a gateway to allow configuration and monitoring tools to gain access to the TTP/A network. Therefore, the IFS-Monitor developed at the institute was used. By realizing the Smart Car project it could be shown that a real-life application with the requirements of avoiding static obstacles, calculating paths and controlling movements could be successfully handled with a TTP/A network of smart transducers. Furthermore, the use of the Sharp infrared sensors has shown that it is - among other things - advantageous to equip every sensor with its own microcontroller which forms a smart transducer. The fact that values provided by the infrared sensors underlie noice-induced errors demands a filtering algorithm, the non-linear sensor output has to be linearised and calibration data have to be considered in the post processing because the sensor outputs of the three sensors deviate in relation to each other. All those tasks are covered by the infrared sensor smart transducers which deliver ready-to-use data. A simple grid-display solution has been built since using the monitoring tool IFS-Monitor turned out to be too slow because of the required bidirectional communication during the master/slave rounds. A tool named Gridiculous has been developed which is running on a PC directly connected to the navigation node that sends only uni-directional data allowing the display of the grid map in real-time. If there are further questions regarding TTP/A or Sensor Fusion [12] please have a look at the resources provided at the homepage of the Real-Time Systems Group at the Vienna University of Technology1.
1 http://www.vmars.tuwien.ac.at
55
Research Report 45/2002
8 Bibliography
1 Atmel Corporation (2002), “AT90S4433 and AT90LS4433 Documentation (Rev. 1042F-AVR-02/02)”, Atmel Corporation [http://www.atmel.com]
2 Atmel Corporation (2002), “ATMega128 and ATMega128L Preliminary Documentation (Rev. 2467E-AVR-05/02)”, Atmel Corporation [http://www.atmel.com]
3 Elmenreich, W., Schneider, L., Kirner, R. (2001), “A robust certainty grid algorithm for robotic vision”, 6th IEEE International Conference on Intelligent Engineering Systems (INES), May 2001, Opajita, Croatia [http://www.vmars.tuwien.ac.at]
4 Trödhandl, C. (2002), “Master’s Thesis: Architectural requirements for TTP/A nodes”, Institut für Technische Informatik der Technischen Universität Wien, Vienna [http://www.vmars.tuwien.ac.at]
5 Elmenreich, W., Haidinger, W., Kopetz, H., Losert, T., Obermaisser, R., Paulitsch, M., Trödhandl, C. (2002), “A Smart Sensor LIF Case Study: Autonomous Mobile Robot”, DSoS Project – Deliverable PCE3, Institut für Technische Informatik der Technischen Universität Wien, Vienna [http://www.vmars.tuwien.ac.at]
6 Elmenreich, W., Haidinger, W., Kopetz, H. (2001), “Interface Design for Smart Transducers”, IEEE Instrumentation and Measurement Technology Conference (IMTC), Budapest, Hungary, May 2001 [http://www.vmars.tuwien.ac.at]
7 Kopetz, H., Bauer, G. (2002), “The Time Triggered Architecture”, Proceedings of the IEEE special issue on Modeling and Design of Embedded Software, October 2002
8 Atmel Corporation (2001), “AT90S8515 Documentation (Rev. 0841G-09/01)”, Atmel Corporation [http://www.atmel.com]
9 Atmel Corporation (2003), “AT90S4433 Documentation (Rev. 1042H–AVR–04/03)”, Atmel Corporation [http://www.atmel.com]
10 Elmenreich, W., Haidinger, W., Kirner, R., Losert, T., Obermaisser, R., Trödhandl, C. (2002) „TTP/A Smart Transducer Programming - A Beginner's Guide“ , Research Report 33/2002, 2002 [http://www.vmars.tuwien.ac.at]
11 Csisinko, M., (2004), “Smart car simulation environment”, Informatikpraktikum II, June 2004 [http://www.vmars.tuwien.ac.at]
12 Elmenreich, W., (2002), “Sensor Fusion in Time-Triggered Systems”, PhD Thesis, October 2002 [http://www.vmars.tuwien.ac.at]
56
Research Report 45/2002
9 Appendix
9.1 Sharp GP2D02 Infrared sensor calibration: Raw data
Sensor1 kg = 67 ko = 1765 Sensor2 kg = 46 ko = 1320 Sensor3 kg = 45 ko = 1375
readings and calculations (independent without interference from other sensors) sensor1 sensor2 sensor3
actual distance
(cm) test1 test2 mean d() error test1 test2 mean d() error test1 test2 mean d() error 10 229 229 229 11 9% 187 187 187 10 0% 209 209 209 10 4% 15 179 180 180 16 5% 143 144 144 15 1% 152 152 152 15 2% 20 149 149 149 22 8% 124 124 124 19 4% 126 126 126 19 3% 25 134 134 134 26 5% 110 110 110 24 4% 109 109 109 25 2% 30 122 122 122 32 7% 101 100 101 29 3% 98 98 98 30 1% 35 115 115 115 37 5% 93 93 93 35 1% 90 90 90 35 0% 40 108 109 109 43 6% 88 88 88 40 0% 84 84 84 40 1% 45 104 105 105 47 5% 84 84 84 46 1% 80 80 80 45 0% 50 101 100 101 53 5% 81 81 81 51 2% 76 77 77 50 0% 55 98 98 98 57 4% 79 78 79 56 2% 74 74 74 54 1% 60 96 96 96 61 1% 76 77 77 61 2% 71 71 71 61 1% 65 94 95 95 64 1% 75 74 75 68 4% 69 69 69 66 1% 70 93 92 93 69 1% 73 74 74 71 2% 68 68 68 68 2% 75 91 91 91 74 2% 72 73 73 75 1% 66 66 66 75 0% 80 90 91 91 75 6% 71 72 72 80 0% 65 65 65 79 2% 85 89 90 90 78 8% 70 71 71 85 0% 64 63 64 85 0% 90 89 89 89 80 11% 70 70 70 88 2% 63 62 63 90 0% 95 88 87 88 86 9% 70 69 70 91 4% 61 62 62 95 0%
100 89 89 89 80 20% 69 69 69 94 6% 59 61 60 105 5% 105 85 84 85 101 4% 69 68 69 98 7% 59 59 59 113 7% 110 83 84 84 107 3% 68 69 69 98 11% 58 59 59 117 6%
6% 3% 2%
9.2 Polaroid 6500 series Ultrasonic ranging sensor calibration: Raw data
Sensor1 C1 = 4, C2 = 1 Sensor2 C1 = 4, C2 = 1
57
Research Report 45/2002
readings and calculations sensor1 sensor2
actual distance (10 cm) test1 test2 mean d() error test1 test2 mean d() Error
2.5 19 21 20 4 60% 19 21 20 4 60%5.0 22 22 22 5 10% 22 22 22 5 10%7.5 32 33 33 7 5% 32 33 33 7 5%
10.0 42 43 43 10 4% 42 43 43 10 4%12.5 53 54 54 12 1% 53 53 53 12 2%15.0 63 64 64 15 1% 63 64 64 15 1%17.5 74 74 74 18 0% 74 74 74 18 0%20.0 84 84 84 20 0% 84 84 84 20 0%22.5 94 94 94 23 0% 94 94 94 23 0%25.0 103 103 103 25 1% 103 103 103 25 1%
8% 8%
9.3 Sharp GP2D02 Infrared sensor Infinity detection: Raw data
Standard deviation Sensor 1 (Infinity) = 7.98 Sensor 2 (Infinity) = 4.28 Sensor 3 (Infinity) = 4.09 Random (100) = 0.23
Frequency Frequency Frequency Frequency
8bit
Rea
ding
Sen
sor 1
(In
finity
) S
enso
r 2
(Infin
ity)
Sen
sor 3
(In
finity
) R
ando
m
(100
)
8bit
Rea
ding
Sen
sor 1
(In
finity
) S
enso
r 2
(Infin
ity)
Sen
sor 3
(In
finity
) R
ando
m
(100
)
8bit
Rea
ding
Sen
sor 1
(In
finity
) S
enso
r 2
(Infin
ity)
Sen
sor 3
(In
finity
) R
ando
m
(100
)
8bit
Rea
ding
Sen
sor 1
(In
finity
) S
enso
r 2
(Infin
ity)
Sen
sor 3
(In
finity
) R
ando
m
(100
) 0 12 0 1 0 64 25 6 6 0 128 0 0 0 0 192 0 0 0 0
1 0 1 1 0 65 0 0 0 0 129 0 0 0 0 193 0 0 0 0
2 0 1 0 0 66 0 0 0 0 130 0 0 0 0 194 0 0 0 0
3 0 0 0 0 67 0 0 0 0 131 0 0 0 0 195 0 0 0 0
4 0 0 0 0 68 0 0 0 0 132 0 0 0 0 196 0 0 0 0
5 0 1 0 0 69 0 0 0 0 133 0 0 0 0 197 0 0 0 0
6 0 0 0 0 70 0 0 0 0 134 0 0 0 0 198 0 0 0 0
7 0 0 0 0 71 0 0 0 0 135 0 0 0 0 199 0 0 0 0
8 0 0 0 0 72 0 0 0 0 136 0 0 0 0 200 0 0 0 0
9 0 0 0 0 73 0 0 0 0 137 0 0 0 0 201 0 0 0 0
10 0 0 0 0 74 0 0 0 0 138 0 0 0 0 202 0 0 0 0
11 0 0 0 0 75 0 0 0 0 139 0 0 0 0 203 0 0 0 0
12 0 0 0 0 76 0 0 0 0 140 0 0 0 0 204 0 0 0 0
13 0 0 0 0 77 0 0 0 0 141 0 0 0 0 205 0 0 0 0
14 0 0 0 0 78 0 0 0 0 142 0 0 0 0 206 0 0 0 0
15 0 0 0 0 79 0 0 0 0 143 0 0 0 0 207 0 0 0 0
16 0 0 0 0 80 0 0 0 0 144 0 0 0 0 208 0 0 0 0
17 0 0 0 0 81 0 0 0 0 145 0 0 0 0 209 0 0 0 0
18 0 0 0 0 82 0 0 0 0 146 0 0 0 0 210 0 0 0 0
19 0 0 0 0 83 0 0 0 0 147 0 0 0 0 211 0 0 0 0
20 0 0 0 0 84 0 0 0 0 148 0 0 0 0 212 0 0 0 0
58
Research Report 45/2002
21 0 0 0 0 85 0 0 0 0 149 0 0 0 0 213 0 0 0 0
22 0 0 0 0 86 1 0 0 0 150 0 0 0 0 214 0 0 0 0
23 0 0 0 0 87 0 0 0 0 151 0 0 0 0 215 0 0 0 0
24 0 0 0 0 88 2 1 0 0 152 0 0 0 0 216 0 0 0 0
25 0 0 0 0 89 9 0 0 0 153 0 0 0 0 217 0 0 0 0
26 0 0 0 0 90 8 2 0 0 154 0 0 0 0 218 0 0 0 0
27 0 0 0 0 91 21 2 1 0 155 0 0 0 0 219 0 0 0 0
28 0 0 0 0 92 25 3 4 0 156 0 0 0 0 220 0 0 0 0
29 0 0 0 0 93 44 12 9 0 157 0 0 0 0 221 0 0 0 0
30 0 0 0 0 94 60 17 22 0 158 0 0 0 0 222 0 0 0 0
31 0 0 0 0 95 85 39 49 0 159 0 0 0 0 223 0 0 0 0
32 0 0 0 0 96 167 268 194 16 160 0 0 0 0 224 0 0 0 0
33 0 0 0 0 97 137 180 173 0 161 0 0 0 0 225 0 0 0 0
34 0 0 0 0 98 204 291 255 0 162 0 0 0 0 226 0 0 0 0
35 0 0 0 0 99 225 370 358 0 163 0 0 0 0 227 0 0 0 0
36 0 0 0 0 100 300 540 491 4983 164 0 0 0 0 228 0 0 0 0
37 0 0 0 0 101 328 612 560 0 165 0 0 0 0 229 0 0 0 0
38 0 0 0 0 102 333 605 638 0 166 0 0 0 0 230 0 0 0 0
39 0 0 0 0 103 364 557 585 0 167 0 0 0 0 231 0 0 0 0
40 0 0 0 0 104 368 452 510 0 168 0 0 0 0 232 0 0 0 0
41 0 0 0 0 105 357 399 414 0 169 0 0 0 0 233 0 0 0 0
42 0 0 0 0 106 338 274 288 0 170 0 0 0 0 234 0 0 0 0
43 0 0 0 0 107 299 188 193 0 171 0 0 0 0 235 0 0 0 0
44 0 0 0 0 108 234 84 122 0 172 0 0 0 0 236 0 0 0 0
45 0 0 0 0 109 234 38 63 0 173 0 0 0 0 237 0 0 0 0
46 0 0 0 0 110 188 35 34 0 174 0 0 0 0 238 0 0 0 0
47 0 0 0 0 111 162 18 20 0 175 0 0 0 0 239 0 0 0 0
48 0 0 0 0 112 135 2 5 0 176 0 0 0 0 240 0 0 0 0
49 0 0 0 0 113 98 1 1 0 177 0 0 0 0 241 0 0 0 0
50 0 0 0 0 114 64 0 1 0 178 0 0 0 0 242 0 0 0 0
51 0 0 0 0 115 70 0 0 0 179 0 0 0 0 243 0 0 0 0
52 0 0 0 0 116 45 0 1 0 180 0 0 0 0 244 0 0 0 0
53 0 0 0 0 117 15 0 0 0 181 0 0 0 0 245 0 0 0 0
54 0 0 0 0 118 16 0 0 0 182 0 0 0 0 246 0 0 0 0
55 0 0 0 0 119 5 0 0 0 183 0 0 0 0 247 0 0 0 0
56 0 0 0 0 120 9 0 0 0 184 0 0 0 0 248 0 0 0 0
57 0 0 0 0 121 4 0 0 0 185 0 0 0 0 249 0 0 0 0
58 0 0 0 0 122 3 0 0 0 186 0 0 0 0 250 0 0 0 0
59 0 0 0 0 123 1 0 0 0 187 0 0 0 0 251 0 0 0 0
60 0 0 0 0 124 3 0 0 0 188 0 0 0 0 252 0 0 0 0
61 0 0 0 0 125 0 0 0 0 189 0 0 0 0 253 0 0 0 0
62 0 0 0 0 126 0 0 0 0 190 0 0 0 0 254 0 0 0 0
63 0 0 0 0 127 0 0 0 0 191 0 0 0 0 255 0 0 0 0
59
