root / trunk / code / projects / traffic_navigation / mapping.c @ 1984
History | View | Annotate | Download (4.15 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 | |||
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 | 1973 | dgurjar | /*
|
32 | * Traverses the map using DFS
|
||
33 | * Returns 0 if all of the intersections in the database were seen
|
||
34 | * 1 otherwise
|
||
35 | */
|
||
36 | 1984 | dgurjar | 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 | 1976 | pdeo | } |
57 | |||
58 | 1984 | dgurjar | /* This function performs mapping */
|
59 | 1973 | dgurjar | int createMap(){
|
60 | |||
61 | int seenout_count = 0; /* The number of intersections we have completely seen */ |
||
62 | |||
63 | 1980 | alevkoy | char seen[NUM_FEATURES]; /* Have we seen this node? */ |
64 | char seenout[NUM_FEATURES]; /* Have we seen the outoging edges of this node? */ |
||
65 | 1973 | dgurjar | |
66 | 1984 | dgurjar | 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 | 1973 | dgurjar | 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 | 1984 | dgurjar | } |
130 | 1973 | dgurjar | |
131 | |||
132 | /* Given an intersection type, returns the number of outgoing edges */
|
||
133 | int getNumOut(int type) { |
||
134 | switch(type){
|
||
135 | 1984 | dgurjar | 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 | 1973 | dgurjar | } |
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 | 1984 | dgurjar | return 0; |
150 | 1973 | dgurjar | } |
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 | 1984 | dgurjar | return 0; |
160 | 1973 | dgurjar | } |