amiro-os / devices / DiWheelDrive / amiro_map.cpp @ 4d54a507
History | View | Annotate | Download (8.476 KB)
1 |
#include "amiro_map.hpp" |
---|---|
2 |
#include "linefollow.hpp" |
3 |
#include <cstdint> |
4 |
|
5 |
uint8_t AmiroMap::initialize(){ |
6 |
|
7 |
// Clear old values in case the map is initialized again
|
8 |
this->state.current = 0; |
9 |
this->state.next = 0; |
10 |
this->state.valid = false; |
11 |
this->nodeCount = 0; |
12 |
this->state.strategy = 0x1; |
13 |
|
14 |
// convert proto map to internal representation
|
15 |
for (int i=0; i<MAX_NODES; i++){ |
16 |
if(global->testmap[i][2] == 0xff && i != 0){ |
17 |
break;
|
18 |
} else if (global->testmap[i][2] == 0xff && i == 0) { |
19 |
this->state.valid = false; |
20 |
return 255; |
21 |
} |
22 |
|
23 |
addNode(global->testmap[i][0], global->testmap[i][1], |
24 |
global->testmap[i][2]);
|
25 |
|
26 |
// look for start node (default is Node 0)
|
27 |
if (global->testmap[i][2] == 1) { |
28 |
this->state.current = this->nodeCount; |
29 |
} |
30 |
} |
31 |
// Set the next node according to the current strategy
|
32 |
this->state.next = this->nodeList[this->state.current].edge.arr[state.strategy-1]; |
33 |
|
34 |
// TODO make validity check
|
35 |
for (int j=0; j<nodeCount; j++) { |
36 |
this->nodeList[j].visited = 0; |
37 |
visitNode(j); |
38 |
for (int k = 0; k < nodeCount; k++) { |
39 |
if (this->nodeList[k].visited == 1) { |
40 |
this->nodeList[k].visited = 0; |
41 |
} else {
|
42 |
this->state.valid = false; |
43 |
return k;
|
44 |
} |
45 |
} |
46 |
} |
47 |
|
48 |
this->state.valid = true; |
49 |
return 42; |
50 |
} |
51 |
|
52 |
void AmiroMap::visitNode(uint8_t id){
|
53 |
if (this->nodeList[id].visited == 1){ |
54 |
return;
|
55 |
}else{
|
56 |
nodeList[id].visited = 1;
|
57 |
visitNode(this->nodeList[id].edge.edge_id.left);
|
58 |
visitNode(this->nodeList[id].edge.edge_id.right);
|
59 |
} |
60 |
} |
61 |
|
62 |
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 |
|
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 |
uint8_t flag = 0;
|
70 |
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 |
flag |= 255;
|
79 |
} |
80 |
else if (left && !fxpDetected) { // Driving on the right edge |
81 |
// 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 |
state.strategy = 0x02;
|
85 |
switchToNext(¤tPos); |
86 |
flag |= 0x2;
|
87 |
} |
88 |
else if (right && !fxpDetected) { // Driving on the left edge |
89 |
// Same as left only for the right sensor.
|
90 |
state.strategy = 0x01;
|
91 |
switchToNext(¤tPos); |
92 |
flag |= 0x1;
|
93 |
} |
94 |
else if (!left && !right) { |
95 |
// in case the fixpoint is not detected anymore
|
96 |
fxpDetected = false;
|
97 |
flag |= 0x4;
|
98 |
} |
99 |
|
100 |
// Update dist and edge length if possible
|
101 |
calTravelState(¤tPos); |
102 |
|
103 |
return flag;
|
104 |
} |
105 |
|
106 |
uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) { |
107 |
return (uint32_t) sqrt(pow((p2->x - p1->x)/10000, 2) + |
108 |
pow((p2->y - p1->y)/10000, 2)); |
109 |
} |
110 |
|
111 |
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 |
this->lfStrategy = strategy;
|
125 |
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 |
// Do not update if update was already applied the round before (|| rightDetected) == true
|
131 |
if ((left || right) && !fxpDetected) {
|
132 |
// Determine what strategy to use
|
133 |
// assignFxp() will use strategy to assign the next point
|
134 |
state.strategy = left ? 1 : 2; |
135 |
|
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 |
fxpDetected = true;
|
145 |
} |
146 |
} |
147 |
|
148 |
void AmiroMap::calTravelState(types::position *p1) {
|
149 |
// Calculate the moved distance from last detected fixpoint
|
150 |
// if current was visited
|
151 |
if ((state.strategy & nodeList[state.current].visited) > 0) { |
152 |
state.dist = |
153 |
calculateDist(p1, &nodeList[state.current].p.arr[state.strategy - 1]);
|
154 |
} |
155 |
// 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 |
|
165 |
void AmiroMap::checkMap() {
|
166 |
// 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 |
} |
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 |
fxpDetected = true;
|
189 |
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 |
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 |
|
218 |
uint8_t AmiroMap::getNearest(types::position *p1) { |
219 |
|
220 |
uint8_t actualStrategy = this->lfStrategy == EDGE_LEFT ? 1 : 2; |
221 |
uint32_t thresh = global->nodeDistThresh; // TODO: find good thresh value in cm
|
222 |
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 |
} |
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 |
|
274 |
// 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 |
} |
281 |
|
282 |
void AmiroMap::reset(){
|
283 |
this->nodeCount = 0; |
284 |
this->state.current = 0; |
285 |
this->state.next= 0; |
286 |
this->state.valid = false; |
287 |
this->state.strategy = this->lfStrategy == EDGE_LEFT ? 1 : 2; |
288 |
} |