root / trunk / code / projects / traffic_navigation / mapping.cold @ 1979
History | View | Annotate | Download (3.83 KB)
1 |
#include "" |
---|---|
2 |
#include "mapping.h" |
3 |
#include "lineDrive.h" |
4 |
#include <dragonfly_lib.h> |
5 |
|
6 |
//The last intersection that we encountered |
7 |
int lastInt; |
8 |
|
9 |
|
10 |
/* |
11 |
* Traverses the map using DFS |
12 |
* Returns 0 if all of the intersections in the database were seen |
13 |
* 1 otherwise |
14 |
*/ |
15 |
int createEdge(edge* newEdge){ |
16 |
char barcode; |
17 |
char time; |
18 |
rtc_init(SIXTEENTH_SECOND, NULL); |
19 |
rtc_reset(); |
20 |
while(barcode = NOBARCODE){ |
21 |
barcode = (char) doDrive(200); |
22 |
} |
23 |
time = rtc_get(); |
24 |
newEdge.to = barcode; |
25 |
newEdge.dist = time; |
26 |
return 0; |
27 |
} |
28 |
|
29 |
int createMap(){ |
30 |
|
31 |
int seenout_count = 0; /* The number of intersections we have completely seen */ |
32 |
//int hist_count = 0; /* The size of our history stack */ |
33 |
|
34 |
char[NUM_FEATURES] seen; /* Have we seen this node? */ |
35 |
char[NUM_FEATURES] seenout; /* Have we seen the outoging edges of this node? */ |
36 |
//char[NUM_FEATURES] history; /* A stack representing the history */ |
37 |
|
38 |
int i; |
39 |
int outEdges, currInt, chosenEdge; |
40 |
|
41 |
|
42 |
/* Initialize all of these arrays */ |
43 |
for(i = 0; i<NUM_FEATURES; i++) { |
44 |
seen[i] = 0; |
45 |
seenout[i] = 0; |
46 |
history[i] = 0; |
47 |
intersections[i].type = 0; |
48 |
intersections[i].intNum = 0; |
49 |
intersections[i].numOut = 0; |
50 |
intersections[i].outSeen = 0; |
51 |
intersections[i].outgoingEdges[0].to = 0; |
52 |
intersections[i].outgoingEdges[0].dist = 0; |
53 |
intersections[i].outgoingEdges[1].to = 0; |
54 |
intersections[i].outgoingEdges[1].dist = 0; |
55 |
intersections[i].outgoingEdges[2].to = 0; |
56 |
intersections[i].outgoingEdges[2].dist = 0; |
57 |
intersections[i].outgoingEdges[3].to = 0; |
58 |
intersections[i].outgoingEdges[3].dist = 0; |
59 |
} |
60 |
|
61 |
|
62 |
/* Drives to the nearest intersection */ |
63 |
barcode = nextInt(); |
64 |
|
65 |
while(seenout_count < NUM_FEATURES) |
66 |
{ |
67 |
currInt = getIntersectionNum(barcode); |
68 |
seen[currInt] = 1; |
69 |
|
70 |
/* Get the number of outgoing edges */ |
71 |
outEdges = getNumOut(getIntersectType(currInt)); |
72 |
|
73 |
/* Randomly choose an outgoing edge that we have not seen to go down |
74 |
* if no such edge exists */ |
75 |
chosenEdge = rand() % outEdges; |
76 |
|
77 |
/* We have not seen all the outgoing edges */ |
78 |
if(!seenout[currInt]){ |
79 |
intersection[currInt].outSeen++; |
80 |
/* We have finished seeing all outgoing edges of the intersection */ |
81 |
if(intersections[currInt].numOut == intersection[currInt].outSeen){ |
82 |
seenout[currInt] = 1; |
83 |
seenout_count ++; |
84 |
} |
85 |
} |
86 |
/* Traverses edge, stores information in struct */ |
87 |
traverseEdge(chosenEdge, &(intersection[currInt].outgoingEdges[chosenEdge])); |
88 |
} |
89 |
|
90 |
/* We are done traversing send the graph to the robots */ |
91 |
sendIntersectionGraph(); |
92 |
|
93 |
|
94 |
return 0; |
95 |
} |
96 |
|
97 |
/* Given an intersection type, returns the number of outgoing edges */ |
98 |
int getNumOut(int type) { |
99 |
switch(type){ |
100 |
case 0: return; |
101 |
case 1: return; |
102 |
case 2: return; |
103 |
case 3: return; |
104 |
case 4: return; |
105 |
} |
106 |
|
107 |
return -1; |
108 |
} |
109 |
|
110 |
|
111 |
/* Creates an edge in the graph */ |
112 |
int insertEdge(){ |
113 |
|
114 |
} |
115 |
|
116 |
|
117 |
/* |
118 |
* Drives to the next intersection and returns its ID |
119 |
* If we are at a dead end, returns -1 |
120 |
*/ |
121 |
int nextInt(){ |
122 |
|
123 |
} |
124 |
|
125 |
|
126 |
/* |
127 |
* Given an intersection node returns an integer that |
128 |
* can be sent wirelessly |
129 |
* |
130 |
*/ |
131 |
int encodeNode(node n){ |
132 |
|
133 |
|
134 |
} |
135 |
|
136 |
/* |
137 |
* Uses wireless to send the graph sructure |
138 |
* to other robots |
139 |
* |
140 |
* @return |
141 |
* |
142 |
*/ |
143 |
void sendIntersectionGraph() { |
144 |
int i; |
145 |
node_union graph; |
146 |
|
147 |
for(i=0; i< NUM_FEATURES; i++){ |
148 |
graph.n = intersctions[i]; |
149 |
wl_basic_send_global(42, graph.array, 12); |
150 |
} |
151 |
} |