Revision 4d54a507 devices/DiWheelDrive/amiro_map.cpp

View differences:

devices/DiWheelDrive/amiro_map.cpp
20 20 
      return 255;
21 21 
    }
22 22 

  		




  23
  
  
    
    //look for start node (default is Node 0)


  		




  24
  
  
    
    if (global->testmap[i][2] == 1 ) {


  		




  25
  
  
    
      this->state.current = i;


  		




  26
  
  
    
    }


  		




  
  23
  
    
    addNode(global->testmap[i][0], global->testmap[i][1],


  		




  
  24
  
    
            global->testmap[i][2]);


  		




  27
  25
  
    

  		




  28
  
  
    
    this->nodeList[i].id = i;


  		




  29
  
  
    
    this->nodeList[i].edge.edge_id.left = global->testmap[i][0];


  		




  30
  
  
    
    this->nodeList[i].edge.edge_id.right = global->testmap[i][1];


  		




  31
  
  
    
    this->nodeList[i].flag = global->testmap[i][2];


  		




  32
  
  
    
    this->nodeCount++;


  		




  
  26
  
    
    // look for start node (default is Node 0)


  		




  
  27
  
    
    if (global->testmap[i][2] == 1) {


  		




  
  28
  
    
      this->state.current = this->nodeCount;


  		




  
  29
  
    
    }


  		




  33
  30
  
    
  }


  		




  34
  
  
    
  this->state.next = this->nodeList[this->state.current].edge.edge_id.right;


  		




  
  31
  
    
  // Set the next node according to the current strategy


  		




  
  32
  
    
  this->state.next = this->nodeList[this->state.current].edge.arr[state.strategy-1];


  		




  35
  33
  
    

  		




  36
  34
  
    
  // TODO make validity check


  		




  37
  
  
    

  		




  38
  35
  
    
  for (int j=0; j<nodeCount; j++) {


  		




  39
  36
  
    
    this->nodeList[j].visited = 0;


  		




  40
  37
  
    
    visitNode(j);


  		




  ......




  209
  206
  
    
  state.next = 255;


  		




  210
  207
  
    
}


  		




  211
  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
  
    
}


  		




  212
  217
  
    

  		




  213
  
  
    
uint8_t AmiroMap::getNearest(types::position *p1) {


  		




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


  		




  214
  219
  
    

  		




  215
  220
  
    
  uint8_t actualStrategy = this->lfStrategy == EDGE_LEFT ? 1 : 2;


  		




  216
  221
  
    
  uint32_t thresh = global->nodeDistThresh;          // TODO: find good thresh value in cm


  		




  ......




  278
  283
  
    
  this->nodeCount = 0;


  		




  279
  284
  
    
  this->state.current = 0;


  		




  280
  285
  
    
  this->state.next= 0;


  		




  281
  
  
    

  		




  
  286
  
    
  this->state.valid = false;


  		




  
  287
  
    
  this->state.strategy = this->lfStrategy == EDGE_LEFT ? 1 : 2;


  		




  282
  288
  
    
}


  		












Also available in:  Unified diff