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