Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / amiro_map.cpp @ 7520e117

History | View | Annotate | Download (4.38 KB)

1
#include "amiro_map.hpp"
2

    
3
// #include <cstdint>
4
// #include <math.h>
5

    
6

    
7
uint8_t AmiroMap::initialize(){
8

    
9
  // Clear old values in case the map is initialized again
10
  this->state.current = 0;
11
  this->state.next = 0;
12
  this->state.valid = false;
13

    
14
  // convert proto map to internal representation
15
  for (int i=0; i<MAX_NODES; i++){
16
    if(global->testmap[i][2] == 0xff && i != 0){
17
      break;
18
    } else if (global->testmap[i][2] == 0xff && i == 0) {
19
      this->state.valid = false;
20
      return 255;
21
    }
22

    
23
    //look for start node (default is Node 0)
24
    if (global->testmap[i][2] == 1 ) {
25
      this->state.current = i;
26
    }
27

    
28
    this->nodeList[i].id = i;
29
    this->nodeList[i].left = global->testmap[i][0];
30
    this->nodeList[i].right = global->testmap[i][1];
31
    this->nodeList[i].flag = global->testmap[i][2];
32
    this->nodeCount++;
33
  }
34

    
35
  // TODO make validity check
36

    
37
  for (int j=0; j<nodeCount; j++) {
38
    this->nodeList[j].visited = 0;
39
    visitNode(j);
40
    for (int k = 0; k < nodeCount; k++) {
41
      if (this->nodeList[k].visited == 1) {
42
        this->nodeList[k].visited = 0;
43
      } else {
44
        this->state.valid = false;
45
        return k;
46
      }
47
    }
48
  }
49

    
50
  this->state.valid = true;
51
  return 42;
52
}
53

    
54
void AmiroMap::visitNode(uint8_t id){
55
  if (this->nodeList[id].visited == 1){
56
    return;
57
  }else{
58
    nodeList[id].visited = 1;
59
    visitNode(this->nodeList[id].left);
60
    visitNode(this->nodeList[id].right);
61
  }
62
}
63

    
64
void AmiroMap::update(LineFollowStrategy strategy) {
65
  // Each time at the end of the user thread state maschine, when the strategy
66
  // is changed or a fixpoint is detected, this method gets called.
67
  // The strategy change only has an effect on the fixpoint or in particular if
68
  // a fixpoint is detected.
69

    
70
  // set the strategy directly, actually there is no need to store that variable in the class
71
  // but we will go with it for now to initialize everything properly.
72
  this->lfStrategy = strategy;
73
  uint16_t WL = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
74
  uint16_t WR = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
75

    
76
  // Check the wheel sensors
77
  bool left = global->linePID.BThresh >= WL;
78
  bool right = global->linePID.BThresh >= WR;
79
  types::position currentPos = global->odometry.getPosition();
80

    
81
  if (left && right) {
82
    // TODO A dangerous case -> amiro could be lifted
83
  }
84
  else if (left && !leftDetected) {
85
    // The sensor on the left side of the Amiro is driving on black
86
    // To prevent continous fixpoint detection a point needs to be marked as currently detected
87
    // and released.
88
    leftDetected = true;
89
    nodeList[state.next].pR.f_x = currentPos.f_x;
90
    nodeList[state.next].pR.f_y = currentPos.f_y;
91
    nodeList[state.next].pR.f_z = currentPos.f_z;
92
    nodeList[state.next].pR.x = currentPos.x;
93
    nodeList[state.next].pR.y = currentPos.y;
94
    nodeList[state.next].pR.z = currentPos.z;
95
    nodeList[state.next].visited |= 0x01;
96
    state.current = state.next;
97
    state.next = nodeList[state.current].right;
98
    state.strategy = 0x01;
99
  }
100
  else if (right && !rightDetected) {
101
    // Same as left only for the right sensor.
102
    rightDetected = true;
103
    nodeList[state.next].pL.f_x = currentPos.f_x;
104
    nodeList[state.next].pL.f_y = currentPos.f_y;
105
    nodeList[state.next].pL.f_z = currentPos.f_z;
106
    nodeList[state.next].pL.x = currentPos.x;
107
    nodeList[state.next].pL.y = currentPos.y;
108
    nodeList[state.next].pL.z = currentPos.z;
109
    nodeList[state.next].visited |= 0x02;
110
    state.current = state.next;
111
    state.next = nodeList[state.current].left;
112
    state.strategy = 0x02;
113
  }
114
  else if (!left && !right) {
115
    // in case the fixpoint is not detected anymore
116
    leftDetected = false;
117
    rightDetected = false;
118
  }
119

    
120

    
121
  // update internal map_state
122
  // Update travel distance
123
  // check if the nodes of the specific strategy where visited
124
  // if (state.strategy
125
  //     == (nodeList[state.current].visited & nodeList[state.next].visited)) {
126
  //   // only update distance if both nodes were visited
127
  //   if (state.strategy == 0x01) {
128
  //     // Amiro is driving on the right edge
129
  //     state.dist = (uint8_t) sqrt(
130
  //                                 pow(nodeList[state.next].pR.x - nodeList[state.current].pR.x, 2)
131
  //                                 + pow(nodeList[state.next].pR.y - nodeList[state.current].pR.y, 2));
132
  //   } else {
133
  //     // Driving on the left edge
134

    
135
  //   }
136
  // }
137
}