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