root / trunk / code / projects / traffic_navigation / mapping.cold @ 1987
History | View | Annotate | Download (3.83 KB)
1 | 1979 | dgurjar | #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 | } |