Revision d02c536e

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
  
    
}


  		












  

    
      devices/DiWheelDrive/amiro_map.hpp
    
  





  5
  5
  
    
#include "linefollow.hpp"


  		




  6
  6
  
    
#include <amiroosconf.h>


  		




  7
  7
  
    
#include <ch.hpp>


  		




  
  8
  
    
#include <math.h>


  		




  8
  9
  
    

  		




  9
  10
  
    

  		




  10
  11
  
    
namespace amiro {


  		












  

    
      devices/DiWheelDrive/userthread.cpp
    
  





  1
  
  
    
#include "global.hpp"


  		




  2
  
  
    
#include <cmath>


  		




  3
  
  
    
#include "linefollow.hpp"


  		




  4
  1
  
    
#include "userthread.hpp"


  		




  5
  2
  
    
#include "amiro_map.hpp"


  		




  
  3
  
    
#include "global.hpp"


  		




  
  4
  
    
#include "linefollow.hpp"


  		




  6
  5
  
    

  		




  7
  6
  
    
using namespace amiro;


  		




  8
  7
  
    

  		




  ......




  842
  841
  
    

  		




  843
  842
  
    

  		




  844
  843
  
    
      global.odometry.setPosition(1.0, 0.0, 0.0);


  		




  
  844
  
    
      chprintf((BaseSequentialStream *)&global.sercanmux1, "Current Point: %d\n", global.odometry.getPosition().x);


  		




  845
  845
  
    
      global.tcase++; // 6


  		




  846
  846
  
    
      // Fixpoint on left side


  		




  847
  847
  
    
      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);


  		




  ......




  858
  858
  
    

  		




  859
  859
  
    

  		




  860
  860
  
    
      global.odometry.setPosition(1.5, 0.0, 0.0);


  		




  
  861
  
    
      chprintf((BaseSequentialStream *)&global.sercanmux1,


  		




  
  862
  
    
               "Current Point: %d\n", global.odometry.getPosition().x);


  		




  861
  863
  
    
      global.tcase++; // 7


  		




  862
  864
  
    
      // Fixpoint on left side, no update should appear because fixpoint already


  		




  863
  865
  
    
      // marked


  		




  ......




  871
  873
  
    
        && (map.get_state()->strategy == 0x01);


  		




  872
  874
  
    
        // && (map.get_state()->dist == 0);


  		




  873
  875
  
    

  		




  
  876
  
    
      global.odometry.setPosition(1.2, 0.0, 0.0);


  		




  
  877
  
    
      global.tcase++; // 8


  		




  
  878
  
    
      // Fixpoint on left side, no update should appear because fixpoint already


  		




  
  879
  
    
      // marked


  		




  
  880
  
    
      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);


  		




  
  881
  
    
      chprintf((BaseSequentialStream *)&global.sercanmux1,


  		




  
  882
  
    
               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n", global.tcase, ret,


  		




  
  883
  
    
               map.get_state()->current, map.get_state()->next, map.get_state()->dist, map.get_state()->eLength);


  		




  
  884
  
    
      // No case should be true because neither was a node visited nor


  		




  
  885
  
    
      // was a fixpoint detected.


  		




  
  886
  
    
      global.testres[global.tcase] =


  		




  
  887
  
    
          (ret == 0x04) && (map.get_state()->strategy == 0x01)


  		




  
  888
  
    
        && (map.get_state()->dist == 0);


  		




  874
  889
  
    

  		




  
  890
  
    
      global.odometry.setPosition(.5, 0.0, 0.0);


  		




  
  891
  
    
      chprintf((BaseSequentialStream *)&global.sercanmux1,


  		




  
  892
  
    
               "Current Point: %d\n", global.odometry.getPosition().x);


  		




  
  893
  
    
      global.tcase++; // 9


  		




  
  894
  
    
      // Fixpoint on left side, no update should appear because fixpoint already


  		




  
  895
  
    
      // marked


  		




  
  896
  
    
      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);


  		




  
  897
  
    
      chprintf((BaseSequentialStream *)&global.sercanmux1,


  		




  
  898
  
    
               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",


  		




  
  899
  
    
               global.tcase, ret, map.get_state()->current,


  		




  
  900
  
    
               map.get_state()->next, map.get_state()->dist,


  		




  
  901
  
    
               map.get_state()->eLength);


  		




  
  902
  
    
      // No case should be true because neither was a node visited nor


  		




  
  903
  
    
      // was a fixpoint detected.


  		




  
  904
  
    
      global.testres[global.tcase] =


  		




  
  905
  
    
        (ret == 9) &&


  		




  
  906
  
    
        (map.get_state()->strategy == 1) &&


  		




  
  907
  
    
        (map.get_state()->dist == 0) &&


  		




  
  908
  
    
        (map.get_state()->eLength == 50);


  		




  875
  909
  
    

  		




  876
  
  
    

  		




  
  910
  
    
      global.odometry.setPosition(.75, 0.0, 0.0);


  		




  
  911
  
    
      chprintf((BaseSequentialStream *)&global.sercanmux1,


  		




  
  912
  
    
               "Current Point: %d\n", global.odometry.getPosition().x);


  		




  
  913
  
    
      global.tcase++; // 10


  		




  
  914
  
    
      // Fixpoint on left side, no update should appear because fixpoint already


  		




  
  915
  
    
      // marked


  		




  
  916
  
    
      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);


  		




  
  917
  
    
      chprintf((BaseSequentialStream *)&global.sercanmux1,


  		




  
  918
  
    
               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",


  		




  
  919
  
    
               global.tcase, ret, map.get_state()->current,


  		




  
  920
  
    
               map.get_state()->next, map.get_state()->dist,


  		




  
  921
  
    
               map.get_state()->eLength);


  		




  
  922
  
    
      // No case should be true because neither was a node visited nor


  		




  
  923
  
    
      // was a fixpoint detected.


  		




  
  924
  
    
      global.testres[global.tcase] =


  		




  
  925
  
    
          (ret == 12) && (map.get_state()->strategy == 1) &&


  		




  
  926
  
    
          (map.get_state()->dist == 50) && (map.get_state()->eLength == 50);


  		




  877
  927
  
    

  		




  878
  928
  
    
      int failed = 0;


  		




  879
  929
  
    
      int passed = 0;


  		












Also available in:  Unified diff