Revision b8b3a9c9 devices/DiWheelDrive/amiro_map.cpp
| devices/DiWheelDrive/amiro_map.cpp | ||
|---|---|---|
| 1 | 1 |
#include "amiro_map.hpp" |
| 2 | 2 |
|
| 3 | 3 |
|
| 4 |
void AmiroMap::initialize(int8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
|
|
| 4 |
void AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
|
|
| 5 | 5 |
|
| 6 |
uint8_t nodeCount = 0; |
|
| 7 |
bool end = false; |
|
| 6 |
// Clear old values in case the map is initialized again |
|
| 7 |
this->current = 0; |
|
| 8 |
this->next = 0; |
|
| 9 |
this->valid = false; |
|
| 8 | 10 |
|
| 11 |
// convert proto map to internal representation |
|
| 9 | 12 |
for (int i=0; i<MAX_NODES; i++){
|
| 10 |
if(config[i][NODE_ATTRIBUTES - 1] == 255){
|
|
| 11 |
end = true;
|
|
| 13 |
if(config[i][2] == 0xff && i != 0){
|
|
| 14 |
this->valid = true;
|
|
| 12 | 15 |
break; |
| 16 |
} else if (config[i][2] == 0xff && i == 0) {
|
|
| 17 |
this->valid = false; |
|
| 18 |
return; |
|
| 13 | 19 |
} |
| 14 |
nodeCount++; |
|
| 15 |
} |
|
| 16 |
if (!end || nodeCount == 0 ){
|
|
| 17 |
|
|
| 18 |
// Failed to determine the amount of nodes |
|
| 19 |
return; |
|
| 20 |
} |
|
| 21 | 20 |
|
| 22 |
this->nodeList = (struct node*) malloc(nodeCount * sizeof(struct node)); |
|
| 21 |
//look for start node (default is Node 0) |
|
| 22 |
if (config[i][2] == 1 ) {
|
|
| 23 |
this->current= i; |
|
| 24 |
} |
|
| 23 | 25 |
|
| 24 |
|
|
| 26 |
this->nodeList[i].id = i; |
|
| 27 |
this->nodeList[i].left = config[i][0]; |
|
| 28 |
this->nodeList[i].right = config[i][1]; |
|
| 29 |
this->nodeList[i].flag = config[i][2]; |
|
| 30 |
this->nodeCount++; |
|
| 31 |
} |
|
| 25 | 32 |
|
| 26 |
}
|
|
| 33 |
// TODO make validity check
|
|
| 27 | 34 |
|
| 28 | 35 |
|
| 29 |
void AmiroMap::update(){
|
|
| 30 |
|
|
| 31 | 36 |
} |
| 32 | 37 |
|
| 38 |
void AmiroMap::update(bool left, bool right, LineFollowStrategy strategy) {
|
|
| 33 | 39 |
|
| 34 |
void AmiroMap::uninitialize(){
|
|
| 35 |
|
|
| 36 | 40 |
} |
| 37 |
|
|
Also available in: Unified diff