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