root / trunk / code / projects / traffic_navigation / mapping.c @ 1988
History | View | Annotate | Download (4.65 KB)
1 | 1984 | dgurjar | #include "stdlib.h" |
---|---|---|---|
2 | 1973 | dgurjar | #include "mapping.h" |
3 | #include "lineDrive.h" |
||
4 | #include <dragonfly_lib.h> |
||
5 | 1980 | alevkoy | #include <wl_basic.h> |
6 | 1973 | dgurjar | |
7 | //The last intersection that we encountered
|
||
8 | int lastInt;
|
||
9 | |||
10 | 1980 | alevkoy | /* 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 | 1973 | dgurjar | |
14 | 1984 | dgurjar | |
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 | 1987 | dgurjar | |
21 | /* Keep driving until we see a barcode */
|
||
22 | 1984 | dgurjar | do {
|
23 | 1987 | dgurjar | barcode = (char) doDrive(200); |
24 | 1984 | dgurjar | } |
25 | while(barcode < 0); /* Condition codes are all neg. */ |
||
26 | |||
27 | return barcode;
|
||
28 | } |
||
29 | |||
30 | |||
31 | |||
32 | 1973 | dgurjar | /*
|
33 | * Traverses the map using DFS
|
||
34 | * Returns 0 if all of the intersections in the database were seen
|
||
35 | * 1 otherwise
|
||
36 | */
|
||
37 | 1984 | dgurjar | 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 | 1987 | dgurjar | /* Keep driving until we see a barcode */
|
47 | 1984 | dgurjar | do {
|
48 | 1987 | dgurjar | barcode = (char) doDrive(200); |
49 | 1984 | dgurjar | } |
50 | while(barcode < 0); /* Condition codes are all neg. */ |
||
51 | |||
52 | time = rtc_get(); |
||
53 | |||
54 | newEdge->to = barcode; |
||
55 | newEdge->dist = time; |
||
56 | 1987 | dgurjar | |
57 | 1984 | dgurjar | return barcode;
|
58 | 1976 | pdeo | } |
59 | |||
60 | 1984 | dgurjar | /* This function performs mapping */
|
61 | 1973 | dgurjar | int createMap(){
|
62 | |||
63 | 1987 | dgurjar | 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 | 1973 | dgurjar | |
66 | 1987 | dgurjar | char seenall[NUM_FEATURES]; /* This is how the array should look like once we have |
67 | * seen everything */
|
||
68 | 1973 | dgurjar | |
69 | 1987 | dgurjar | int outEdges, currInt, chosenDir, i;
|
70 | 1984 | dgurjar | |
71 | 1987 | dgurjar | /* Initialize the graph to all zeros */
|
72 | initGraph(seen, seenout); |
||
73 | 1984 | dgurjar | |
74 | 1987 | dgurjar | /* 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 | 1984 | dgurjar | |
79 | 1987 | dgurjar | /* First drives to the nearest intersection */
|
80 | char barcode = driveToNextInt();
|
||
81 | 1984 | dgurjar | |
82 | |||
83 | 1987 | dgurjar | /* 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 | 1984 | dgurjar | } |
114 | |||
115 | 1987 | dgurjar | /* We are done traversing send the graph to the robots */
|
116 | while(1) sendIntersectionGraph(); |
||
117 | 1984 | dgurjar | |
118 | 1987 | dgurjar | return 0; |
119 | 1984 | dgurjar | } |
120 | |||
121 | /* Initializes the elements in the graph to 0 */
|
||
122 | void initGraph(char* seen, char* seenout){ |
||
123 | |||
124 | 1987 | dgurjar | int i, j;
|
125 | 1973 | dgurjar | |
126 | 1987 | dgurjar | /* 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 | 1984 | dgurjar | } |
140 | 1973 | dgurjar | |
141 | |||
142 | /* Given an intersection type, returns the number of outgoing edges */
|
||
143 | int getNumOut(int type) { |
||
144 | 1987 | dgurjar | 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 | 1973 | dgurjar | } |