Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

Map.cpp@35:68f9edbb3cff, 2017-06-11 (annotated)

Committer:
Ludwigfr
Date:
Sun Jun 11 22:40:37 2017 +0000
Revision:
35:68f9edbb3cff
Parent:
34:c208497dd079
Child:
39:890439b495e3
change so every command to the robot is in world coordinate

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ludwigfr 33:814bcd7d3cfe 1 #include "Map.hpp"
Ludwigfr 33:814bcd7d3cfe 2
Ludwigfr 34:c208497dd079 3
Ludwigfr 34:c208497dd079 4 Map::Map(float widthRealMap, float heightRealMap, int nbCellWidth, int nbCellHeight){
Ludwigfr 33:814bcd7d3cfe 5 this->widthRealMap=widthRealMap;
Ludwigfr 34:c208497dd079 6 this->heightRealMap=heightRealMap;
Ludwigfr 33:814bcd7d3cfe 7 this->nbCellWidth=nbCellWidth;
Ludwigfr 33:814bcd7d3cfe 8 this->nbCellHeight=nbCellHeight;
Ludwigfr 33:814bcd7d3cfe 9 this->sizeCellWidth=widthRealMap/(float)nbCellWidth;
Ludwigfr 34:c208497dd079 10 this->sizeCellHeight=heightRealMap/(float)nbCellHeight;
Ludwigfr 33:814bcd7d3cfe 11
Ludwigfr 33:814bcd7d3cfe 12 this->cellsLogValues= new float*[nbCellWidth];
Ludwigfr 33:814bcd7d3cfe 13 for(int i = 0; i < nbCellWidth; ++i)
Ludwigfr 33:814bcd7d3cfe 14 this->cellsLogValues[i] = new float[nbCellHeight];
Ludwigfr 33:814bcd7d3cfe 15
Ludwigfr 33:814bcd7d3cfe 16 this->initialLogValues= new float*[nbCellWidth];
Ludwigfr 33:814bcd7d3cfe 17 for(int i = 0; i < nbCellWidth; ++i)
Ludwigfr 33:814bcd7d3cfe 18 this->initialLogValues[i] = new float[nbCellHeight];
Ludwigfr 33:814bcd7d3cfe 19
Ludwigfr 33:814bcd7d3cfe 20 this->fill_initialLogValues();
Ludwigfr 33:814bcd7d3cfe 21 }
Ludwigfr 33:814bcd7d3cfe 22
Ludwigfr 33:814bcd7d3cfe 23 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 33:814bcd7d3cfe 24 void Map::fill_initialLogValues(){
Ludwigfr 33:814bcd7d3cfe 25 //Fill map, we know the border are occupied
Ludwigfr 33:814bcd7d3cfe 26 for (int i = 0; i<this->nbCellWidth; i++) {
Ludwigfr 34:c208497dd079 27 for (int j = 0; j<this->nbCellHeight; j++) {
Ludwigfr 34:c208497dd079 28 if(j==0 || j==this->nbCellHeight-1 || i==0 || i==this->nbCellWidth-1)
Ludwigfr 34:c208497dd079 29 this->initialLogValues[i][j] = this->proba_to_log(1);
Ludwigfr 33:814bcd7d3cfe 30 else
Ludwigfr 34:c208497dd079 31 this->initialLogValues[i][j] = this->proba_to_log(0.5);
Ludwigfr 33:814bcd7d3cfe 32 }
Ludwigfr 33:814bcd7d3cfe 33 }
Ludwigfr 33:814bcd7d3cfe 34 }
Ludwigfr 33:814bcd7d3cfe 35
Ludwigfr 33:814bcd7d3cfe 36 //returns the probability [0,1] that the cell is occupied from the log valAue lt
Ludwigfr 33:814bcd7d3cfe 37 float Map::log_to_proba(float lt){
Ludwigfr 33:814bcd7d3cfe 38 return 1-1/(1+exp(lt));
Ludwigfr 33:814bcd7d3cfe 39 }
Ludwigfr 33:814bcd7d3cfe 40
Ludwigfr 34:c208497dd079 41 void Map::update_cell_value(int widthIndice,int heightIndice ,float proba){
Ludwigfr 34:c208497dd079 42 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 34:c208497dd079 43 }
Ludwigfr 34:c208497dd079 44
Ludwigfr 34:c208497dd079 45 float Map::cell_width_coordinate_to_world(int i){
Ludwigfr 34:c208497dd079 46 return this->sizeCellWidth/2+i*this->sizeCellWidth;
Ludwigfr 34:c208497dd079 47 }
Ludwigfr 34:c208497dd079 48
Ludwigfr 34:c208497dd079 49 float Map::cell_height_coordinate_to_world(int j){
Ludwigfr 34:c208497dd079 50 return this->sizeCellHeight/2+j*this->sizeCellHeight;
Ludwigfr 34:c208497dd079 51 }
Ludwigfr 34:c208497dd079 52
Ludwigfr 34:c208497dd079 53 float Map::get_proba_cell(int widthIndice, int heightIndice){
Ludwigfr 34:c208497dd079 54 return this->log_to_proba(this->cellsLogValues[widthIndice][heightIndice]);
Ludwigfr 34:c208497dd079 55 }
Ludwigfr 34:c208497dd079 56
Ludwigfr 35:68f9edbb3cff 57 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 35:68f9edbb3cff 58 float Map::proba_to_log(float p){
Ludwigfr 35:68f9edbb3cff 59 return log(p/(1-p));
Ludwigfr 35:68f9edbb3cff 60 }
Ludwigfr 35:68f9edbb3cff 61
Ludwigfr 34:c208497dd079 62 /*
Ludwigfr 34:c208497dd079 63
Ludwigfr 35:68f9edbb3cff 64 float Map::robot_x_coordinate_in_world(float robot_x, float robot_y){
Ludwigfr 35:68f9edbb3cff 65 return this->nbCellWidth*this->sizeCellWidth-robot_y;
Ludwigfr 35:68f9edbb3cff 66 }
Ludwigfr 35:68f9edbb3cff 67
Ludwigfr 35:68f9edbb3cff 68 float Map::robot_y_coordinate_in_world(float robot_x, float robot_y){
Ludwigfr 35:68f9edbb3cff 69 return robot_x;
Ludwigfr 35:68f9edbb3cff 70 }
Ludwigfr 35:68f9edbb3cff 71
Ludwigfr 35:68f9edbb3cff 72
Ludwigfr 34:c208497dd079 73 void MiniExplorerCoimbra::print_final_map() {
Ludwigfr 33:814bcd7d3cfe 74 float currProba;
Ludwigfr 33:814bcd7d3cfe 75 pc.printf("\n\r");
Ludwigfr 33:814bcd7d3cfe 76 for (int y = this->nbCellHeight -1; y>-1; y--) {
Ludwigfr 33:814bcd7d3cfe 77 for (int x= 0; x<this->nbCellWidth; x++) {
Ludwigfr 33:814bcd7d3cfe 78 currProba=this->log_to_proba(this->cellsLogValues[x][y]);
Ludwigfr 33:814bcd7d3cfe 79 if ( currProba < 0.5) {
Ludwigfr 33:814bcd7d3cfe 80 pc.printf(" ");
Ludwigfr 33:814bcd7d3cfe 81 } else {
Ludwigfr 33:814bcd7d3cfe 82 if(currProba==0.5)
Ludwigfr 33:814bcd7d3cfe 83 pc.printf(" . ");
Ludwigfr 33:814bcd7d3cfe 84 else
Ludwigfr 33:814bcd7d3cfe 85 pc.printf(" X ");
Ludwigfr 33:814bcd7d3cfe 86 }
Ludwigfr 33:814bcd7d3cfe 87 }
Ludwigfr 33:814bcd7d3cfe 88 pc.printf("\n\r");
Ludwigfr 33:814bcd7d3cfe 89 }
Ludwigfr 33:814bcd7d3cfe 90 }
Ludwigfr 33:814bcd7d3cfe 91
Ludwigfr 33:814bcd7d3cfe 92
Ludwigfr 33:814bcd7d3cfe 93 void Map::print_final_map_with_robot_position(float robot_x,float robot_y) {
Ludwigfr 33:814bcd7d3cfe 94 float currProba;
Ludwigfr 33:814bcd7d3cfe 95 float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y);
Ludwigfr 33:814bcd7d3cfe 96 float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y);
Ludwigfr 33:814bcd7d3cfe 97
Ludwigfr 33:814bcd7d3cfe 98 float heightIndiceInOrthonormal;
Ludwigfr 33:814bcd7d3cfe 99 float widthIndiceInOrthonormal;
Ludwigfr 33:814bcd7d3cfe 100
Ludwigfr 33:814bcd7d3cfe 101 float widthMalus=-(3*sizeCellWidth/2);
Ludwigfr 33:814bcd7d3cfe 102 float widthBonus=sizeCellWidth/2;
Ludwigfr 33:814bcd7d3cfe 103
Ludwigfr 33:814bcd7d3cfe 104 float heightMalus=-(3*sizeCellHeight/2);
Ludwigfr 33:814bcd7d3cfe 105 float heightBonus=sizeCellHeight/2;
Ludwigfr 33:814bcd7d3cfe 106
Ludwigfr 33:814bcd7d3cfe 107 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 108 for (int y = this->nbCellHeight -1; y>-1; y--) {
Ludwigfr 34:c208497dd079 109 for (int x= 0; x<this->nbCellWidth; x++) {
Ludwigfr 33:814bcd7d3cfe 110 heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y);
Ludwigfr 33:814bcd7d3cfe 111 widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x);
Ludwigfr 33:814bcd7d3cfe 112 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 33:814bcd7d3cfe 113 pc.printf(" R ");
Ludwigfr 33:814bcd7d3cfe 114 else{
Ludwigfr 33:814bcd7d3cfe 115 currProba=this->log_to_proba(this->cellsLogValues[x][y]);
Ludwigfr 33:814bcd7d3cfe 116 if ( currProba < 0.5)
Ludwigfr 33:814bcd7d3cfe 117 pc.printf(" ");
Ludwigfr 33:814bcd7d3cfe 118 else{
Ludwigfr 33:814bcd7d3cfe 119 if(currProba==0.5)
Ludwigfr 33:814bcd7d3cfe 120 pc.printf(" . ");
Ludwigfr 33:814bcd7d3cfe 121 else
Ludwigfr 33:814bcd7d3cfe 122 pc.printf(" X ");
Ludwigfr 33:814bcd7d3cfe 123 }
Ludwigfr 33:814bcd7d3cfe 124 }
Ludwigfr 33:814bcd7d3cfe 125 }
Ludwigfr 33:814bcd7d3cfe 126 pc.printf("\n\r");
Ludwigfr 33:814bcd7d3cfe 127 }
Ludwigfr 33:814bcd7d3cfe 128 }
Ludwigfr 33:814bcd7d3cfe 129
Ludwigfr 33:814bcd7d3cfe 130 void Map::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) {
Ludwigfr 33:814bcd7d3cfe 131 float currProba;
Ludwigfr 33:814bcd7d3cfe 132 float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y);
Ludwigfr 33:814bcd7d3cfe 133 float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y);
Ludwigfr 33:814bcd7d3cfe 134
Ludwigfr 33:814bcd7d3cfe 135 float heightIndiceInOrthonormal;
Ludwigfr 33:814bcd7d3cfe 136 float widthIndiceInOrthonormal;
Ludwigfr 33:814bcd7d3cfe 137
Ludwigfr 33:814bcd7d3cfe 138 float widthMalus=-(3*sizeCellWidth/2);
Ludwigfr 33:814bcd7d3cfe 139 float widthBonus=sizeCellWidth/2;
Ludwigfr 33:814bcd7d3cfe 140
Ludwigfr 33:814bcd7d3cfe 141 float heightMalus=-(3*sizeCellHeight/2);
Ludwigfr 33:814bcd7d3cfe 142 float heightBonus=sizeCellHeight/2;
Ludwigfr 33:814bcd7d3cfe 143
Ludwigfr 33:814bcd7d3cfe 144 pc.printf("\n\r");
Ludwigfr 33:814bcd7d3cfe 145 for (int y = this->nbCellHeight -1; y>-1; y--) {
Ludwigfr 33:814bcd7d3cfe 146 for (int x= 0; x<this->nbCellWidth; x++) {
Ludwigfr 33:814bcd7d3cfe 147 heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y);
Ludwigfr 33:814bcd7d3cfe 148 widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x);
Ludwigfr 33:814bcd7d3cfe 149 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 33:814bcd7d3cfe 150 pc.printf(" R ");
Ludwigfr 33:814bcd7d3cfe 151 else{
Ludwigfr 33:814bcd7d3cfe 152 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 33:814bcd7d3cfe 153 pc.printf(" T ");
Ludwigfr 33:814bcd7d3cfe 154 else{
Ludwigfr 33:814bcd7d3cfe 155 currProba=this->log_to_proba(this->cellsLogValues[x][y]);
Ludwigfr 33:814bcd7d3cfe 156 if ( currProba < 0.5)
Ludwigfr 33:814bcd7d3cfe 157 pc.printf(" ");
Ludwigfr 33:814bcd7d3cfe 158 else{
Ludwigfr 33:814bcd7d3cfe 159 if(currProba==0.5)
Ludwigfr 33:814bcd7d3cfe 160 pc.printf(" . ");
Ludwigfr 33:814bcd7d3cfe 161 else
Ludwigfr 33:814bcd7d3cfe 162 pc.printf(" X ");
Ludwigfr 33:814bcd7d3cfe 163 }
Ludwigfr 33:814bcd7d3cfe 164 }
Ludwigfr 33:814bcd7d3cfe 165 }
Ludwigfr 33:814bcd7d3cfe 166 }
Ludwigfr 33:814bcd7d3cfe 167 pc.printf("\n\r");
Ludwigfr 33:814bcd7d3cfe 168 }
Ludwigfr 33:814bcd7d3cfe 169 }
Ludwigfr 34:c208497dd079 170 */