lab robotic coimbra
Dependencies: ISR_Mini-explorer mbed
Map.cpp@0:9f7ee7ed13e4, 2017-06-26 (annotated)
- Committer:
- Ludwigfr
- Date:
- Mon Jun 26 12:05:20 2017 +0000
- Revision:
- 0:9f7ee7ed13e4
this version should work, though it would be nice to test all the lab demo; there's also some code on the 4th lab at the end of MiniExplorerCoimbra.cpp
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Ludwigfr | 0:9f7ee7ed13e4 | 1 | #include "Map.hpp" |
Ludwigfr | 0:9f7ee7ed13e4 | 2 | |
Ludwigfr | 0:9f7ee7ed13e4 | 3 | Map::Map(float widthRealMap, float heightRealMap, int nbCellWidth, int nbCellHeight){ |
Ludwigfr | 0:9f7ee7ed13e4 | 4 | this->widthRealMap=widthRealMap; |
Ludwigfr | 0:9f7ee7ed13e4 | 5 | this->heightRealMap=heightRealMap; |
Ludwigfr | 0:9f7ee7ed13e4 | 6 | this->nbCellWidth=nbCellWidth; |
Ludwigfr | 0:9f7ee7ed13e4 | 7 | this->nbCellHeight=nbCellHeight; |
Ludwigfr | 0:9f7ee7ed13e4 | 8 | this->sizeCellWidth=widthRealMap/(float)nbCellWidth; |
Ludwigfr | 0:9f7ee7ed13e4 | 9 | this->sizeCellHeight=heightRealMap/(float)nbCellHeight; |
Ludwigfr | 0:9f7ee7ed13e4 | 10 | |
Ludwigfr | 0:9f7ee7ed13e4 | 11 | this->cellsLogValues= new float*[nbCellWidth]; |
Ludwigfr | 0:9f7ee7ed13e4 | 12 | for(int i = 0; i < nbCellWidth; ++i) |
Ludwigfr | 0:9f7ee7ed13e4 | 13 | this->cellsLogValues[i] = new float[nbCellHeight]; |
Ludwigfr | 0:9f7ee7ed13e4 | 14 | |
Ludwigfr | 0:9f7ee7ed13e4 | 15 | this->initialLogValues= new float*[nbCellWidth]; |
Ludwigfr | 0:9f7ee7ed13e4 | 16 | for(int i = 0; i < nbCellWidth; ++i) |
Ludwigfr | 0:9f7ee7ed13e4 | 17 | this->initialLogValues[i] = new float[nbCellHeight]; |
Ludwigfr | 0:9f7ee7ed13e4 | 18 | |
Ludwigfr | 0:9f7ee7ed13e4 | 19 | this->fill_initialLogValues(); |
Ludwigfr | 0:9f7ee7ed13e4 | 20 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 21 | |
Ludwigfr | 0:9f7ee7ed13e4 | 22 | //fill initialLogValues with the values we already know (here the bordurs) |
Ludwigfr | 0:9f7ee7ed13e4 | 23 | void Map::fill_initialLogValues(){ |
Ludwigfr | 0:9f7ee7ed13e4 | 24 | //Fill map, we know the border are occupied |
Ludwigfr | 0:9f7ee7ed13e4 | 25 | for (int i = 0; i<this->nbCellWidth; i++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 26 | for (int j = 0; j<this->nbCellHeight; j++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 27 | if(j==0 || j==this->nbCellHeight-1 || i==0 || i==this->nbCellWidth-1) |
Ludwigfr | 0:9f7ee7ed13e4 | 28 | this->initialLogValues[i][j] = this->proba_to_log(1); |
Ludwigfr | 0:9f7ee7ed13e4 | 29 | else |
Ludwigfr | 0:9f7ee7ed13e4 | 30 | this->initialLogValues[i][j] = this->proba_to_log(0.5); |
Ludwigfr | 0:9f7ee7ed13e4 | 31 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 32 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 33 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 34 | |
Ludwigfr | 0:9f7ee7ed13e4 | 35 | void Map::fill_map_with_initial(){ |
Ludwigfr | 0:9f7ee7ed13e4 | 36 | for (int i = 0; i<this->nbCellWidth; i++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 37 | for (int j = 0; j<this->nbCellHeight; j++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 38 | this->cellsLogValues[i][j] = initialLogValues[i][j]; |
Ludwigfr | 0:9f7ee7ed13e4 | 39 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 40 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 41 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 42 | |
Ludwigfr | 0:9f7ee7ed13e4 | 43 | //returns the probability [0,1] that the cell is occupied from the log valAue lt |
Ludwigfr | 0:9f7ee7ed13e4 | 44 | float Map::log_to_proba(float lt){ |
Ludwigfr | 0:9f7ee7ed13e4 | 45 | return 1-1/(1+exp(lt)); |
Ludwigfr | 0:9f7ee7ed13e4 | 46 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 47 | |
Ludwigfr | 0:9f7ee7ed13e4 | 48 | void Map::update_cell_value(int widthIndice,int heightIndice ,float proba){ |
Ludwigfr | 0:9f7ee7ed13e4 | 49 | this->cellsLogValues[widthIndice][heightIndice]=this->cellsLogValues[widthIndice][heightIndice]+this->proba_to_log(proba)+this->initialLogValues[widthIndice][heightIndice];//map is filled as map[0][0] get the data for the point closest to the origin |
Ludwigfr | 0:9f7ee7ed13e4 | 50 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 51 | |
Ludwigfr | 0:9f7ee7ed13e4 | 52 | float Map::cell_width_coordinate_to_world(int i){ |
Ludwigfr | 0:9f7ee7ed13e4 | 53 | return this->sizeCellWidth/2+i*this->sizeCellWidth; |
Ludwigfr | 0:9f7ee7ed13e4 | 54 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 55 | |
Ludwigfr | 0:9f7ee7ed13e4 | 56 | float Map::cell_height_coordinate_to_world(int j){ |
Ludwigfr | 0:9f7ee7ed13e4 | 57 | return this->sizeCellHeight/2+j*this->sizeCellHeight; |
Ludwigfr | 0:9f7ee7ed13e4 | 58 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 59 | |
Ludwigfr | 0:9f7ee7ed13e4 | 60 | float Map::get_proba_cell(int widthIndice, int heightIndice){ |
Ludwigfr | 0:9f7ee7ed13e4 | 61 | return this->log_to_proba(this->cellsLogValues[widthIndice][heightIndice]); |
Ludwigfr | 0:9f7ee7ed13e4 | 62 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 63 | |
Ludwigfr | 0:9f7ee7ed13e4 | 64 | //returns the log value that the cell is occupied from the probability value [0,1] |
Ludwigfr | 0:9f7ee7ed13e4 | 65 | float Map::proba_to_log(float p){ |
Ludwigfr | 0:9f7ee7ed13e4 | 66 | return log(p/(1-p)); |
Ludwigfr | 0:9f7ee7ed13e4 | 67 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 68 | |
Ludwigfr | 0:9f7ee7ed13e4 | 69 | /* |
Ludwigfr | 0:9f7ee7ed13e4 | 70 | |
Ludwigfr | 0:9f7ee7ed13e4 | 71 | float Map::robot_x_coordinate_in_world(float robot_x, float robot_y){ |
Ludwigfr | 0:9f7ee7ed13e4 | 72 | return this->nbCellWidth*this->sizeCellWidth-robot_y; |
Ludwigfr | 0:9f7ee7ed13e4 | 73 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 74 | |
Ludwigfr | 0:9f7ee7ed13e4 | 75 | float Map::robot_y_coordinate_in_world(float robot_x, float robot_y){ |
Ludwigfr | 0:9f7ee7ed13e4 | 76 | return robot_x; |
Ludwigfr | 0:9f7ee7ed13e4 | 77 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 78 | |
Ludwigfr | 0:9f7ee7ed13e4 | 79 | |
Ludwigfr | 0:9f7ee7ed13e4 | 80 | void MiniExplorerCoimbra::print_final_map() { |
Ludwigfr | 0:9f7ee7ed13e4 | 81 | float currProba; |
Ludwigfr | 0:9f7ee7ed13e4 | 82 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 83 | for (int y = this->nbCellHeight -1; y>-1; y--) { |
Ludwigfr | 0:9f7ee7ed13e4 | 84 | for (int x= 0; x<this->nbCellWidth; x++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 85 | currProba=this->log_to_proba(this->cellsLogValues[x][y]); |
Ludwigfr | 0:9f7ee7ed13e4 | 86 | if ( currProba < 0.5) { |
Ludwigfr | 0:9f7ee7ed13e4 | 87 | pc.printf(" "); |
Ludwigfr | 0:9f7ee7ed13e4 | 88 | } else { |
Ludwigfr | 0:9f7ee7ed13e4 | 89 | if(currProba==0.5) |
Ludwigfr | 0:9f7ee7ed13e4 | 90 | pc.printf(" . "); |
Ludwigfr | 0:9f7ee7ed13e4 | 91 | else |
Ludwigfr | 0:9f7ee7ed13e4 | 92 | pc.printf(" X "); |
Ludwigfr | 0:9f7ee7ed13e4 | 93 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 94 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 95 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 96 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 97 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 98 | |
Ludwigfr | 0:9f7ee7ed13e4 | 99 | |
Ludwigfr | 0:9f7ee7ed13e4 | 100 | void Map::print_final_map_with_robot_position(float robot_x,float robot_y) { |
Ludwigfr | 0:9f7ee7ed13e4 | 101 | float currProba; |
Ludwigfr | 0:9f7ee7ed13e4 | 102 | float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 0:9f7ee7ed13e4 | 103 | float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 0:9f7ee7ed13e4 | 104 | |
Ludwigfr | 0:9f7ee7ed13e4 | 105 | float heightIndiceInOrthonormal; |
Ludwigfr | 0:9f7ee7ed13e4 | 106 | float widthIndiceInOrthonormal; |
Ludwigfr | 0:9f7ee7ed13e4 | 107 | |
Ludwigfr | 0:9f7ee7ed13e4 | 108 | float widthMalus=-(3*sizeCellWidth/2); |
Ludwigfr | 0:9f7ee7ed13e4 | 109 | float widthBonus=sizeCellWidth/2; |
Ludwigfr | 0:9f7ee7ed13e4 | 110 | |
Ludwigfr | 0:9f7ee7ed13e4 | 111 | float heightMalus=-(3*sizeCellHeight/2); |
Ludwigfr | 0:9f7ee7ed13e4 | 112 | float heightBonus=sizeCellHeight/2; |
Ludwigfr | 0:9f7ee7ed13e4 | 113 | |
Ludwigfr | 0:9f7ee7ed13e4 | 114 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 115 | for (int y = this->nbCellHeight -1; y>-1; y--) { |
Ludwigfr | 0:9f7ee7ed13e4 | 116 | for (int x= 0; x<this->nbCellWidth; x++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 117 | heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y); |
Ludwigfr | 0:9f7ee7ed13e4 | 118 | widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x); |
Ludwigfr | 0:9f7ee7ed13e4 | 119 | if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) |
Ludwigfr | 0:9f7ee7ed13e4 | 120 | pc.printf(" R "); |
Ludwigfr | 0:9f7ee7ed13e4 | 121 | else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 122 | currProba=this->log_to_proba(this->cellsLogValues[x][y]); |
Ludwigfr | 0:9f7ee7ed13e4 | 123 | if ( currProba < 0.5) |
Ludwigfr | 0:9f7ee7ed13e4 | 124 | pc.printf(" "); |
Ludwigfr | 0:9f7ee7ed13e4 | 125 | else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 126 | if(currProba==0.5) |
Ludwigfr | 0:9f7ee7ed13e4 | 127 | pc.printf(" . "); |
Ludwigfr | 0:9f7ee7ed13e4 | 128 | else |
Ludwigfr | 0:9f7ee7ed13e4 | 129 | pc.printf(" X "); |
Ludwigfr | 0:9f7ee7ed13e4 | 130 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 131 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 132 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 133 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 134 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 135 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 136 | |
Ludwigfr | 0:9f7ee7ed13e4 | 137 | void Map::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) { |
Ludwigfr | 0:9f7ee7ed13e4 | 138 | float currProba; |
Ludwigfr | 0:9f7ee7ed13e4 | 139 | float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 0:9f7ee7ed13e4 | 140 | float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 0:9f7ee7ed13e4 | 141 | |
Ludwigfr | 0:9f7ee7ed13e4 | 142 | float heightIndiceInOrthonormal; |
Ludwigfr | 0:9f7ee7ed13e4 | 143 | float widthIndiceInOrthonormal; |
Ludwigfr | 0:9f7ee7ed13e4 | 144 | |
Ludwigfr | 0:9f7ee7ed13e4 | 145 | float widthMalus=-(3*sizeCellWidth/2); |
Ludwigfr | 0:9f7ee7ed13e4 | 146 | float widthBonus=sizeCellWidth/2; |
Ludwigfr | 0:9f7ee7ed13e4 | 147 | |
Ludwigfr | 0:9f7ee7ed13e4 | 148 | float heightMalus=-(3*sizeCellHeight/2); |
Ludwigfr | 0:9f7ee7ed13e4 | 149 | float heightBonus=sizeCellHeight/2; |
Ludwigfr | 0:9f7ee7ed13e4 | 150 | |
Ludwigfr | 0:9f7ee7ed13e4 | 151 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 152 | for (int y = this->nbCellHeight -1; y>-1; y--) { |
Ludwigfr | 0:9f7ee7ed13e4 | 153 | for (int x= 0; x<this->nbCellWidth; x++) { |
Ludwigfr | 0:9f7ee7ed13e4 | 154 | heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y); |
Ludwigfr | 0:9f7ee7ed13e4 | 155 | widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x); |
Ludwigfr | 0:9f7ee7ed13e4 | 156 | if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) |
Ludwigfr | 0:9f7ee7ed13e4 | 157 | pc.printf(" R "); |
Ludwigfr | 0:9f7ee7ed13e4 | 158 | else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 159 | if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus)) |
Ludwigfr | 0:9f7ee7ed13e4 | 160 | pc.printf(" T "); |
Ludwigfr | 0:9f7ee7ed13e4 | 161 | else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 162 | currProba=this->log_to_proba(this->cellsLogValues[x][y]); |
Ludwigfr | 0:9f7ee7ed13e4 | 163 | if ( currProba < 0.5) |
Ludwigfr | 0:9f7ee7ed13e4 | 164 | pc.printf(" "); |
Ludwigfr | 0:9f7ee7ed13e4 | 165 | else{ |
Ludwigfr | 0:9f7ee7ed13e4 | 166 | if(currProba==0.5) |
Ludwigfr | 0:9f7ee7ed13e4 | 167 | pc.printf(" . "); |
Ludwigfr | 0:9f7ee7ed13e4 | 168 | else |
Ludwigfr | 0:9f7ee7ed13e4 | 169 | pc.printf(" X "); |
Ludwigfr | 0:9f7ee7ed13e4 | 170 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 171 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 172 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 173 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 174 | pc.printf("\n\r"); |
Ludwigfr | 0:9f7ee7ed13e4 | 175 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 176 | } |
Ludwigfr | 0:9f7ee7ed13e4 | 177 | */ |