Revision a07a7a1c devices/DiWheelDrive/amiro_map.cpp
| devices/DiWheelDrive/amiro_map.cpp | ||
|---|---|---|
| 1 | 1 |
#include "amiro_map.hpp" |
| 2 |
#include <cstdint> |
|
| 2 | 3 |
|
| 3 | 4 |
|
| 4 | 5 |
void AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
|
| 5 | 6 |
|
| 6 | 7 |
// Clear old values in case the map is initialized again |
| 8 |
uint8_t visitTable[MAX_NODES]; |
|
| 7 | 9 |
this->current = 0; |
| 8 | 10 |
this->next = 0; |
| 9 | 11 |
this->valid = false; |
| ... | ... | |
| 11 | 13 |
// convert proto map to internal representation |
| 12 | 14 |
for (int i=0; i<MAX_NODES; i++){
|
| 13 | 15 |
if(config[i][2] == 0xff && i != 0){
|
| 14 |
this->valid = true; |
|
| 15 | 16 |
break; |
| 16 | 17 |
} else if (config[i][2] == 0xff && i == 0) {
|
| 17 | 18 |
this->valid = false; |
| ... | ... | |
| 31 | 32 |
} |
| 32 | 33 |
|
| 33 | 34 |
// TODO make validity check |
| 35 |
bool all_visited = true; |
|
| 36 |
for (int j=0; j<nodeCount; j++) {
|
|
| 37 |
visitNode(j); |
|
| 38 |
for (int k = 0; k < nodeCount; k++) {
|
|
| 39 |
if (nodeList[k].visited == 1) {
|
|
| 40 |
nodeList[k].visited = 0; |
|
| 41 |
} else {
|
|
| 42 |
this->valid = false; |
|
| 43 |
return; |
|
| 44 |
} |
|
| 45 |
} |
|
| 46 |
} |
|
| 34 | 47 |
|
| 48 |
this->valid = true; |
|
| 49 |
} |
|
| 35 | 50 |
|
| 51 |
void AmiroMap::visitNode(uint8_t id){
|
|
| 52 |
if (this->nodeList[id].visited > 0){
|
|
| 53 |
return; |
|
| 54 |
}else{
|
|
| 55 |
nodeList[id].visited++; |
|
| 56 |
visitNode(this->nodeList[id].left); |
|
| 57 |
visitNode(this->nodeList[id].right); |
|
| 58 |
} |
|
| 36 | 59 |
} |
| 37 | 60 |
|
| 61 |
|
|
| 38 | 62 |
void AmiroMap::update(bool left, bool right, LineFollowStrategy strategy) {
|
| 39 | 63 |
|
| 40 | 64 |
} |
Also available in: Unified diff