Revision 81b48c66
| devices/DiWheelDrive/amiro_map.cpp | ||
|---|---|---|
| 182 | 182 |
}else if (!(left || right)) {
|
| 183 | 183 |
// TODO: do we need both? |
| 184 | 184 |
leftDetected = rightDetected = true; |
| 185 |
} |
|
| 185 | 186 |
} |
| 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]); |
|
| 187 | 191 |
|
| 188 |
}
|
|
| 189 |
|
|
| 190 |
|
|
| 191 |
|
|
| 192 |
void AmiroMap::calTravelState() {
|
|
| 193 |
|
|
| 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 |
} |
|
| 194 | 198 |
} |
| 195 | 199 |
|
| 196 | 200 |
void AmiroMap::checkMap() {
|
| 197 |
|
|
| 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; |
|
| 198 | 212 |
} |
| 199 | 213 |
|
| 200 | 214 |
|
| devices/DiWheelDrive/amiro_map.hpp | ||
|---|---|---|
| 125 | 125 |
uint8_t addNode(uint8_t l, uint8_t r, uint8_t flags); |
| 126 | 126 |
|
| 127 | 127 |
// Calculate dist and elength if possibel |
| 128 |
void calTravelState(); |
|
| 128 |
void calTravelState(types::position *p1);
|
|
| 129 | 129 |
|
| 130 | 130 |
// Check if all nodes have followers |
| 131 | 131 |
// Calculate validity check |
Also available in: Unified diff