amiro-os / devices / DiWheelDrive / amiro_map.cpp @ d2230e6e
History | View | Annotate | Download (1.44 KB)
1 |
#include "amiro_map.hpp" |
---|---|
2 |
#include <cstdint> |
3 |
|
4 |
|
5 |
uint8_t AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){ |
6 |
|
7 |
// Clear old values in case the map is initialized again
|
8 |
uint8_t visitTable[MAX_NODES]; |
9 |
this->current = 0; |
10 |
this->next = 0; |
11 |
this->valid = false; |
12 |
|
13 |
// convert proto map to internal representation
|
14 |
for (int i=0; i<MAX_NODES; i++){ |
15 |
if(config[i][2] == 0xff && i != 0){ |
16 |
break;
|
17 |
} else if (config[i][2] == 0xff && i == 0) { |
18 |
this->valid = false; |
19 |
return 255; |
20 |
} |
21 |
|
22 |
//look for start node (default is Node 0)
|
23 |
if (config[i][2] == 1 ) { |
24 |
this->current= i;
|
25 |
} |
26 |
|
27 |
this->nodeList[i].id = i;
|
28 |
this->nodeList[i].left = config[i][0]; |
29 |
this->nodeList[i].right = config[i][1]; |
30 |
this->nodeList[i].flag = config[i][2]; |
31 |
this->nodeCount++;
|
32 |
} |
33 |
|
34 |
// TODO make validity check
|
35 |
|
36 |
for (int j=0; j<nodeCount; j++) { |
37 |
this->nodeList[j].visited = 0; |
38 |
visitNode(j); |
39 |
for (int k = 0; k < nodeCount; k++) { |
40 |
if (this->nodeList[k].visited == 1) { |
41 |
this->nodeList[k].visited = 0; |
42 |
} else {
|
43 |
this->valid = false; |
44 |
return k;
|
45 |
} |
46 |
} |
47 |
} |
48 |
|
49 |
this->valid = true; |
50 |
return 42; |
51 |
} |
52 |
|
53 |
void AmiroMap::visitNode(uint8_t id){
|
54 |
if (this->nodeList[id].visited == 1){ |
55 |
return;
|
56 |
}else{
|
57 |
nodeList[id].visited = 1;
|
58 |
visitNode(this->nodeList[id].left);
|
59 |
visitNode(this->nodeList[id].right);
|
60 |
} |
61 |
} |
62 |
|
63 |
|
64 |
void AmiroMap::update(bool left, bool right, LineFollowStrategy strategy) { |
65 |
|
66 |
} |