Revision a3c54343 devices/DiWheelDrive/amiro_map.cpp

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
  
    
}


  		












Also available in:  Unified diff