## root / trunk / code / projects / traffic_navigation / mapping.cold @ 1979

History | View | Annotate | Download (3.83 KB)

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 createEdge(edge* newEdge){ |

16 |
char barcode; |

17 |
char time; |

18 |
rtc_init(SIXTEENTH_SECOND, NULL); |

19 |
rtc_reset(); |

20 |
while(barcode = NOBARCODE){ |

21 |
barcode = (char) doDrive(200); |

22 |
} |

23 |
time = rtc_get(); |

24 |
newEdge.to = barcode; |

25 |
newEdge.dist = time; |

26 |
return 0; |

27 |
} |

28 | |

29 |
int createMap(){ |

30 | |

31 |
int seenout_count = 0; /* The number of intersections we have completely seen */ |

32 |
//int hist_count = 0; /* The size of our history stack */ |

33 | |

34 |
char[NUM_FEATURES] seen; /* Have we seen this node? */ |

35 |
char[NUM_FEATURES] seenout; /* Have we seen the outoging edges of this node? */ |

36 |
//char[NUM_FEATURES] history; /* A stack representing the history */ |

37 | |

38 |
int i; |

39 |
int outEdges, currInt, chosenEdge; |

40 | |

41 | |

42 |
/* Initialize all of these arrays */ |

43 |
for(i = 0; i<NUM_FEATURES; i++) { |

44 |
seen[i] = 0; |

45 |
seenout[i] = 0; |

46 |
history[i] = 0; |

47 |
intersections[i].type = 0; |

48 |
intersections[i].intNum = 0; |

49 |
intersections[i].numOut = 0; |

50 |
intersections[i].outSeen = 0; |

51 |
intersections[i].outgoingEdges[0].to = 0; |

52 |
intersections[i].outgoingEdges[0].dist = 0; |

53 |
intersections[i].outgoingEdges[1].to = 0; |

54 |
intersections[i].outgoingEdges[1].dist = 0; |

55 |
intersections[i].outgoingEdges[2].to = 0; |

56 |
intersections[i].outgoingEdges[2].dist = 0; |

57 |
intersections[i].outgoingEdges[3].to = 0; |

58 |
intersections[i].outgoingEdges[3].dist = 0; |

59 |
} |

60 | |

61 | |

62 |
/* Drives to the nearest intersection */ |

63 |
barcode = nextInt(); |

64 | |

65 |
while(seenout_count < NUM_FEATURES) |

66 |
{ |

67 |
currInt = getIntersectionNum(barcode); |

68 |
seen[currInt] = 1; |

69 | |

70 |
/* Get the number of outgoing edges */ |

71 |
outEdges = getNumOut(getIntersectType(currInt)); |

72 | |

73 |
/* Randomly choose an outgoing edge that we have not seen to go down |

74 |
* if no such edge exists */ |

75 |
chosenEdge = rand() % outEdges; |

76 | |

77 |
/* We have not seen all the outgoing edges */ |

78 |
if(!seenout[currInt]){ |

79 |
intersection[currInt].outSeen++; |

80 |
/* We have finished seeing all outgoing edges of the intersection */ |

81 |
if(intersections[currInt].numOut == intersection[currInt].outSeen){ |

82 |
seenout[currInt] = 1; |

83 |
seenout_count ++; |

84 |
} |

85 |
} |

86 |
/* Traverses edge, stores information in struct */ |

87 |
traverseEdge(chosenEdge, &(intersection[currInt].outgoingEdges[chosenEdge])); |

88 |
} |

89 | |

90 |
/* We are done traversing send the graph to the robots */ |

91 |
sendIntersectionGraph(); |

92 | |

93 | |

94 |
return 0; |

95 |
} |

96 | |

97 |
/* Given an intersection type, returns the number of outgoing edges */ |

98 |
int getNumOut(int type) { |

99 |
switch(type){ |

100 |
case 0: return; |

101 |
case 1: return; |

102 |
case 2: return; |

103 |
case 3: return; |

104 |
case 4: return; |

105 |
} |

106 | |

107 |
return -1; |

108 |
} |

109 | |

110 | |

111 |
/* Creates an edge in the graph */ |

112 |
int insertEdge(){ |

113 | |

114 |
} |

115 | |

116 | |

117 |
/* |

118 |
* Drives to the next intersection and returns its ID |

119 |
* If we are at a dead end, returns -1 |

120 |
*/ |

121 |
int nextInt(){ |

122 | |

123 |
} |

124 | |

125 | |

126 |
/* |

127 |
* Given an intersection node returns an integer that |

128 |
* can be sent wirelessly |

129 |
* |

130 |
*/ |

131 |
int encodeNode(node n){ |

132 | |

133 | |

134 |
} |

135 | |

136 |
/* |

137 |
* Uses wireless to send the graph sructure |

138 |
* to other robots |

139 |
* |

140 |
* @return |

141 |
* |

142 |
*/ |

143 |
void sendIntersectionGraph() { |

144 |
int i; |

145 |
node_union graph; |

146 | |

147 |
for(i=0; i< NUM_FEATURES; i++){ |

148 |
graph.n = intersctions[i]; |

149 |
wl_basic_send_global(42, graph.array, 12); |

150 |
} |

151 |
} |