Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / amiro_map.cpp @ 4d54a507

History | View | Annotate | Download (8.48 KB)

1
#include "amiro_map.hpp"
2
#include "linefollow.hpp"
3
#include <cstdint>
4

    
5
uint8_t AmiroMap::initialize(){
6

    
7
  // Clear old values in case the map is initialized again
8
  this->state.current = 0;
9
  this->state.next = 0;
10
  this->state.valid = false;
11
  this->nodeCount = 0;
12
  this->state.strategy = 0x1;
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
    addNode(global->testmap[i][0], global->testmap[i][1],
24
            global->testmap[i][2]);
25

    
26
    // look for start node (default is Node 0)
27
    if (global->testmap[i][2] == 1) {
28
      this->state.current = this->nodeCount;
29
    }
30
  }
31
  // Set the next node according to the current strategy
32
  this->state.next = this->nodeList[this->state.current].edge.arr[state.strategy-1];
33

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

    
48
  this->state.valid = true;
49
  return 42;
50
}
51

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

    
62
uint8_t AmiroMap::update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy) {
63
  // Called each time at the end of the user thread state machine
64
  // The bottom sensors will be checked for black ground which is interpreted as
65
  // filxpoint
66

    
67
  // set the strategy directly, actually there is no need to store that variable in the class
68
  // but we will go with it for now to initialize everything properly.
69
  uint8_t flag = 0;
70
  this->lfStrategy = strategy;
71
  // Check the wheel sensors
72
  bool left = global->linePID.BThresh >= WL;
73
  bool right = global->linePID.BThresh >= WR;
74
  types::position currentPos = global->odometry.getPosition();
75

    
76
  if (left && right) {
77
    // TODO A dangerous case -> amiro could be lifted
78
    flag |= 255;
79
  }
80
  else if (left && !fxpDetected) { // Driving on the right edge
81
    // The sensor on the left side of the Amiro is driving on black
82
    // To prevent continous fixpoint detection a point needs to be marked as currently detected
83
    // and released.
84
    state.strategy = 0x02;
85
    switchToNext(&currentPos);
86
    flag |= 0x2;
87
  }
88
  else if (right && !fxpDetected) { // Driving on the left edge
89
    // Same as left only for the right sensor.
90
    state.strategy = 0x01;
91
    switchToNext(&currentPos);
92
    flag |= 0x1;
93
  }
94
  else if (!left && !right) {
95
    // in case the fixpoint is not detected anymore
96
    fxpDetected = false;
97
    flag |= 0x4;
98
  }
99

    
100
  // Update dist and edge length if possible
101
  calTravelState(&currentPos);
102

    
103
  return flag;
104
}
105

    
106
uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) {
107
  return (uint32_t)  sqrt(pow((p2->x - p1->x)/10000, 2) +
108
                          pow((p2->y - p1->y)/10000, 2));
109
}
110

    
111
uint8_t AmiroMap::trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy,
112
                    ut_states ut_state) {
113
  // Check if map is valid
114
  if (this->state.valid){
115
    return update(WL, WR, strategy);
116
  }
117

    
118
  // Create init node if none is there
119
  // We will not assign a point to the initial fixpoint because it is not clear if
120
  // start position is at the correct point
121
  if (nodeCount == 0) {
122
    createInitNode();
123
  }
124
  this->lfStrategy = strategy;
125
  bool left = global->linePID.BThresh >= WL;
126
  bool right = global->linePID.BThresh >= WR;
127
  types::position currentPos = global->odometry.getPosition();
128

    
129
  // Assign fixpoint if side sensor is black
130
  // Do not update if update was already applied the round before (|| rightDetected) == true
131
  if ((left || right) && !fxpDetected) {
132
    // Determine what strategy to use
133
    // assignFxp() will use strategy to assign the next point
134
    state.strategy = left ? 1 : 2;
135

    
136
    // Check if next point is reachable
137
    if (state.next == 255){
138
      // Prepare state values for switch
139
      assignFxp(&currentPos);
140
    }
141

    
142
  }else if (!(left || right)) {
143
    // TODO: do we need both?
144
    fxpDetected = true;
145
  }
146
}
147

    
148
void AmiroMap::calTravelState(types::position *p1) {
149
  // Calculate the moved distance from last detected  fixpoint
150
  // if current was visited
151
  if ((state.strategy & nodeList[state.current].visited) > 0) {
152
    state.dist =
153
        calculateDist(p1, &nodeList[state.current].p.arr[state.strategy - 1]);
154
  }
155
    // Calculate elength if it is 0
156
    // and if the point of the next node was visited before
157
    if ((state.eLength == 0) &&
158
        ((state.strategy & nodeList[state.next].visited) > 0)) {
159
      state.eLength =
160
          calculateDist(&nodeList[state.current].p.arr[state.strategy - 1],
161
                        &nodeList[state.next].p.arr[state.strategy - 1]);
162
    }
163
  }
164

    
165
void AmiroMap::checkMap() {
166
  // The check will basically only consist in checking if all nodes
167
  // are connected to following nodes
168

    
169
  for(int i=0; i < nodeCount; i++){
170
    for(int j=0; j < nodeCount; j++)
171
      if(nodeList[i].edge.arr[j] == 255){
172
        state.valid = false;
173
        return;
174
      }
175
  }
176
  state.valid = true;
177
}
178

    
179

    
180
void AmiroMap::switchToNext(types::position *p1) {
181

    
182
  // Update point if node was not visited before
183
  if ((nodeList[state.next].visited & state.strategy) == 0){
184
      copyPoint(p1, &nodeList[state.next].p.arr[state.strategy - 1]);
185
      nodeList[state.next].visited |= state.strategy;
186
  }
187

    
188
  fxpDetected = true;
189
  state.current = state.next;
190
  state.next = nodeList[state.current].edge.arr[state.strategy - 1];
191
  state.eLength = 0; // Reset length to get recalculated after fixpoint
192
  return;
193
}
194

    
195

    
196
void AmiroMap::copyPoint(types::position* from, types::position* to) {
197
  to->x = from->x;
198
  to->y = from->y;
199
  to->f_x = from->f_x;
200
}
201

    
202

    
203
void AmiroMap::createInitNode() {
204
  this->nodeCount = 0;
205
  state.current = addNode(255, 255, 1);
206
  state.next = 255;
207
}
208

    
209
uint8_t AmiroMap::addNode(uint8_t l, uint8_t r, uint8_t flags){
210
  uint8_t id = nodeCount;
211
  nodeCount++;
212
  nodeList[id].edge.edge_id.left = l;
213
  nodeList[id].edge.edge_id.right = r;
214
  nodeList[id].flag = flags;
215
  return id;
216
}
217

    
218
    uint8_t AmiroMap::getNearest(types::position *p1) {
219

    
220
  uint8_t actualStrategy = this->lfStrategy == EDGE_LEFT ? 1 : 2;
221
  uint32_t thresh = global->nodeDistThresh;          // TODO: find good thresh value in cm
222
  uint8_t id = 255;
223
  uint32_t smallestDist = thresh;
224
  uint8_t currentStrategy;
225
  // Calculate the point which is nearest to the current one
226
  // check if distance and strategy match
227
  // If right point is found but no left point set choose this as the fitting point
228
  // Check how point was visited before calculating distance (non visited points are always (0,0))
229

    
230
  for (int i = 0; i < nodeCount; i++) {
231
    for (int j = 0; j < 2; j++){ // Iterate over l and r point
232
      if ((nodeList[i].visited & (j+1)) == 0){
233
        // Skip point if it was not visited for the given strategy
234
        continue;
235
      }
236

    
237
      uint32_t tmpDist = calculateDist(&nodeList[i].p.arr[j], p1);
238
      if (tmpDist < smallestDist){
239
        smallestDist = tmpDist;
240
        id = i;
241
        // Store strategy to match the correct point at the end
242
        currentStrategy = j;
243
      }
244
    }
245
  }
246

    
247
  if (id == 255){
248
    return 255;
249
  }
250

    
251
  // update point at fixpoint if it is not visited
252
  if ((nodeList[id].visited & actualStrategy) == 0){
253
    copyPoint(p1, &nodeList[id].p.arr[actualStrategy]);
254
    nodeList[id].visited |= actualStrategy;
255
  } // else point was already visited and is assigned
256

    
257
  return id;
258
}
259

    
260
uint8_t AmiroMap::assignFxp(types::position *p1) {
261

    
262
  // Magic happens to determine if fixpoint is close enough
263
  uint8_t id = getNearest(p1);
264
  if(id < 255){
265
    // strategy is either 1 - right or 2 - left
266
    // Copy current point to either left or right point
267
    copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);
268
    }else {
269
    // A new fixpoint needs to be created
270
    id = addNode(255, 255, 0);
271
    copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);
272
  }
273

    
274
  // Prepare values for switching
275
  state.next = id;
276
  nodeList[state.current].edge.arr[state.strategy - 1] = id;
277
  // Mark point as visited
278
  nodeList[id].visited |= state.strategy;
279
  return id;
280
}
281

    
282
void AmiroMap::reset(){
283
  this->nodeCount = 0;
284
  this->state.current = 0;
285
  this->state.next= 0;
286
  this->state.valid = false;
287
  this->state.strategy = this->lfStrategy == EDGE_LEFT ? 1 : 2;
288
}