amiro-os / devices / DiWheelDrive / amiro_map.cpp @ ec052975
History | View | Annotate | Download (7.52 KB)
1 | e1f1c4b5 | galberding | #include "amiro_map.hpp" |
---|---|---|---|
2 | |||
3 | 7520e117 | galberding | uint8_t AmiroMap::initialize(){ |
4 | e1f1c4b5 | galberding | |
5 | b8b3a9c9 | galberding | // Clear old values in case the map is initialized again
|
6 | 7520e117 | galberding | this->state.current = 0; |
7 | this->state.next = 0; |
||
8 | this->state.valid = false; |
||
9 | 6acaea07 | galberding | this->nodeCount = 0; |
10 | bdac5bec | galberding | this->state.strategy = 0x1; |
11 | e1f1c4b5 | galberding | |
12 | b8b3a9c9 | galberding | // convert proto map to internal representation
|
13 | e1f1c4b5 | galberding | for (int i=0; i<MAX_NODES; i++){ |
14 | 7520e117 | galberding | if(global->testmap[i][2] == 0xff && i != 0){ |
15 | e1f1c4b5 | galberding | break;
|
16 | 7520e117 | galberding | } else if (global->testmap[i][2] == 0xff && i == 0) { |
17 | this->state.valid = false; |
||
18 | d2230e6e | galberding | return 255; |
19 | e1f1c4b5 | galberding | } |
20 | |||
21 | b8b3a9c9 | galberding | //look for start node (default is Node 0)
|
22 | 7520e117 | galberding | if (global->testmap[i][2] == 1 ) { |
23 | this->state.current = i;
|
||
24 | b8b3a9c9 | galberding | } |
25 | e1f1c4b5 | galberding | |
26 | b8b3a9c9 | galberding | this->nodeList[i].id = i;
|
27 | 7520e117 | galberding | 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 | b8b3a9c9 | galberding | this->nodeCount++;
|
31 | } |
||
32 | bdac5bec | galberding | this->state.next = this->nodeList[this->state.current].right; |
33 | e1f1c4b5 | galberding | |
34 | b8b3a9c9 | galberding | // TODO make validity check
|
35 | d2230e6e | galberding | |
36 | a07a7a1c | galberding | for (int j=0; j<nodeCount; j++) { |
37 | d2230e6e | galberding | this->nodeList[j].visited = 0; |
38 | a07a7a1c | galberding | visitNode(j); |
39 | for (int k = 0; k < nodeCount; k++) { |
||
40 | d2230e6e | galberding | if (this->nodeList[k].visited == 1) { |
41 | this->nodeList[k].visited = 0; |
||
42 | a07a7a1c | galberding | } else {
|
43 | 7520e117 | galberding | this->state.valid = false; |
44 | d2230e6e | galberding | return k;
|
45 | a07a7a1c | galberding | } |
46 | } |
||
47 | } |
||
48 | e1f1c4b5 | galberding | |
49 | 7520e117 | galberding | this->state.valid = true; |
50 | d2230e6e | galberding | return 42; |
51 | a07a7a1c | galberding | } |
52 | e1f1c4b5 | galberding | |
53 | a07a7a1c | galberding | void AmiroMap::visitNode(uint8_t id){
|
54 | d2230e6e | galberding | if (this->nodeList[id].visited == 1){ |
55 | a07a7a1c | galberding | return;
|
56 | }else{
|
||
57 | d2230e6e | galberding | nodeList[id].visited = 1;
|
58 | a07a7a1c | galberding | visitNode(this->nodeList[id].left);
|
59 | visitNode(this->nodeList[id].right);
|
||
60 | } |
||
61 | e1f1c4b5 | galberding | } |
62 | |||
63 | bdac5bec | galberding | 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 | 7520e117 | galberding | |
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 | bdac5bec | galberding | uint8_t flag = 0;
|
71 | 7520e117 | galberding | this->lfStrategy = strategy;
|
72 | bdac5bec | galberding | // uint16_t WL = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
|
73 | // uint16_t WR = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
|
||
74 | 7520e117 | galberding | |
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 | bdac5bec | galberding | flag |= 255;
|
83 | 7520e117 | galberding | } |
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 | ec052975 | galberding | copyPoint(¤tPos, &nodeList[state.next].pR); |
90 | 7520e117 | galberding | nodeList[state.next].visited |= 0x01;
|
91 | state.current = state.next; |
||
92 | state.next = nodeList[state.current].right; |
||
93 | state.strategy = 0x01;
|
||
94 | bdac5bec | galberding | state.eLength = 0; // Reset length to get recalculated after fixpoint |
95 | flag |= 0x1;
|
||
96 | 7520e117 | galberding | } |
97 | else if (right && !rightDetected) { |
||
98 | // Same as left only for the right sensor.
|
||
99 | rightDetected = true;
|
||
100 | ec052975 | galberding | copyPoint(¤tPos, &nodeList[state.next].pR); |
101 | 7520e117 | galberding | nodeList[state.next].visited |= 0x02;
|
102 | state.current = state.next; |
||
103 | state.next = nodeList[state.current].left; |
||
104 | bdac5bec | galberding | state.strategy = 0x2;
|
105 | state.eLength = 0; // Reset length to get recalculated after fixpoint |
||
106 | flag |= 0x2;
|
||
107 | 7520e117 | galberding | } |
108 | else if (!left && !right) { |
||
109 | // in case the fixpoint is not detected anymore
|
||
110 | leftDetected = false;
|
||
111 | rightDetected = false;
|
||
112 | bdac5bec | galberding | flag |= 0x4;
|
113 | 7520e117 | galberding | } |
114 | |||
115 | a07a7a1c | galberding | |
116 | 7520e117 | galberding | // update internal map_state
|
117 | // Update travel distance
|
||
118 | // check if the nodes of the specific strategy where visited
|
||
119 | bdac5bec | galberding | if (state.strategy
|
120 | a3c54343 | galberding | == nodeList[state.current].visited) { |
121 | bdac5bec | galberding | 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 | a3c54343 | galberding | // only calculate edge length if the node is already vivited
|
127 | if ((state.eLength == 0) && (state.strategy == nodeList[state.current].visited)) { |
||
128 | bdac5bec | galberding | state.eLength = calculateDist(&nodeList[state.next].pR, |
129 | &nodeList[state.current].pR); |
||
130 | } |
||
131 | a3c54343 | galberding | state.dist = calculateDist(&nodeList[state.current].pR, ¤tPos); |
132 | bdac5bec | galberding | } else {
|
133 | // Driving on the left edge
|
||
134 | a3c54343 | galberding | 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 | bdac5bec | galberding | } |
139 | a3c54343 | galberding | state.dist = calculateDist(&nodeList[state.current].pL, ¤tPos); |
140 | bdac5bec | galberding | |
141 | } |
||
142 | } |
||
143 | return flag;
|
||
144 | } |
||
145 | |||
146 | uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) { |
||
147 | d02c536e | galberding | return (uint32_t) sqrt(pow((p2->x - p1->x)/10000, 2) + |
148 | pow((p2->y - p1->y)/10000, 2)); |
||
149 | e1f1c4b5 | galberding | } |
150 | a3c54343 | galberding | |
151 | ec052975 | galberding | 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 | } |
||
189 | |||
190 | |||
191 | |||
192 | void AmiroMap::calTravelState() {
|
||
193 | |||
194 | } |
||
195 | |||
196 | void AmiroMap::checkMap() {
|
||
197 | |||
198 | } |
||
199 | |||
200 | |||
201 | void AmiroMap::switchToNext(types::position *p1) {
|
||
202 | |||
203 | // Update point if node was not visited before
|
||
204 | if ((nodeList[state.next].visited & state.strategy) == 0){ |
||
205 | copyPoint(p1, &nodeList[state.next].p.arr[state.strategy - 1]);
|
||
206 | nodeList[state.next].visited |= state.strategy; |
||
207 | } |
||
208 | |||
209 | leftDetected = true;
|
||
210 | state.current = state.next; |
||
211 | state.next = nodeList[state.current].edge.arr[state.strategy - 1];
|
||
212 | state.eLength = 0; // Reset length to get recalculated after fixpoint |
||
213 | return;
|
||
214 | } |
||
215 | |||
216 | |||
217 | void AmiroMap::copyPoint(types::position* from, types::position* to) {
|
||
218 | to->x = from->x; |
||
219 | to->y = from->y; |
||
220 | to->f_x = from->f_x; |
||
221 | } |
||
222 | |||
223 | |||
224 | void AmiroMap::createInitNode() {
|
||
225 | this->nodeCount = 0; |
||
226 | state.current = addNode(255, 255, 1); |
||
227 | state.next = 255;
|
||
228 | } |
||
229 | |||
230 | uint8_t AmiroMap::getNearest(types::position *p1) { |
||
231 | uint8_t b; |
||
232 | return b;
|
||
233 | } |
||
234 | |||
235 | uint8_t AmiroMap::assignFxp(types::position *p1) { |
||
236 | |||
237 | // Magic happens to determine if fixpoint is close enough
|
||
238 | uint8_t id = getNearest(p1); |
||
239 | if(id < 255){ |
||
240 | // strategy is either 1 - right or 2 - left
|
||
241 | // Copy current point to either left or right point
|
||
242 | copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);
|
||
243 | }else {
|
||
244 | // A new fixpoint needs to be created
|
||
245 | id = addNode(255, 255, 0); |
||
246 | copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);
|
||
247 | } |
||
248 | a3c54343 | galberding | |
249 | ec052975 | galberding | // Prepare values for switching
|
250 | state.next = id; |
||
251 | nodeList[state.current].edge.arr[state.strategy - 1] = id;
|
||
252 | // Mark point as visited
|
||
253 | nodeList[id].visited |= state.strategy; |
||
254 | return id;
|
||
255 | a3c54343 | galberding | } |