Revision d2230e6e
| 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 |
} |
| devices/DiWheelDrive/amiro_map.hpp | ||
|---|---|---|
| 50 | 50 |
* @param config map configuration array |
| 51 | 51 |
* |
| 52 | 52 |
*/ |
| 53 |
void initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]);
|
|
| 53 |
uint8_t initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]);
|
|
| 54 | 54 |
|
| 55 | 55 |
|
| 56 | 56 |
/** |
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 989 | 989 |
int tcase = 0; |
| 990 | 990 |
map.initialize(testmap); |
| 991 | 991 |
testres[tcase] = map.get_valid(); |
| 992 |
uint8_t ini = map.initialize(testmap); |
|
| 993 |
chprintf(chp, "Initialized returned: %d \n", ini); |
|
| 994 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 992 | 995 |
|
| 993 | 996 |
tcase++; // 1 |
| 994 | 997 |
setAttributes(testmap, 0, 1, 2, 0xff); |
| 995 |
map.initialize(testmap); |
|
| 998 |
ini = map.initialize(testmap); |
|
| 999 |
chprintf(chp, "Initialized returned: %d \n", ini); |
|
| 996 | 1000 |
testres[tcase] = !map.get_valid(); |
| 1001 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 997 | 1002 |
|
| 998 | 1003 |
tcase++; // 2 |
| 999 | 1004 |
setAttributes(testmap, 0, 1, 2, 0); |
| 1000 | 1005 |
setAttributes(testmap, 2, 1, 0, 1); |
| 1001 | 1006 |
map.initialize(testmap); |
| 1002 | 1007 |
testres[tcase] = map.get_current() == 2; |
| 1008 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1003 | 1009 |
|
| 1004 | 1010 |
tcase++; // 3 |
| 1005 | 1011 |
setAttributes(testmap, 3, 0, 0, 0); |
Also available in: Unified diff