documentation of the smart car demonstrator
TRANSCRIPT
Documentation of the Smart Car Demonstrator
Albert Dias
Institut für Technische Informatik Technische Universität Wien, Vienna, Austria
August 26, 2002
Research Report 45/2002
2
Research Report 45/2002
3
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.2.1 Robust certainty grid algorithm .............................................................................. 13 2.3.2.2 Simplified grid algorithm......................................................................................... 13
2.3.3 Object recognition.................................................................................................................... 13 2.3.4 Grid updating .......................................................................................................................... 14
2.4 Navigation decisions............................................................................................................................ 14 2.4.1 Path planning........................................................................................................................... 14 2.4.2 Path assessment ....................................................................................................................... 16 2.4.3 Decision making ...................................................................................................................... 16 2.4.4 Aborting navigation ................................................................................................................ 17
2.5 Calibration and Filtering...................................................................................................................... 18 2.5.1 Sharp GP2D02 Infrared sensor calibration............................................................................. 18 2.5.2 Sharp GP2D02 Infrared sensor: Infinity detection filtering .................................................. 19 2.5.3 Polaroid 6500 series Ultrasonic ranging sensor calibration .................................................. 20 2.5.4 Futaba S3003 Servo motor calibration.................................................................................... 21
3 Smart Car Hardware architecture.................................................................................................................. 22
3.1 Mechanical ............................................................................................................................................ 22 3.1.1 Smart Car body........................................................................................................................ 22 3.1.2 Steering and drive.................................................................................................................... 22
3.2 Electrical................................................................................................................................................ 22 3.2.1 Power supply system .............................................................................................................. 22 3.2.2 TTP/A bus ............................................................................................................................... 22 3.2.3 Sharp GP2D02 Infrared sensors.............................................................................................. 23 3.2.4 Futaba S3003 Analog Servo..................................................................................................... 23 3.2.5 Polaroid 6500 series Ultrasonic ranging................................................................................. 24 3.2.6 Euroturn Shaft Position Encoder ............................................................................................ 25
3.3 Microprocessors.................................................................................................................................... 25 3.3.1 Master Node............................................................................................................................. 25 3.3.2 ATMega128 Slave Node.......................................................................................................... 25 3.3.3 AT90S4433 Slave Node............................................................................................................ 26
4 Smart Car Software architecture .................................................................................................................... 27
4.1 General software architecture of the Smart Car ................................................................................. 27 4.1.1 Distributed processing ............................................................................................................ 27 4.1.2 Communication ....................................................................................................................... 27
4.2 General software architecture of a TTP/A node................................................................................ 27 4.3 Specific software architecture of TTP/A nodes in the Smart Car ..................................................... 28
Research Report 45/2002
4
4.3.1 TTP/A Master: Master node (Master) ................................................................................... 28 4.3.2 TTP/A Slave: Navigation node (Nav) ................................................................................... 28 4.3.3 TTP/A Slave: Position node (Pos).......................................................................................... 28 4.3.4 TTP/A Slave: Infrared sensor nodes (Infra_) ........................................................................ 28 4.3.5 TTP/A Slave: Ultrasonic sensor nodes (Ultra_) .................................................................... 30 4.3.6 TTP/A Slave: Light sensor node (Lightsens)......................................................................... 31 4.3.7 TTP/A Slave: LED light emitting nodes (Light_).................................................................. 31 4.3.8 TTP/A Slave: Servo nodes (Servo_ and Steering)................................................................. 31 4.3.9 TTP/A Slave: Digital control node (Speed)........................................................................... 32
5 Smart Car Monitoring utilities ....................................................................................................................... 33
5.1 Monitor ................................................................................................................................................. 33 5.2 Gridiculous ........................................................................................................................................... 33
6 Performance evaluation.................................................................................................................................. 34
6.1 Hardware performance evaluation..................................................................................................... 34 6.1.1 Sharp GP2D02 Infrared Sensors: Accuracy ........................................................................... 34 6.1.2 Polaroid 6500 series Ultrasonic ranging Sensors: High voltage interference ...................... 34
6.2 Software performance evaluation....................................................................................................... 34 6.2.1 Grid generation ....................................................................................................................... 34
7 Conclusion....................................................................................................................................................... 35
8 Bibliography.................................................................................................................................................... 36
9 Appendix......................................................................................................................................................... 37
9.1 Sharp GP2D02 Infrared sensor calibration: Raw data ....................................................................... 37 9.2 Polaroid 6500 series Ultrasonic ranging sensor calibration: Raw data ............................................. 37 9.3 Sharp GP2D02 Infrared sensor Infinity detection: Raw data ............................................................ 38
Research Report 45/2002
5
1 Introduction
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. 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. 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.
Research Report 45/2002
6
TTP/A
MechanicalHardware
Electrical/Electro-
MechanicalHardware
ElectricalHardware
Software
Non-TTP/A
Figure 2: Smart Car categorization and scope of TTP/A
Research Report 45/2002
7
2 Theory
2.1 Time Triggered Protocol for SAE Class A applications (TTP/A)
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)
Research Report 45/2002
8
Figure 6: Smart Car RODL Configuration Table
Research Report 45/2002
9
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 tested version of the Smart Car, a dummy master was used, which simulated 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.
Research Report 45/2002
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
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
Research Report 45/2002
11
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)));
( )pointpo 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 po int pointcell 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.
Research Report 45/2002
12
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
Research Report 45/2002
13
2.3.2 Grid entry algorithms
2.3.2.1 Robust certainty grid algorithm
The robust certainty grid algorithm 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. It works with at least 3 sensors, 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’.
Research Report 45/2002
14
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 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.
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.
Research Report 45/2002
15
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 13: 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. typedef struct smartline__ uint8_t start; uint8_t length; smartline_; smartline_ direction0[8] = 0, 0, //SMARTLINE 0 0, 0, //SMARTLINE 1
Research Report 45/2002
16
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 14: 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 ;
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.
Research Report 45/2002
17
n
i 0gridweight[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 15: 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.
Research Report 45/2002
18
Figure 16: 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
Research Report 45/2002
19
Sensor 1
Sensor 2Sensor 3
0
50
100
150
200
250
0 20 40 60 80 100 120
Actual Distance (cm)
Sen
sor R
eadi
ng (8
-bit)
Figure 17: 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];
Research Report 45/2002
20
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
50
100
150
200
250
300
350
400
0 50 100 150 200 250
8-bit sensor reading
frequ
ency
0
100
200
300
400
500
600
700
0 50 100 150 200 250
8-bit sensor reading
freq
uenc
y
0
100
200
300
400
500
600
700
0 50 100 150 200 250
8-bit sensor reading
freq
uenc
y
0
1000
2000
3000
4000
5000
6000
0 50 100 150 200 250
8-bit sensor reading
freq
uenc
y
Figure 18: 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
Research Report 45/2002
21
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
Actual Distance (dm)
Sen
sor R
eadi
ng (8
-bit)
Figure 19: 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 20: 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.
Research Report 45/2002
22
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
3.1.2 Steering and drive
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. 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 21: 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. The pull-up resistor acts in a wired-and configuration, so that if one or more devices try to communicate simultaneously, the resulting data on the bus will be the wired-and of the output of each of the devices.
Research Report 45/2002
23
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 22: Connection diagram for the Sharp GP2D02 Infrared sensor
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 23: 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.
Research Report 45/2002
24
1.0 ~ 2.0 ms ~ 2.0 ms
Servo active Inter pulse gapServo inactive
CONTROL
Figure 24: 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.
4.7kΩ
Figure 25: 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 26: Polaroid 6500 series timing diagram for Init and Echo
Research Report 45/2002
25
3.2.6 Euroturn Shaft Position Encoder
Clockwise(A leads B)
No rotation
A
B
Counter-clockwise(B leads A)
Figure 27:Euroturn shaft position encoder timing diagram for channels A and B
3.3 Microprocessors
3.3.1 Master Node
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 on the ATMega128, 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 28: ATMega128 Slave Node size comparison
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 upto 8MHz (factory-preset to 1MHz). As a result, the
Research Report 45/2002
26
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 29: Programming the ATMega128’s fuses using Atmel AVR Studio 4
3.3.3 AT90S4433 Slave Node
Figure 30: AT90S4433 Slave Node size comparison
Research Report 45/2002
27
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 31: Distributed processing on the TTP/A bus
4.1.2 Communication
4.2 General software architecture of a TTP/A node
Background Task
TTP/A Task
Interrupt Routines
SlaveCode
Slave Node Application
TTP/AProtocolSoftware
UART
ReservedI/O
TTP/A Commn.
InterruptHandler
ExternalI/O
ProcessorHardware
M/SHandler
TTP/A IFSFilesystem
Figure 32: Software architecture of a TTP/A node
Research Report 45/2002
28
4.3 Specific software architecture of TTP/A nodes in the Smart Car
4.3.1 TTP/A Master: Master node (Master)
4.3.2 TTP/A Slave: Navigation node (Nav)
4.3.3 TTP/A Slave: Position node (Pos)
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
Research Report 45/2002
29
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; 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;
Research Report 45/2002
30
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;
Research Report 45/2002
31
4.3.6 TTP/A Slave: Light sensor node (Lightsens)
4.3.7 TTP/A Slave: LED light emitting nodes (Light_)
4.3.8 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
Research Report 45/2002
32
else outp(0x8C,TCNT0); //CONTINUE 1ms TIMER break;
4.3.9 TTP/A Slave: Digital control node (Speed)
Research Report 45/2002
33
5 Smart Car Monitoring utilities
5.1 Monitor
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. 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 33: Gridiculous grid monitoring utility
Research Report 45/2002
34
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 34: Sample Grid setup in the laboratory and corresponding Smart Car grid evaluation
Research Report 45/2002
35
7 Conclusion
Research Report 45/2002
36
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
Research Report 45/2002
37
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
Research Report 45/2002
38
C2 = 1
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
Sens
or 1
(In
finity
) Se
nsor
2
(Infin
ity)
Sens
or 3
(In
finity
) R
ando
m
(100
)
8bit
Rea
ding
Sens
or 1
(In
finity
) Se
nsor
2
(Infin
ity)
Sens
or 3
(In
finity
) R
ando
m
(100
)
8bit
Rea
ding
Sens
or 1
(In
finity
) Se
nsor
2
(Infin
ity)
Sens
or 3
(In
finity
) R
ando
m
(100
)
8bit
Rea
ding
Sens
or 1
(In
finity
) Se
nsor
2
(Infin
ity)
Sens
or 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
Research Report 45/2002
39
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
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