Revision d02c536e devices/DiWheelDrive/amiro_map.cpp
devices/DiWheelDrive/amiro_map.cpp | ||
---|---|---|
137 | 137 |
state.eLength = calculateDist(&nodeList[state.next].pR, |
138 | 138 |
&nodeList[state.current].pR); |
139 | 139 |
} |
140 |
state.dist = calculateDist(&nodeList[state.next].pR, ¤tPos) * 100 /
|
|
140 |
state.dist = calculateDist(&nodeList[state.current].pR, ¤tPos) * 100 /
|
|
141 | 141 |
state.eLength; |
142 | 142 |
} else { |
143 | 143 |
// Driving on the left edge |
... | ... | |
145 | 145 |
state.eLength = |
146 | 146 |
calculateDist(&nodeList[state.next].pL, &nodeList[state.current].pL); |
147 | 147 |
} |
148 |
state.dist = calculateDist(&nodeList[state.next].pL, ¤tPos) * 100 / state.eLength;
|
|
148 |
state.dist = calculateDist(&nodeList[state.current].pL, ¤tPos) * 100 / state.eLength;
|
|
149 | 149 |
|
150 | 150 |
} |
151 | 151 |
} |
... | ... | |
153 | 153 |
} |
154 | 154 |
|
155 | 155 |
uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) { |
156 |
return (uint32_t) sqrt(pow(p2->x - p1->x, 2) +
|
|
157 |
pow(p2->y - p1->y, 2));
|
|
156 |
return (uint32_t) sqrt(pow((p2->x - p1->x)/10000, 2) +
|
|
157 |
pow((p2->y - p1->y)/10000, 2));
|
|
158 | 158 |
} |
Also available in: Unified diff