amiro-os / devices / DiWheelDrive / amiro_map.cpp @ 64cba697
History | View | Annotate | Download (8.104 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 |
//look for start node (default is Node 0)
|
24 |
if (global->testmap[i][2] == 1 ) { |
25 |
this->state.current = i;
|
26 |
} |
27 |
|
28 |
this->nodeList[i].id = i;
|
29 |
this->nodeList[i].edge.edge_id.left = global->testmap[i][0]; |
30 |
this->nodeList[i].edge.edge_id.right = global->testmap[i][1]; |
31 |
this->nodeList[i].flag = global->testmap[i][2]; |
32 |
this->nodeCount++;
|
33 |
} |
34 |
this->state.next = this->nodeList[this->state.current].edge.edge_id.right; |
35 |
|
36 |
// TODO make validity check
|
37 |
|
38 |
for (int j=0; j<nodeCount; j++) { |
39 |
this->nodeList[j].visited = 0; |
40 |
visitNode(j); |
41 |
for (int k = 0; k < nodeCount; k++) { |
42 |
if (this->nodeList[k].visited == 1) { |
43 |
this->nodeList[k].visited = 0; |
44 |
} else {
|
45 |
this->state.valid = false; |
46 |
return k;
|
47 |
} |
48 |
} |
49 |
} |
50 |
|
51 |
this->state.valid = true; |
52 |
return 42; |
53 |
} |
54 |
|
55 |
void AmiroMap::visitNode(uint8_t id){
|
56 |
if (this->nodeList[id].visited == 1){ |
57 |
return;
|
58 |
}else{
|
59 |
nodeList[id].visited = 1;
|
60 |
visitNode(this->nodeList[id].edge.edge_id.left);
|
61 |
visitNode(this->nodeList[id].edge.edge_id.right);
|
62 |
} |
63 |
} |
64 |
|
65 |
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 |
|
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 |
uint8_t flag = 0;
|
73 |
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 |
flag |= 255;
|
82 |
} |
83 |
else if (left && !fxpDetected) { // Driving on the right edge |
84 |
// 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 |
state.strategy = 0x02;
|
88 |
switchToNext(¤tPos); |
89 |
flag |= 0x2;
|
90 |
} |
91 |
else if (right && !fxpDetected) { // Driving on the left edge |
92 |
// Same as left only for the right sensor.
|
93 |
state.strategy = 0x01;
|
94 |
switchToNext(¤tPos); |
95 |
flag |= 0x1;
|
96 |
} |
97 |
else if (!left && !right) { |
98 |
// in case the fixpoint is not detected anymore
|
99 |
fxpDetected = false;
|
100 |
flag |= 0x4;
|
101 |
} |
102 |
|
103 |
// Update dist and edge length if possible
|
104 |
calTravelState(¤tPos); |
105 |
|
106 |
return flag;
|
107 |
} |
108 |
|
109 |
uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) { |
110 |
return (uint32_t) sqrt(pow((p2->x - p1->x)/10000, 2) + |
111 |
pow((p2->y - p1->y)/10000, 2)); |
112 |
} |
113 |
|
114 |
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 |
this->lfStrategy = strategy;
|
128 |
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 |
// Do not update if update was already applied the round before (|| rightDetected) == true
|
134 |
if ((left || right) && !fxpDetected) {
|
135 |
// Determine what strategy to use
|
136 |
// assignFxp() will use strategy to assign the next point
|
137 |
state.strategy = left ? 1 : 2; |
138 |
|
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 |
fxpDetected = true;
|
148 |
} |
149 |
} |
150 |
|
151 |
void AmiroMap::calTravelState(types::position *p1) {
|
152 |
// Calculate the moved distance from last detected fixpoint
|
153 |
// if current was visited
|
154 |
if ((state.strategy & nodeList[state.current].visited) > 0) { |
155 |
state.dist = |
156 |
calculateDist(p1, &nodeList[state.current].p.arr[state.strategy - 1]);
|
157 |
} |
158 |
// 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 |
|
168 |
void AmiroMap::checkMap() {
|
169 |
// 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 |
} |
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 |
fxpDetected = true;
|
192 |
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 |
|
213 |
uint8_t AmiroMap::getNearest(types::position *p1) { |
214 |
|
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 |
} |
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 |
|
269 |
// 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 |
} |