Yo Fleyt / Mbed 2 deprecated roboticLab_withclass

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?

UserRevisionLine numberNew 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 */