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 
} 