Revision d2230e6e devices/DiWheelDrive/amiro_map.cpp
| devices/DiWheelDrive/amiro_map.cpp | ||
|---|---|---|
| 2 | 2 |
#include <cstdint> |
| 3 | 3 |
|
| 4 | 4 |
|
| 5 |
void AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
|
|
| 5 |
uint8_t AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
|
|
| 6 | 6 |
|
| 7 | 7 |
// Clear old values in case the map is initialized again |
| 8 | 8 |
uint8_t visitTable[MAX_NODES]; |
| ... | ... | |
| 16 | 16 |
break; |
| 17 | 17 |
} else if (config[i][2] == 0xff && i == 0) {
|
| 18 | 18 |
this->valid = false; |
| 19 |
return; |
|
| 19 |
return 255;
|
|
| 20 | 20 |
} |
| 21 | 21 |
|
| 22 | 22 |
//look for start node (default is Node 0) |
| ... | ... | |
| 32 | 32 |
} |
| 33 | 33 |
|
| 34 | 34 |
// TODO make validity check |
| 35 |
bool all_visited = true; |
|
| 35 |
|
|
| 36 | 36 |
for (int j=0; j<nodeCount; j++) {
|
| 37 |
this->nodeList[j].visited = 0; |
|
| 37 | 38 |
visitNode(j); |
| 38 | 39 |
for (int k = 0; k < nodeCount; k++) {
|
| 39 |
if (nodeList[k].visited == 1) {
|
|
| 40 |
nodeList[k].visited = 0; |
|
| 40 |
if (this->nodeList[k].visited == 1) {
|
|
| 41 |
this->nodeList[k].visited = 0;
|
|
| 41 | 42 |
} else {
|
| 42 | 43 |
this->valid = false; |
| 43 |
return; |
|
| 44 |
return k;
|
|
| 44 | 45 |
} |
| 45 | 46 |
} |
| 46 | 47 |
} |
| 47 | 48 |
|
| 48 | 49 |
this->valid = true; |
| 50 |
return 42; |
|
| 49 | 51 |
} |
| 50 | 52 |
|
| 51 | 53 |
void AmiroMap::visitNode(uint8_t id){
|
| 52 |
if (this->nodeList[id].visited > 0){
|
|
| 54 |
if (this->nodeList[id].visited == 1){
|
|
| 53 | 55 |
return; |
| 54 | 56 |
}else{
|
| 55 |
nodeList[id].visited++;
|
|
| 57 |
nodeList[id].visited = 1;
|
|
| 56 | 58 |
visitNode(this->nodeList[id].left); |
| 57 | 59 |
visitNode(this->nodeList[id].right); |
| 58 | 60 |
} |
Also available in: Unified diff