|
1 |
#include ""
|
|
2 |
#include "mapping.h"
|
|
3 |
#include "lineDrive.h"
|
|
4 |
#include <dragonfly_lib.h>
|
|
5 |
|
|
6 |
//The last intersection that we encountered
|
|
7 |
int lastInt;
|
|
8 |
|
|
9 |
|
|
10 |
/*
|
|
11 |
* Traverses the map using DFS
|
|
12 |
* Returns 0 if all of the intersections in the database were seen
|
|
13 |
* 1 otherwise
|
|
14 |
*/
|
|
15 |
int createMap(){
|
|
16 |
|
|
17 |
int seenout_count = 0; /* The number of intersections we have completely seen */
|
|
18 |
//int hist_count = 0; /* The size of our history stack */
|
|
19 |
|
|
20 |
char[NUM_FEATURES] seen; /* Have we seen this node? */
|
|
21 |
char[NUM_FEATURES] seenout; /* Have we seen the outoging edges of this node? */
|
|
22 |
//char[NUM_FEATURES] history; /* A stack representing the history */
|
|
23 |
|
|
24 |
int i;
|
|
25 |
int outEdges, currInt, chosenEdge;
|
|
26 |
|
|
27 |
|
|
28 |
/* Initialize all of these arrays */
|
|
29 |
for(i = 0; i<NUM_FEATURES; i++) {
|
|
30 |
seen[i] = 0;
|
|
31 |
seenout[i] = 0;
|
|
32 |
history[i] = 0;
|
|
33 |
intersections[i].type = 0;
|
|
34 |
intersections[i].intNum = 0;
|
|
35 |
intersections[i].numOut = 0;
|
|
36 |
intersections[i].outSeen = 0;
|
|
37 |
intersections[i].outgoingEdges[0].to = 0;
|
|
38 |
intersections[i].outgoingEdges[0].dist = 0;
|
|
39 |
intersections[i].outgoingEdges[1].to = 0;
|
|
40 |
intersections[i].outgoingEdges[1].dist = 0;
|
|
41 |
intersections[i].outgoingEdges[2].to = 0;
|
|
42 |
intersections[i].outgoingEdges[2].dist = 0;
|
|
43 |
intersections[i].outgoingEdges[3].to = 0;
|
|
44 |
intersections[i].outgoingEdges[3].dist = 0;
|
|
45 |
}
|
|
46 |
|
|
47 |
|
|
48 |
/* Drives to the nearest intersection */
|
|
49 |
barcode = nextInt();
|
|
50 |
|
|
51 |
while(seenout_count < NUM_FEATURES)
|
|
52 |
{
|
|
53 |
currInt = getIntersectionNum(barcode);
|
|
54 |
seen[currInt] = 1;
|
|
55 |
|
|
56 |
/* Get the number of outgoing edges */
|
|
57 |
outEdges = getNumOut(getIntersectType(currInt));
|
|
58 |
|
|
59 |
/* Randomly choose an outgoing edge that we have not seen to go down
|
|
60 |
* if no such edge exists */
|
|
61 |
chosenEdge = rand() % outEdges;
|
|
62 |
|
|
63 |
/* We have not seen all the outgoing edges */
|
|
64 |
if(!seenout[currInt]){
|
|
65 |
intersection[currInt].outSeen++;
|
|
66 |
/* We have finished seeing all outgoing edges of the intersection */
|
|
67 |
if(intersections[currInt].numOut == intersection[currInt].outSeen){
|
|
68 |
seenout[currInt] = 1;
|
|
69 |
seenout_count ++;
|
|
70 |
}
|
|
71 |
}
|
|
72 |
/* Traverses edge, stores information in struct */
|
|
73 |
traverseEdge(chosenEdge, &(intersection[currInt].outgoingEdges[chosenEdge]));
|
|
74 |
}
|
|
75 |
|
|
76 |
/* We are done traversing send the graph to the robots */
|
|
77 |
sendIntersectionGraph();
|
|
78 |
|
|
79 |
|
|
80 |
return 0;
|
|
81 |
}
|
|
82 |
|
|
83 |
/* Given an intersection type, returns the number of outgoing edges */
|
|
84 |
int getNumOut(int type) {
|
|
85 |
switch(type){
|
|
86 |
case 0: return;
|
|
87 |
case 1: return;
|
|
88 |
case 2: return;
|
|
89 |
case 3: return;
|
|
90 |
case 4: return;
|
|
91 |
}
|
|
92 |
|
|
93 |
return -1;
|
|
94 |
}
|
|
95 |
|
|
96 |
|
|
97 |
/* Creates an edge in the graph */
|
|
98 |
int insertEdge(){
|
|
99 |
|
|
100 |
}
|
|
101 |
|
|
102 |
|
|
103 |
/*
|
|
104 |
* Drives to the next intersection and returns its ID
|
|
105 |
* If we are at a dead end, returns -1
|
|
106 |
*/
|
|
107 |
int nextInt(){
|
|
108 |
|
|
109 |
}
|
|
110 |
|
|
111 |
|
|
112 |
/*
|
|
113 |
* Given an intersection node returns an integer that
|
|
114 |
* can be sent wirelessly
|
|
115 |
*
|
|
116 |
*/
|
|
117 |
int encodeNode(node n){
|
|
118 |
|
|
119 |
|
|
120 |
}
|
|
121 |
|
|
122 |
/*
|
|
123 |
* Uses wireless to send the graph sructure
|
|
124 |
* to other robots
|
|
125 |
*
|
|
126 |
* @return
|
|
127 |
*
|
|
128 |
*/
|
|
129 |
void sendIntersectionGraph() {
|
|
130 |
int i;
|
|
131 |
node_union graph;
|
|
132 |
|
|
133 |
for(i=0; i< NUM_FEATURES; i++){
|
|
134 |
graph.n = intersctions[i];
|
|
135 |
wl_basic_send_global(42, graph.array, 12);
|
|
136 |
}
|
|
137 |
}
|