Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / amiro_map.cpp @ 81b48c66

History | View | Annotate | Download (8.25 KB)

1
#include "amiro_map.hpp"
2

    
3
uint8_t AmiroMap::initialize(){
4

    
5
  // Clear old values in case the map is initialized again
6
  this->state.current = 0;
7
  this->state.next = 0;
8
  this->state.valid = false;
9
  this->nodeCount = 0;
10
  this->state.strategy = 0x1;
11

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

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

    
26
    this->nodeList[i].id = i;
27
    this->nodeList[i].left = global->testmap[i][0];
28
    this->nodeList[i].right = global->testmap[i][1];
29
    this->nodeList[i].flag = global->testmap[i][2];
30
    this->nodeCount++;
31
  }
32
  this->state.next = this->nodeList[this->state.current].right;
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->state.valid = false;
44
        return k;
45
      }
46
    }
47
  }
48

    
49
  this->state.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
uint8_t AmiroMap::update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy) {
64
  // Called each time at the end of the user thread state machine
65
  // The bottom sensors will be checked for black ground which is interpreted as
66
  // filxpoint
67

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

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

    
80
  if (left && right) {
81
    // TODO A dangerous case -> amiro could be lifted
82
    flag |= 255;
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
    copyPoint(&currentPos, &nodeList[state.next].pR);
90
    nodeList[state.next].visited |= 0x01;
91
    state.current = state.next;
92
    state.next = nodeList[state.current].right;
93
    state.strategy = 0x01;
94
    state.eLength = 0; // Reset length to get recalculated after fixpoint
95
    flag |= 0x1;
96
  }
97
  else if (right && !rightDetected) {
98
    // Same as left only for the right sensor.
99
    rightDetected = true;
100
    copyPoint(&currentPos, &nodeList[state.next].pR);
101
    nodeList[state.next].visited |= 0x02;
102
    state.current = state.next;
103
    state.next = nodeList[state.current].left;
104
    state.strategy = 0x2;
105
    state.eLength = 0; // Reset length to get recalculated after fixpoint
106
    flag |= 0x2;
107
  }
108
  else if (!left && !right) {
109
    // in case the fixpoint is not detected anymore
110
    leftDetected = false;
111
    rightDetected = false;
112
    flag |= 0x4;
113
  }
114

    
115

    
116
  // update internal map_state
117
  // Update travel distance
118
  // check if the nodes of the specific strategy where visited
119
  if (state.strategy
120
      == nodeList[state.current].visited) {
121
    flag |= 0x8;
122
    // only update distance if both nodes were visited
123
    // Calculate estimated length of the edge
124
    if (state.strategy == 0x01) {
125
      // Amiro is driving on the right edge
126
      // only calculate edge length if the node is already vivited
127
      if ((state.eLength == 0) && (state.strategy == nodeList[state.current].visited)) {
128
        state.eLength = calculateDist(&nodeList[state.next].pR,
129
                                      &nodeList[state.current].pR);
130
      }
131
      state.dist = calculateDist(&nodeList[state.current].pR, &currentPos);
132
    } else {
133
      // Driving on the left edge
134
      if ((state.eLength == 0) &&
135
          (state.strategy == nodeList[state.current].visited)) {
136
        state.eLength = calculateDist(&nodeList[state.next].pR,
137
                                      &nodeList[state.current].pR);
138
      }
139
      state.dist = calculateDist(&nodeList[state.current].pL, &currentPos);
140

    
141
    }
142
  }
143
  return flag;
144
}
145

    
146
uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) {
147
  return (uint32_t)  sqrt(pow((p2->x - p1->x)/10000, 2) +
148
                          pow((p2->y - p1->y)/10000, 2));
149
}
150

    
151
uint8_t AmiroMap::trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy,
152
                    ut_states ut_state) {
153
  // Check if map is valid
154
  if (this->state.valid){
155
    return update(WL, WR, strategy);
156
  }
157

    
158
  // Create init node if none is there
159
  // We will not assign a point to the initial fixpoint because it is not clear if
160
  // start position is at the correct point
161
  if (nodeCount == 0) {
162
    createInitNode();
163
  }
164

    
165
  bool left = global->linePID.BThresh >= WL;
166
  bool right = global->linePID.BThresh >= WR;
167
  types::position currentPos = global->odometry.getPosition();
168

    
169
  // Assign fixpoint if side sensor is black
170
  // Do not update if update was already applied the round before (leftDetected || rightDetected) == true
171
  if ((left || right) && !(leftDetected || rightDetected)) {
172
    // Determine what strategy to use
173
    // assignFxp() will use strategy to assign the next point
174
    state.strategy = right ? 1 : 2;
175

    
176
    // Check if next point is reachable
177
    if (state.next == 255){
178
      // Prepare state values for switch
179
      assignFxp(&currentPos);
180
    }
181

    
182
  }else if (!(left || right)) {
183
    // TODO: do we need both?
184
    leftDetected = rightDetected = true;
185
  }
186
}
187

    
188
void AmiroMap::calTravelState(types::position *p1) {
189
  // Calculate the moved distance from last detected  fixpoint
190
  state.dist = calculateDist(p1, &nodeList[state.current].p.arr[state.strategy - 1]);
191

    
192
  // Calculate elength if it is 0
193
  // and if the point of the next node was visited before
194
  if ((state.eLength == 0) && ((state.strategy & nodeList[state.next].visited) == 1)) {
195
    state.eLength =
196
        calculateDist(p1, &nodeList[state.current].p.arr[state.strategy - 1]);
197
  }
198
}
199

    
200
void AmiroMap::checkMap() {
201
  // The check will basically only consist in checking if all nodes
202
  // are connected to following nodes
203

    
204
  for(int i=0; i < nodeCount; i++){
205
    for(int j=0; j < nodeCount; j++)
206
      if(nodeList[i].edge.arr[j] == 255){
207
        state.valid = false;
208
        return;
209
      }
210
  }
211
  state.valid = true;
212
}
213

    
214

    
215
void AmiroMap::switchToNext(types::position *p1) {
216

    
217
  // Update point if node was not visited before
218
  if ((nodeList[state.next].visited & state.strategy) == 0){
219
      copyPoint(p1, &nodeList[state.next].p.arr[state.strategy - 1]);
220
      nodeList[state.next].visited |= state.strategy;
221
  }
222

    
223
  leftDetected = true;
224
  state.current = state.next;
225
  state.next = nodeList[state.current].edge.arr[state.strategy - 1];
226
  state.eLength = 0; // Reset length to get recalculated after fixpoint
227
  return;
228
}
229

    
230

    
231
void AmiroMap::copyPoint(types::position* from, types::position* to) {
232
  to->x = from->x;
233
  to->y = from->y;
234
  to->f_x = from->f_x;
235
}
236

    
237

    
238
void AmiroMap::createInitNode() {
239
  this->nodeCount = 0;
240
  state.current = addNode(255, 255, 1);
241
  state.next = 255;
242
}
243

    
244
uint8_t AmiroMap::getNearest(types::position *p1) {
245
  uint8_t b;
246
  return b;
247
}
248

    
249
uint8_t AmiroMap::assignFxp(types::position *p1) {
250

    
251
  // Magic happens to determine if fixpoint is close enough
252
  uint8_t id = getNearest(p1);
253
  if(id < 255){
254
    // strategy is either 1 - right or 2 - left
255
    // Copy current point to either left or right point
256
    copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);
257
    }else {
258
    // A new fixpoint needs to be created
259
    id = addNode(255, 255, 0);
260
    copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);
261
  }
262

    
263
  // Prepare values for switching
264
  state.next = id;
265
  nodeList[state.current].edge.arr[state.strategy - 1] = id;
266
  // Mark point as visited
267
  nodeList[id].visited |= state.strategy;
268
  return id;
269
}