Statistics
| Branch: | Tag: | Revision:

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
}