Revision 2af9778e

View differences:

devices/DiWheelDrive/amiro_map.cpp
1 1
#include "amiro_map.hpp"
2
#include "linefollow.hpp"
3
#include <cstdint>
2 4

  
3 5
uint8_t AmiroMap::initialize(){
4 6

  
......
161 163
  if (nodeCount == 0) {
162 164
    createInitNode();
163 165
  }
164

  
166
  this->lfStrategy = strategy;
165 167
  bool left = global->linePID.BThresh >= WL;
166 168
  bool right = global->linePID.BThresh >= WR;
167 169
  types::position currentPos = global->odometry.getPosition();
......
241 243
  state.next = 255;
242 244
}
243 245

  
246

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

  
249
  uint8_t actualStrategy = this->lfStrategy == EDGE_LEFT ? 1 : 2;
250
  uint32_t thresh = 1;          // TODO: find good thresh value in cm
251
  uint8_t id = 255;
252
  uint32_t smallestDist = thresh;
253
  uint8_t currentStrategy;
254
  // Calculate the point which is nearest to the current one
255
  // check if distance and strategy match
256
  // If right point is found but no left point set choose this as the fitting point
257
  // Check how point was visited before calculating distance (non visited points are always (0,0))
258

  
259
  for (int i = 0; i < nodeCount; i++) {
260
    for (int j = 0; j < 2; j++){ // Iterate over l and r point
261
      if ((nodeList[i].visited & (j+1)) == 0){
262
        // Skip point if it was not visited for the given strategy
263
        continue;
264
      }
265

  
266
      uint32_t tmpDist = calculateDist(&nodeList[i].p.arr[j], p1);
267
      if (tmpDist < smallestDist){
268
        smallestDist = tmpDist;
269
        id = i;
270
        // Store strategy to match the correct point at the end
271
        currentStrategy = j;
272
      }
273
    }
274
  }
275

  
276
  if (id == 255){
277
    return 255;
278
  }
279

  
280
  // update point at fixpoint if it is not visited
281
  if ((nodeList[id].visited & actualStrategy) == 0){
282
    copyPoint(p1, &nodeList[id].p.arr[actualStrategy]);
283
    nodeList[id].visited |= actualStrategy;
284
  } // else point was already visited and is assigned
285

  
286
  return id;
247 287
}
248 288

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

Also available in: Unified diff