Revision a07a7a1c
| 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 |
} |
| devices/DiWheelDrive/amiro_map.hpp | ||
|---|---|---|
| 17 | 17 |
uint8_t flag; |
| 18 | 18 |
uint8_t left; |
| 19 | 19 |
uint8_t right; |
| 20 |
uint8_t visited; |
|
| 20 | 21 |
types::position pL; // Left |
| 21 | 22 |
types::position pR; // Right |
| 22 | 23 |
types::position pB; // Back |
| ... | ... | |
| 73 | 74 |
node nodeList[MAX_NODES]; |
| 74 | 75 |
uint8_t current = 0; |
| 75 | 76 |
uint8_t next = 0; |
| 76 |
}; |
|
| 77 |
void visitNode(uint8_t id); |
|
| 78 |
}; |
|
| 77 | 79 |
}; // namespace amiro |
| 78 | 80 |
|
| 79 | 81 |
#endif /* AMIRO_MAP */ |
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 1001 | 1001 |
map.initialize(testmap); |
| 1002 | 1002 |
testres[tcase] = map.get_current() == 2; |
| 1003 | 1003 |
|
| 1004 |
tcase++; // 3 |
|
| 1005 |
setAttributes(testmap, 3, 0, 0, 0); |
|
| 1006 |
setAttributes(testmap, 4, 0, 0, 0xff); |
|
| 1007 |
map.initialize(testmap); |
|
| 1008 |
testres[tcase] = !map.get_valid(); |
|
| 1009 |
|
|
| 1010 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1004 | 1011 |
// -------------------------------------------------- |
| 1005 | 1012 |
|
| 1006 | 1013 |
int failed = 0; |
| ... | ... | |
| 1008 | 1015 |
for (int i=0; i<=tcase; i++) {
|
| 1009 | 1016 |
if (testres[i]){
|
| 1010 | 1017 |
passed++; |
| 1018 |
chprintf(chp, "Test %d Passed!\n", i); |
|
| 1011 | 1019 |
}else{
|
| 1012 | 1020 |
failed++; |
| 1013 | 1021 |
chprintf(chp, "Test %d Failed\n", i); |
| 1014 | 1022 |
} |
| 1023 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 1015 | 1024 |
} |
| 1016 | 1025 |
chprintf(chp, "Total: %d, Passed: %d, Failed: %d\n", tcase + 1, passed, failed); |
| 1017 | 1026 |
} |
Also available in: Unified diff