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 |
} |