Revision 1984
Created some mapping code that compiles!
Lower level mapping functions finalized!
Higher level random mapping still needs some work.
It is probably buggy.
trunk/code/projects/traffic_navigation/mapping.c | ||
---|---|---|
1 |
#include "stdlib.h" |
|
1 | 2 |
#include "mapping.h" |
2 | 3 |
#include "lineDrive.h" |
3 | 4 |
#include <dragonfly_lib.h> |
... | ... | |
10 | 11 |
* after its creation, the graph is transmitted wirelessly */ |
11 | 12 |
node intersections[NUM_FEATURES]; |
12 | 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 |
do { |
|
22 |
barcode = (char) doDrive(200); |
|
23 |
} |
|
24 |
while(barcode < 0); /* Condition codes are all neg. */ |
|
25 |
|
|
26 |
return barcode; |
|
27 |
} |
|
28 |
|
|
29 |
|
|
30 |
|
|
13 | 31 |
/* |
14 | 32 |
* Traverses the map using DFS |
15 | 33 |
* Returns 0 if all of the intersections in the database were seen |
16 | 34 |
* 1 otherwise |
17 | 35 |
*/ |
18 |
int createEdge(edge* newEdge){ |
|
19 |
char barcode; |
|
20 |
char time; |
|
21 |
rtc_init(SIXTEENTH_SECOND, NULL); |
|
22 |
rtc_reset(); |
|
23 |
do |
|
24 |
{ |
|
25 |
barcode = (char) doDrive(200); |
|
26 |
} while(barcode == NOBARCODE); |
|
27 |
time = rtc_get(); |
|
28 |
newEdge->to = barcode; |
|
29 |
newEdge->dist = time; |
|
30 |
return 0; |
|
36 |
char createEdge(edge* newEdge, int type, int direction){ |
|
37 |
char barcode; |
|
38 |
char time; |
|
39 |
|
|
40 |
rtc_init(SIXTEENTH_SECOND, NULL); |
|
41 |
rtc_reset(); |
|
42 |
|
|
43 |
turn(type, direction); |
|
44 |
|
|
45 |
do { |
|
46 |
barcode = (char) doDrive(200); |
|
47 |
} |
|
48 |
while(barcode < 0); /* Condition codes are all neg. */ |
|
49 |
|
|
50 |
time = rtc_get(); |
|
51 |
|
|
52 |
newEdge->to = barcode; |
|
53 |
newEdge->dist = time; |
|
54 |
|
|
55 |
return barcode; |
|
31 | 56 |
} |
32 | 57 |
|
58 |
/* This function performs mapping */ |
|
33 | 59 |
int createMap(){ |
34 | 60 |
|
35 | 61 |
int seenout_count = 0; /* The number of intersections we have completely seen */ |
36 |
//int hist_count = 0; /* The size of our history stack */ |
|
37 | 62 |
|
38 | 63 |
char seen[NUM_FEATURES]; /* Have we seen this node? */ |
39 | 64 |
char seenout[NUM_FEATURES]; /* Have we seen the outoging edges of this node? */ |
40 | 65 |
|
66 |
int outEdges, currInt, chosenDir; |
|
67 |
|
|
68 |
initGraph(seen, seenout); |
|
69 |
|
|
70 |
/* Drives to the nearest intersection */ |
|
71 |
char barcode = driveToNextInt(); |
|
72 |
|
|
73 |
while(seenout_count < NUM_FEATURES) |
|
74 |
{ |
|
75 |
currInt = getIntersectNum(barcode); |
|
76 |
seen[currInt] = 1; |
|
77 |
|
|
78 |
/* Get the number of outgoing edges */ |
|
79 |
outEdges = getNumOut(getIntersectType(currInt)); |
|
80 |
|
|
81 |
/* Randomly choose an outgoing edge that we have not seen to go down |
|
82 |
* if no such edge exists */ |
|
83 |
// TODO: include the appropriate header file for rand() |
|
84 |
chosenDir = rand() % outEdges; |
|
85 |
|
|
86 |
/* We have not seen all the outgoing edges */ |
|
87 |
if(!seenout[currInt]){ |
|
88 |
intersections[currInt].outSeen++; |
|
89 |
/* We have finished seeing all outgoing edges of the intersection */ |
|
90 |
if(intersections[currInt].numOut == intersections[currInt].outSeen){ |
|
91 |
seenout[currInt] = 1; |
|
92 |
seenout_count ++; |
|
93 |
} |
|
94 |
} |
|
95 |
/* Traverses edge, stores information in struct */ |
|
96 |
createEdge(&(intersections[currInt].outgoingEdges[chosenDir]), |
|
97 |
getIntersectType(currInt), chosenDir); |
|
98 |
} |
|
99 |
|
|
100 |
/* We are done traversing send the graph to the robots */ |
|
101 |
sendIntersectionGraph(); |
|
102 |
|
|
103 |
|
|
104 |
return 0; |
|
105 |
} |
|
106 |
|
|
107 |
/* Initializes the elements in the graph to 0 */ |
|
108 |
void initGraph(char* seen, char* seenout){ |
|
109 |
|
|
41 | 110 |
int i; |
42 |
int outEdges, currInt, chosenEdge; |
|
43 | 111 |
|
44 |
|
|
45 | 112 |
/* Initialize all of these arrays */ |
46 | 113 |
for(i = 0; i<NUM_FEATURES; i++) { |
47 | 114 |
seen[i] = 0; |
... | ... | |
59 | 126 |
intersections[i].outgoingEdges[3].to = 0; |
60 | 127 |
intersections[i].outgoingEdges[3].dist = 0; |
61 | 128 |
} |
129 |
} |
|
62 | 130 |
|
63 | 131 |
|
64 |
/* Drives to the nearest intersection */ |
|
65 |
int barcode = nextInt(); |
|
66 |
|
|
67 |
while(seenout_count < NUM_FEATURES) |
|
68 |
{ |
|
69 |
currInt = getIntersectNum(barcode); |
|
70 |
seen[currInt] = 1; |
|
71 |
|
|
72 |
/* Get the number of outgoing edges */ |
|
73 |
outEdges = getNumOut(getIntersectType(currInt)); |
|
74 |
|
|
75 |
/* Randomly choose an outgoing edge that we have not seen to go down |
|
76 |
* if no such edge exists */ |
|
77 |
// TODO: include the appropriate header file for rand() |
|
78 |
chosenEdge = rand() % outEdges; |
|
79 |
|
|
80 |
/* We have not seen all the outgoing edges */ |
|
81 |
if(!seenout[currInt]){ |
|
82 |
intersections[currInt].outSeen++; |
|
83 |
/* We have finished seeing all outgoing edges of the intersection */ |
|
84 |
if(intersections[currInt].numOut == intersections[currInt].outSeen){ |
|
85 |
seenout[currInt] = 1; |
|
86 |
seenout_count ++; |
|
87 |
} |
|
88 |
} |
|
89 |
/* Traverses edge, stores information in struct */ |
|
90 |
/* TODO: Define traverseEdge, then uncomment this line |
|
91 |
traverseEdge(chosenEdge, &(intersections[currInt].outgoingEdges[chosenEdge])); */ |
|
92 |
} |
|
93 |
|
|
94 |
/* We are done traversing send the graph to the robots */ |
|
95 |
sendIntersectionGraph(); |
|
96 |
|
|
97 |
|
|
98 |
return 0; |
|
99 |
} |
|
100 |
|
|
101 | 132 |
/* Given an intersection type, returns the number of outgoing edges */ |
102 | 133 |
int getNumOut(int type) { |
103 | 134 |
switch(type){ |
104 |
case 0: |
|
105 |
return 0; |
|
106 |
case 1: |
|
107 |
return 1; |
|
108 |
case 2: |
|
109 |
return 2; |
|
110 |
case 3: |
|
111 |
return 3; |
|
112 |
case 4: |
|
113 |
return 4; |
|
135 |
case 0: return 3; |
|
136 |
case 1: return -1; |
|
137 |
case 2: return -1; |
|
138 |
case 3: return 3; |
|
139 |
case 4: return 2; |
|
114 | 140 |
} |
115 |
|
|
116 | 141 |
return -1; |
117 | 142 |
} |
118 | 143 |
|
119 |
|
|
120 |
/* Creates an edge in the graph */ |
|
121 |
int insertEdge(){ |
|
122 |
return 0; |
|
123 |
} |
|
124 |
|
|
125 |
|
|
126 | 144 |
/* |
127 | 145 |
* Drives to the next intersection and returns its ID |
128 | 146 |
* If we are at a dead end, returns -1 |
129 | 147 |
*/ |
130 | 148 |
int nextInt(){ |
131 |
return 0;
|
|
149 |
return 0;
|
|
132 | 150 |
} |
133 | 151 |
|
134 | 152 |
|
... | ... | |
138 | 156 |
* |
139 | 157 |
*/ |
140 | 158 |
int encodeNode(node n){ |
141 |
return 0;
|
|
159 |
return 0;
|
|
142 | 160 |
} |
143 | 161 |
|
trunk/code/projects/traffic_navigation/mapping.h | ||
---|---|---|
31 | 31 |
char array[12]; |
32 | 32 |
}node_union; |
33 | 33 |
|
34 |
int createEdge(edge* newEdge); |
|
34 |
char driveToNextInt(void); |
|
35 |
char createEdge(edge* newEdge, int type, int direction); |
|
36 |
void initGraph(char* seen, char* seenout); |
|
35 | 37 |
int createMap(void); |
36 | 38 |
int getNumOut(int type); |
37 | 39 |
int insertEdge(void); |
Also available in: Unified diff