Statistics
| Revision:

root / trunk / code / projects / traffic_navigation / mapping.c @ 1987

History | View | Annotate | Download (4.65 KB)

1
#include "stdlib.h"
2
#include "mapping.h"
3
#include "lineDrive.h"
4
#include <dragonfly_lib.h>
5
#include <wl_basic.h>
6

    
7
//The last intersection that we encountered
8
int lastInt;
9

    
10
/* This array holds all of the intersections that are represented in the graph
11
 * after its creation, the graph is transmitted wirelessly */
12
node intersections[NUM_FEATURES]; 
13

    
14

    
15

    
16
/* This is run only once at the beginning of mapping when the robot is
17
 * placed somewhere randomly on the map in the middle of the road*/
18
char driveToNextInt(){
19
        char barcode; 
20

    
21
        /* Keep driving until we see a barcode */
22
        do {
23
                barcode = (char) doDrive(200);
24
        } 
25
        while(barcode < 0); /* Condition codes are all neg. */
26

    
27
        return barcode;
28
}
29

    
30

    
31

    
32
/* 
33
 * Traverses the map using DFS 
34
 * Returns 0 if all of the intersections in the database were seen
35
 * 1 otherwise 
36
 */
37
char createEdge(edge* newEdge, int type, int direction){
38
        char barcode;
39
        char time;
40

    
41
        rtc_init(SIXTEENTH_SECOND, NULL);
42
        rtc_reset();
43

    
44
        turn(type, direction);
45

    
46
        /* Keep driving until we see a barcode */
47
        do {
48
                barcode = (char) doDrive(200);
49
        }
50
        while(barcode < 0); /* Condition codes are all neg. */
51

    
52
        time = rtc_get();
53

    
54
        newEdge->to = barcode;
55
        newEdge->dist = time;
56

    
57
        return barcode;
58
}
59

    
60
/* This function performs mapping */
61
int createMap(){
62

    
63
        char seen[NUM_FEATURES];                /* Have we been at this intersection before*/
64
        char seenout[NUM_FEATURES];                /* Have we seen the outoging edges of this intersection? */
65

    
66
        char seenall[NUM_FEATURES];     /* This is how the array should look like once we have
67
                                         * seen everything */
68

    
69
        int outEdges, currInt, chosenDir, i;
70

    
71
        /* Initialize the graph to all zeros */
72
        initGraph(seen, seenout);
73

    
74
        /* When we see all the outgoing edges for the intersections, 
75
         * seenout will look as follows */
76
        for(i=0; i<NUM_FEATURES; i++) 
77
                seenall[i] = 1;
78

    
79
        /* First drives to the nearest intersection */
80
        char barcode = driveToNextInt();
81

    
82

    
83
        /* Randomly traverses the graph until all edges are seen */
84
        while(seenout !=  seenall)
85
        {
86
                currInt = getIntersectNum(barcode);
87
                seen[currInt] = 1;
88

    
89
                /* Get the number of outgoing edges */
90
                outEdges = getNumOut(getIntersectType(currInt));
91

    
92
                /* Assign the number of outgoing edges for the current int */ 
93
                intersections[currInt].numOut = outEdges;
94

    
95
                /* Randomly choose an outgoing edge that we have not seen to go down 
96
                 * if no such edge exists */
97
                chosenDir = rand() % outEdges;
98

    
99
                /* We have not seen all the outgoing edges for the intersection we are at */
100
                if(!seenout[currInt]){
101
                        intersections[currInt].outSeen++;
102
                        /* We have finished seeing all outgoing edges of the intersection */
103
                        if(outEdges == intersections[currInt].outSeen){
104
                                seenout[currInt] = 1;
105
                        }
106
                }
107
                
108
                /* Traverses edge, stores information in struct */
109
                //TODO: Does the chosendDir, correspond with what is in Priya's Code?
110
                //A number between 0 and the number of outgoing edges ?
111
                createEdge(&(intersections[currInt].outgoingEdges[chosenDir]),
112
                                getIntersectType(currInt), chosenDir);
113
        }
114

    
115
        /* We are done traversing send the graph to the robots */
116
        while(1) sendIntersectionGraph();
117

    
118
        return 0;
119
}
120

    
121
/* Initializes the elements in the graph to 0 */
122
void initGraph(char* seen, char* seenout){
123

    
124
        int i, j;
125

    
126
        /* Set all graph storge elements to 0 */
127
        for(i = 0; i<NUM_FEATURES; i++) {
128
                seen[i] = 0;
129
                seenout[i] = 0;
130
                intersections[i].type = 0;
131
                intersections[i].intNum = 0;
132
                intersections[i].numOut = 0;
133
                intersections[i].outSeen = 0;
134
                for(j = 0; j<4; j++){
135
                        intersections[i].outgoingEdges[0].to = 0;
136
                        intersections[i].outgoingEdges[0].dist = 0;
137
                }
138
        }
139
}
140

    
141

    
142
/* Given an intersection type, returns the number of outgoing edges */
143
int getNumOut(int type) {
144
        switch(type){
145
                case 0: return 3;
146
                case 1: return -1;
147
                case 2: return -1;
148
                case 3: return 3;
149
                case 4: return 2;
150
        }
151
        return -1;
152
}