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