root / trunk / code / projects / traffic_navigation / mapping.c @ 1984
History  View  Annotate  Download (4.15 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 
do {

22 
barcode = (char) doDrive(200); 
23 
} 
24 
while(barcode < 0); /* Condition codes are all neg. */ 
25  
26 
return barcode;

27 
} 
28  
29  
30  
31 
/*

32 
* Traverses the map using DFS

33 
* Returns 0 if all of the intersections in the database were seen

34 
* 1 otherwise

35 
*/

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;

56 
} 
57  
58 
/* This function performs mapping */

59 
int createMap(){

60  
61 
int seenout_count = 0; /* The number of intersections we have completely seen */ 
62  
63 
char seen[NUM_FEATURES]; /* Have we seen this node? */ 
64 
char seenout[NUM_FEATURES]; /* Have we seen the outoging edges of this node? */ 
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  
110 
int i;

111  
112 
/* Initialize all of these arrays */

113 
for(i = 0; i<NUM_FEATURES; i++) { 
114 
seen[i] = 0;

115 
seenout[i] = 0;

116 
intersections[i].type = 0;

117 
intersections[i].intNum = 0;

118 
intersections[i].numOut = 0;

119 
intersections[i].outSeen = 0;

120 
intersections[i].outgoingEdges[0].to = 0; 
121 
intersections[i].outgoingEdges[0].dist = 0; 
122 
intersections[i].outgoingEdges[1].to = 0; 
123 
intersections[i].outgoingEdges[1].dist = 0; 
124 
intersections[i].outgoingEdges[2].to = 0; 
125 
intersections[i].outgoingEdges[2].dist = 0; 
126 
intersections[i].outgoingEdges[3].to = 0; 
127 
intersections[i].outgoingEdges[3].dist = 0; 
128 
} 
129 
} 
130  
131  
132 
/* Given an intersection type, returns the number of outgoing edges */

133 
int getNumOut(int type) { 
134 
switch(type){

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; 
140 
} 
141 
return 1; 
142 
} 
143  
144 
/*

145 
* Drives to the next intersection and returns its ID

146 
* If we are at a dead end, returns 1

147 
*/

148 
int nextInt(){

149 
return 0; 
150 
} 
151  
152  
153 
/*

154 
* Given an intersection node returns an integer that

155 
* can be sent wirelessly

156 
*

157 
*/

158 
int encodeNode(node n){

159 
return 0; 
160 
} 
161 