Revision a3c54343 devices/DiWheelDrive/amiro_map.cpp
devices/DiWheelDrive/amiro_map.cpp | ||
---|---|---|
127 | 127 |
// Update travel distance |
128 | 128 |
// check if the nodes of the specific strategy where visited |
129 | 129 |
if (state.strategy |
130 |
== (nodeList[state.current].visited & nodeList[state.next].visited)) {
|
|
130 |
== nodeList[state.current].visited) {
|
|
131 | 131 |
flag |= 0x8; |
132 | 132 |
// only update distance if both nodes were visited |
133 | 133 |
// Calculate estimated length of the edge |
134 | 134 |
if (state.strategy == 0x01) { |
135 | 135 |
// Amiro is driving on the right edge |
136 |
if (state.eLength == 0) { |
|
136 |
// only calculate edge length if the node is already vivited |
|
137 |
if ((state.eLength == 0) && (state.strategy == nodeList[state.current].visited)) { |
|
137 | 138 |
state.eLength = calculateDist(&nodeList[state.next].pR, |
138 | 139 |
&nodeList[state.current].pR); |
139 | 140 |
} |
140 |
state.dist = calculateDist(&nodeList[state.current].pR, ¤tPos) * 100 / |
|
141 |
state.eLength; |
|
141 |
state.dist = calculateDist(&nodeList[state.current].pR, ¤tPos); |
|
142 | 142 |
} else { |
143 | 143 |
// Driving on the left edge |
144 |
if (state.eLength == 0) { |
|
145 |
state.eLength = |
|
146 |
calculateDist(&nodeList[state.next].pL, &nodeList[state.current].pL); |
|
144 |
if ((state.eLength == 0) && |
|
145 |
(state.strategy == nodeList[state.current].visited)) { |
|
146 |
state.eLength = calculateDist(&nodeList[state.next].pR, |
|
147 |
&nodeList[state.current].pR); |
|
147 | 148 |
} |
148 |
state.dist = calculateDist(&nodeList[state.current].pL, ¤tPos) * 100 / state.eLength;
|
|
149 |
state.dist = calculateDist(&nodeList[state.current].pL, ¤tPos); |
|
149 | 150 |
|
150 | 151 |
} |
151 | 152 |
} |
... | ... | |
156 | 157 |
return (uint32_t) sqrt(pow((p2->x - p1->x)/10000, 2) + |
157 | 158 |
pow((p2->y - p1->y)/10000, 2)); |
158 | 159 |
} |
160 |
|
|
161 |
uint8_t trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy, ut_states state){ |
|
162 |
|
|
163 |
} |
Also available in: Unified diff