amiro-os / devices / DiWheelDrive / amiro_map.cpp @ 2af9778e
History | View | Annotate | Download (9.59 KB)
1 | e1f1c4b5 | galberding | #include "amiro_map.hpp" |
---|---|---|---|
2 | 2af9778e | galberding | #include "linefollow.hpp" |
3 | #include <cstdint> |
||
4 | e1f1c4b5 | galberding | |
5 | 7520e117 | galberding | uint8_t AmiroMap::initialize(){ |
6 | e1f1c4b5 | galberding | |
7 | b8b3a9c9 | galberding | // Clear old values in case the map is initialized again
|
8 | 7520e117 | galberding | this->state.current = 0; |
9 | this->state.next = 0; |
||
10 | this->state.valid = false; |
||
11 | 6acaea07 | galberding | this->nodeCount = 0; |
12 | bdac5bec | galberding | this->state.strategy = 0x1; |
13 | e1f1c4b5 | galberding | |
14 | b8b3a9c9 | galberding | // convert proto map to internal representation
|
15 | e1f1c4b5 | galberding | for (int i=0; i<MAX_NODES; i++){ |
16 | 7520e117 | galberding | if(global->testmap[i][2] == 0xff && i != 0){ |
17 | e1f1c4b5 | galberding | break;
|
18 | 7520e117 | galberding | } else if (global->testmap[i][2] == 0xff && i == 0) { |
19 | this->state.valid = false; |
||
20 | d2230e6e | galberding | return 255; |
21 | e1f1c4b5 | galberding | } |
22 | |||
23 | b8b3a9c9 | galberding | //look for start node (default is Node 0)
|
24 | 7520e117 | galberding | if (global->testmap[i][2] == 1 ) { |
25 | this->state.current = i;
|
||
26 | b8b3a9c9 | galberding | } |
27 | e1f1c4b5 | galberding | |
28 | b8b3a9c9 | galberding | this->nodeList[i].id = i;
|
29 | 7520e117 | galberding | this->nodeList[i].left = global->testmap[i][0]; |
30 | this->nodeList[i].right = global->testmap[i][1]; |
||
31 | this->nodeList[i].flag = global->testmap[i][2]; |
||
32 | b8b3a9c9 | galberding | this->nodeCount++;
|
33 | } |
||
34 | bdac5bec | galberding | this->state.next = this->nodeList[this->state.current].right; |
35 | e1f1c4b5 | galberding | |
36 | b8b3a9c9 | galberding | // TODO make validity check
|
37 | d2230e6e | galberding | |
38 | a07a7a1c | galberding | for (int j=0; j<nodeCount; j++) { |
39 | d2230e6e | galberding | this->nodeList[j].visited = 0; |
40 | a07a7a1c | galberding | visitNode(j); |
41 | for (int k = 0; k < nodeCount; k++) { |
||
42 | d2230e6e | galberding | if (this->nodeList[k].visited == 1) { |
43 | this->nodeList[k].visited = 0; |
||
44 | a07a7a1c | galberding | } else {
|
45 | 7520e117 | galberding | this->state.valid = false; |
46 | d2230e6e | galberding | return k;
|
47 | a07a7a1c | galberding | } |
48 | } |
||
49 | } |
||
50 | e1f1c4b5 | galberding | |
51 | 7520e117 | galberding | this->state.valid = true; |
52 | d2230e6e | galberding | return 42; |
53 | a07a7a1c | galberding | } |
54 | e1f1c4b5 | galberding | |
55 | a07a7a1c | galberding | void AmiroMap::visitNode(uint8_t id){
|
56 | d2230e6e | galberding | if (this->nodeList[id].visited == 1){ |
57 | a07a7a1c | galberding | return;
|
58 | }else{
|
||
59 | d2230e6e | galberding | nodeList[id].visited = 1;
|
60 | a07a7a1c | galberding | visitNode(this->nodeList[id].left);
|
61 | visitNode(this->nodeList[id].right);
|
||
62 | } |
||
63 | e1f1c4b5 | galberding | } |
64 | |||
65 | bdac5bec | galberding | uint8_t AmiroMap::update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy) { |
66 | // Called each time at the end of the user thread state machine
|
||
67 | // The bottom sensors will be checked for black ground which is interpreted as
|
||
68 | // filxpoint
|
||
69 | 7520e117 | galberding | |
70 | // set the strategy directly, actually there is no need to store that variable in the class
|
||
71 | // but we will go with it for now to initialize everything properly.
|
||
72 | bdac5bec | galberding | uint8_t flag = 0;
|
73 | 7520e117 | galberding | this->lfStrategy = strategy;
|
74 | bdac5bec | galberding | // uint16_t WL = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
|
75 | // uint16_t WR = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
|
||
76 | 7520e117 | galberding | |
77 | // Check the wheel sensors
|
||
78 | bool left = global->linePID.BThresh >= WL;
|
||
79 | bool right = global->linePID.BThresh >= WR;
|
||
80 | types::position currentPos = global->odometry.getPosition(); |
||
81 | |||
82 | if (left && right) {
|
||
83 | // TODO A dangerous case -> amiro could be lifted
|
||
84 | bdac5bec | galberding | flag |= 255;
|
85 | 7520e117 | galberding | } |
86 | else if (left && !leftDetected) { |
||
87 | // The sensor on the left side of the Amiro is driving on black
|
||
88 | // To prevent continous fixpoint detection a point needs to be marked as currently detected
|
||
89 | // and released.
|
||
90 | leftDetected = true;
|
||
91 | ec052975 | galberding | copyPoint(¤tPos, &nodeList[state.next].pR); |
92 | 7520e117 | galberding | nodeList[state.next].visited |= 0x01;
|
93 | state.current = state.next; |
||
94 | state.next = nodeList[state.current].right; |
||
95 | state.strategy = 0x01;
|
||
96 | bdac5bec | galberding | state.eLength = 0; // Reset length to get recalculated after fixpoint |
97 | flag |= 0x1;
|
||
98 | 7520e117 | galberding | } |
99 | else if (right && !rightDetected) { |
||
100 | // Same as left only for the right sensor.
|
||
101 | rightDetected = true;
|
||
102 | ec052975 | galberding | copyPoint(¤tPos, &nodeList[state.next].pR); |
103 | 7520e117 | galberding | nodeList[state.next].visited |= 0x02;
|
104 | state.current = state.next; |
||
105 | state.next = nodeList[state.current].left; |
||
106 | bdac5bec | galberding | state.strategy = 0x2;
|
107 | state.eLength = 0; // Reset length to get recalculated after fixpoint |
||
108 | flag |= 0x2;
|
||
109 | 7520e117 | galberding | } |
110 | else if (!left && !right) { |
||
111 | // in case the fixpoint is not detected anymore
|
||
112 | leftDetected = false;
|
||
113 | rightDetected = false;
|
||
114 | bdac5bec | galberding | flag |= 0x4;
|
115 | 7520e117 | galberding | } |
116 | |||
117 | a07a7a1c | galberding | |
118 | 7520e117 | galberding | // update internal map_state
|
119 | // Update travel distance
|
||
120 | // check if the nodes of the specific strategy where visited
|
||
121 | bdac5bec | galberding | if (state.strategy
|
122 | a3c54343 | galberding | == nodeList[state.current].visited) { |
123 | bdac5bec | galberding | flag |= 0x8;
|
124 | // only update distance if both nodes were visited
|
||
125 | // Calculate estimated length of the edge
|
||
126 | if (state.strategy == 0x01) { |
||
127 | // Amiro is driving on the right edge
|
||
128 | a3c54343 | galberding | // only calculate edge length if the node is already vivited
|
129 | if ((state.eLength == 0) && (state.strategy == nodeList[state.current].visited)) { |
||
130 | bdac5bec | galberding | state.eLength = calculateDist(&nodeList[state.next].pR, |
131 | &nodeList[state.current].pR); |
||
132 | } |
||
133 | a3c54343 | galberding | state.dist = calculateDist(&nodeList[state.current].pR, ¤tPos); |
134 | bdac5bec | galberding | } else {
|
135 | // Driving on the left edge
|
||
136 | a3c54343 | galberding | if ((state.eLength == 0) && |
137 | (state.strategy == nodeList[state.current].visited)) { |
||
138 | state.eLength = calculateDist(&nodeList[state.next].pR, |
||
139 | &nodeList[state.current].pR); |
||
140 | bdac5bec | galberding | } |
141 | a3c54343 | galberding | state.dist = calculateDist(&nodeList[state.current].pL, ¤tPos); |
142 | bdac5bec | galberding | |
143 | } |
||
144 | } |
||
145 | return flag;
|
||
146 | } |
||
147 | |||
148 | uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) { |
||
149 | d02c536e | galberding | return (uint32_t) sqrt(pow((p2->x - p1->x)/10000, 2) + |
150 | pow((p2->y - p1->y)/10000, 2)); |
||
151 | e1f1c4b5 | galberding | } |
152 | a3c54343 | galberding | |
153 | ec052975 | galberding | uint8_t AmiroMap::trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy, |
154 | ut_states ut_state) { |
||
155 | // Check if map is valid
|
||
156 | if (this->state.valid){ |
||
157 | return update(WL, WR, strategy);
|
||
158 | } |
||
159 | |||
160 | // Create init node if none is there
|
||
161 | // We will not assign a point to the initial fixpoint because it is not clear if
|
||
162 | // start position is at the correct point
|
||
163 | if (nodeCount == 0) { |
||
164 | createInitNode(); |
||
165 | } |
||
166 | 2af9778e | galberding | this->lfStrategy = strategy;
|
167 | ec052975 | galberding | bool left = global->linePID.BThresh >= WL;
|
168 | bool right = global->linePID.BThresh >= WR;
|
||
169 | types::position currentPos = global->odometry.getPosition(); |
||
170 | |||
171 | // Assign fixpoint if side sensor is black
|
||
172 | // Do not update if update was already applied the round before (leftDetected || rightDetected) == true
|
||
173 | if ((left || right) && !(leftDetected || rightDetected)) {
|
||
174 | // Determine what strategy to use
|
||
175 | // assignFxp() will use strategy to assign the next point
|
||
176 | state.strategy = right ? 1 : 2; |
||
177 | |||
178 | // Check if next point is reachable
|
||
179 | if (state.next == 255){ |
||
180 | // Prepare state values for switch
|
||
181 | assignFxp(¤tPos); |
||
182 | } |
||
183 | |||
184 | }else if (!(left || right)) { |
||
185 | // TODO: do we need both?
|
||
186 | leftDetected = rightDetected = true;
|
||
187 | 81b48c66 | galberding | } |
188 | ec052975 | galberding | } |
189 | |||
190 | 81b48c66 | galberding | void AmiroMap::calTravelState(types::position *p1) {
|
191 | // Calculate the moved distance from last detected fixpoint
|
||
192 | state.dist = calculateDist(p1, &nodeList[state.current].p.arr[state.strategy - 1]);
|
||
193 | ec052975 | galberding | |
194 | 81b48c66 | galberding | // Calculate elength if it is 0
|
195 | // and if the point of the next node was visited before
|
||
196 | if ((state.eLength == 0) && ((state.strategy & nodeList[state.next].visited) == 1)) { |
||
197 | state.eLength = |
||
198 | calculateDist(p1, &nodeList[state.current].p.arr[state.strategy - 1]);
|
||
199 | } |
||
200 | ec052975 | galberding | } |
201 | |||
202 | void AmiroMap::checkMap() {
|
||
203 | 81b48c66 | galberding | // The check will basically only consist in checking if all nodes
|
204 | // are connected to following nodes
|
||
205 | |||
206 | for(int i=0; i < nodeCount; i++){ |
||
207 | for(int j=0; j < nodeCount; j++) |
||
208 | if(nodeList[i].edge.arr[j] == 255){ |
||
209 | state.valid = false;
|
||
210 | return;
|
||
211 | } |
||
212 | } |
||
213 | state.valid = true;
|
||
214 | ec052975 | galberding | } |
215 | |||
216 | |||
217 | void AmiroMap::switchToNext(types::position *p1) {
|
||
218 | |||
219 | // Update point if node was not visited before
|
||
220 | if ((nodeList[state.next].visited & state.strategy) == 0){ |
||
221 | copyPoint(p1, &nodeList[state.next].p.arr[state.strategy - 1]);
|
||
222 | nodeList[state.next].visited |= state.strategy; |
||
223 | } |
||
224 | |||
225 | leftDetected = true;
|
||
226 | state.current = state.next; |
||
227 | state.next = nodeList[state.current].edge.arr[state.strategy - 1];
|
||
228 | state.eLength = 0; // Reset length to get recalculated after fixpoint |
||
229 | return;
|
||
230 | } |
||
231 | |||
232 | |||
233 | void AmiroMap::copyPoint(types::position* from, types::position* to) {
|
||
234 | to->x = from->x; |
||
235 | to->y = from->y; |
||
236 | to->f_x = from->f_x; |
||
237 | } |
||
238 | |||
239 | |||
240 | void AmiroMap::createInitNode() {
|
||
241 | this->nodeCount = 0; |
||
242 | state.current = addNode(255, 255, 1); |
||
243 | state.next = 255;
|
||
244 | } |
||
245 | |||
246 | 2af9778e | galberding | |
247 | ec052975 | galberding | uint8_t AmiroMap::getNearest(types::position *p1) { |
248 | 2af9778e | galberding | |
249 | uint8_t actualStrategy = this->lfStrategy == EDGE_LEFT ? 1 : 2; |
||
250 | uint32_t thresh = 1; // TODO: find good thresh value in cm |
||
251 | uint8_t id = 255;
|
||
252 | uint32_t smallestDist = thresh; |
||
253 | uint8_t currentStrategy; |
||
254 | // Calculate the point which is nearest to the current one
|
||
255 | // check if distance and strategy match
|
||
256 | // If right point is found but no left point set choose this as the fitting point
|
||
257 | // Check how point was visited before calculating distance (non visited points are always (0,0))
|
||
258 | |||
259 | for (int i = 0; i < nodeCount; i++) { |
||
260 | for (int j = 0; j < 2; j++){ // Iterate over l and r point |
||
261 | if ((nodeList[i].visited & (j+1)) == 0){ |
||
262 | // Skip point if it was not visited for the given strategy
|
||
263 | continue;
|
||
264 | } |
||
265 | |||
266 | uint32_t tmpDist = calculateDist(&nodeList[i].p.arr[j], p1); |
||
267 | if (tmpDist < smallestDist){
|
||
268 | smallestDist = tmpDist; |
||
269 | id = i; |
||
270 | // Store strategy to match the correct point at the end
|
||
271 | currentStrategy = j; |
||
272 | } |
||
273 | } |
||
274 | } |
||
275 | |||
276 | if (id == 255){ |
||
277 | return 255; |
||
278 | } |
||
279 | |||
280 | // update point at fixpoint if it is not visited
|
||
281 | if ((nodeList[id].visited & actualStrategy) == 0){ |
||
282 | copyPoint(p1, &nodeList[id].p.arr[actualStrategy]); |
||
283 | nodeList[id].visited |= actualStrategy; |
||
284 | } // else point was already visited and is assigned
|
||
285 | |||
286 | return id;
|
||
287 | ec052975 | galberding | } |
288 | |||
289 | uint8_t AmiroMap::assignFxp(types::position *p1) { |
||
290 | |||
291 | // Magic happens to determine if fixpoint is close enough
|
||
292 | uint8_t id = getNearest(p1); |
||
293 | if(id < 255){ |
||
294 | // strategy is either 1 - right or 2 - left
|
||
295 | // Copy current point to either left or right point
|
||
296 | copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);
|
||
297 | }else {
|
||
298 | // A new fixpoint needs to be created
|
||
299 | id = addNode(255, 255, 0); |
||
300 | copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);
|
||
301 | } |
||
302 | a3c54343 | galberding | |
303 | ec052975 | galberding | // Prepare values for switching
|
304 | state.next = id; |
||
305 | nodeList[state.current].edge.arr[state.strategy - 1] = id;
|
||
306 | // Mark point as visited
|
||
307 | nodeList[id].visited |= state.strategy; |
||
308 | return id;
|
||
309 | a3c54343 | galberding | } |