Revision a3c54343

View differences:

devices/DiWheelDrive/amiro_map.cpp
127 127 
  // Update travel distance
128 128 
  // check if the nodes of the specific strategy where visited
129 129 
  if (state.strategy
130 
      == (nodeList[state.current].visited & nodeList[state.next].visited)) {
130 
      == nodeList[state.current].visited) {
131 131 
    flag |= 0x8;
132 132 
    // only update distance if both nodes were visited
133 133 
    // Calculate estimated length of the edge
134 134 
    if (state.strategy == 0x01) {
135 135 
      // Amiro is driving on the right edge
136 
      if (state.eLength == 0) {
136 
      // only calculate edge length if the node is already vivited
137 
      if ((state.eLength == 0) && (state.strategy == nodeList[state.current].visited)) {
137 138 
        state.eLength = calculateDist(&nodeList[state.next].pR,
138 139 
                                      &nodeList[state.current].pR);
139 140 
      }
140 
      state.dist = calculateDist(&nodeList[state.current].pR, &currentPos) * 100 /
141 
                   state.eLength;
141 
      state.dist = calculateDist(&nodeList[state.current].pR, &currentPos);
142 142 
    } else {
143 143 
      // Driving on the left edge
144 
      if (state.eLength == 0) {
145 
        state.eLength =
146 
            calculateDist(&nodeList[state.next].pL, &nodeList[state.current].pL);
144 
      if ((state.eLength == 0) &&
145 
          (state.strategy == nodeList[state.current].visited)) {
146 
        state.eLength = calculateDist(&nodeList[state.next].pR,
147 
                                      &nodeList[state.current].pR);
147 148 
      }
148 
      state.dist = calculateDist(&nodeList[state.current].pL, &currentPos) * 100 / state.eLength;
149 
      state.dist = calculateDist(&nodeList[state.current].pL, &currentPos);
149 150 

  		




  150
  151
  
    
    }


  		




  151
  152
  
    
  }


  		




  ......




  156
  157
  
    
  return (uint32_t)  sqrt(pow((p2->x - p1->x)/10000, 2) +


  		




  157
  158
  
    
                          pow((p2->y - p1->y)/10000, 2));


  		




  158
  159
  
    
}


  		




  
  160
  
    

  		




  
  161
  
    
uint8_t trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy, ut_states state){


  		




  
  162
  
    

  		




  
  163
  
    
}


  		












  

    
      devices/DiWheelDrive/amiro_map.hpp
    
  





  5
  5
  
    
#include "linefollow.hpp"


  		




  6
  6
  
    
#include <amiroosconf.h>


  		




  7
  7
  
    
#include <ch.hpp>


  		




  
  8
  
    
#include <cstdint>


  		




  8
  9
  
    
#include <math.h>


  		




  9
  10
  
    

  		




  10
  11
  
    

  		




  ......




  39
  40
  
    
class AmiroMap {


  		




  40
  41
  
    
public:


  		




  41
  42
  
    
  map_state * get_state() { return &state; }


  		




  
  43
  
    
  uint8_t getNodeCount(){ return nodeCount; }


  		




  
  44
  
    
  node * getNodeList(){return nodeList; }


  		




  
  45
  
    

  		




  42
  46
  
    

  		




  43
  47
  
    
  AmiroMap(Global *global) : global{global} {}


  		




  44
  48
  
    

  		




  ......




  64
  68
  
    
   */


  		




  65
  69
  
    
  uint8_t update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy);


  		




  66
  70
  
    

  		




  
  71
  
    

  		




  
  72
  
    
  /**


  		




  
  73
  
    
    If this is called instead of update new fixpoints can automativally


  		




  
  74
  
    
    get detected and will be added to the current nodeList.


  		




  
  75
  
    
    Internally the update will be called.


  		




  
  76
  
    
    If there are no nodes in the node list they will be created.


  		




  
  77
  
    
   */


  		




  
  78
  
    
  uint8_t trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy, ut_states state);


  		




  
  79
  
    

  		




  67
  80
  
    
private:


  		




  68
  81
  
    
  Global *global;


  		




  69
  82
  
    
  LineFollowStrategy lfStrategy = LineFollowStrategy::EDGE_RIGHT;


  		












Also available in:  Unified diff