Revision d02c536e devices/DiWheelDrive/amiro_map.cpp

View differences:

devices/DiWheelDrive/amiro_map.cpp
137 137 
        state.eLength = calculateDist(&nodeList[state.next].pR,
138 138 
                                      &nodeList[state.current].pR);
139 139 
      }
140 
      state.dist = calculateDist(&nodeList[state.next].pR, &currentPos) * 100 /
140 
      state.dist = calculateDist(&nodeList[state.current].pR, &currentPos) * 100 /
141 141 
                   state.eLength;
142 142 
    } else {
143 143 
      // Driving on the left edge
......
145 145 
        state.eLength =
146 146 
            calculateDist(&nodeList[state.next].pL, &nodeList[state.current].pL);
147 147 
      }
148 
      state.dist = calculateDist(&nodeList[state.next].pL, &currentPos) * 100 / state.eLength;
148 
      state.dist = calculateDist(&nodeList[state.current].pL, &currentPos) * 100 / state.eLength;
149 149 

  		




  150
  150
  
    
    }


  		




  151
  151
  
    
  }


  		




  ......




  153
  153
  
    
}


  		




  154
  154
  
    

  		




  155
  155
  
    
uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) {


  		




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


  		




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


  		




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


  		




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


  		




  158
  158
  
    
}


  		












Also available in:  Unified diff