Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

main.cpp@25:572c9e9a8809, 2017-04-20 (annotated)

Committer:
Ludwigfr
Date:
Thu Apr 20 15:02:28 2017 +0000
Revision:
25:572c9e9a8809
Parent:
24:8f4b820d8de8
Child:
26:b020cf253059
fixed a few things, and I did print_final_map_with_robot if you want to check where odometria thinks the robot is in the map

Who changed what in which revision?

UserRevisionLine numberNew contents of line
geotsam 0:8bffb51cc345 1 #include "mbed.h"
geotsam 0:8bffb51cc345 2 #include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
geotsam 0:8bffb51cc345 3 #include "math.h"
Ludwigfr 22:ebb37a249b5f 4
Ludwigfr 22:ebb37a249b5f 5 using namespace std;
AurelienBernier 4:8c56c3ba6e54 6
Ludwigfr 22:ebb37a249b5f 7 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 22:ebb37a249b5f 8 void fill_initial_log_values();
Ludwigfr 22:ebb37a249b5f 9 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 22:ebb37a249b5f 10 void randomize_and_map();
Ludwigfr 22:ebb37a249b5f 11 //go the the given position while updating the map
Ludwigfr 22:ebb37a249b5f 12 void go_to_point_with_angle(float target_x, float target_y, float target_angle);
Ludwigfr 22:ebb37a249b5f 13 //Updates sonar values
geotsam 24:8f4b820d8de8 14 void update_sonar_values(float leftMm, float frontMm, float rightMm);
Ludwigfr 22:ebb37a249b5f 15 //function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left pi/3, right -pi/3) returns the probability it's occupied/empty [0;1]
Ludwigfr 22:ebb37a249b5f 16 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
Ludwigfr 22:ebb37a249b5f 17 //print the map
Ludwigfr 22:ebb37a249b5f 18 void print_final_map();
Ludwigfr 25:572c9e9a8809 19 //print the map with the robot marked on it
Ludwigfr 25:572c9e9a8809 20 void print_final_map_with_robot_position();
geotsam 0:8bffb51cc345 21
Ludwigfr 22:ebb37a249b5f 22 //MATHS heavy functions
Ludwigfr 22:ebb37a249b5f 23 float dist(float robot_x, float robot_y, float target_x, float target_y);
Ludwigfr 22:ebb37a249b5f 24 //returns the probability [0,1] that the cell is occupied from the log value lt
Ludwigfr 22:ebb37a249b5f 25 float log_to_proba(float lt);
Ludwigfr 22:ebb37a249b5f 26 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 22:ebb37a249b5f 27 float proba_to_log(float p);
Ludwigfr 22:ebb37a249b5f 28 //returns the new log value
Ludwigfr 22:ebb37a249b5f 29 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue );
Ludwigfr 22:ebb37a249b5f 30 //makes the angle inAngle between 0 and 2pi
Ludwigfr 22:ebb37a249b5f 31 float rad_angle_check(float inAngle);
Ludwigfr 22:ebb37a249b5f 32 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 22:ebb37a249b5f 33 float compute_angle_between_vectors(float x, float y,float xs,float ys);
Ludwigfr 25:572c9e9a8809 34 float robot_center_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 35 float robot_center_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 36 float robot_sonar_front_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 37 float robot_sonar_front_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 38 float robot_sonar_right_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 39 float robot_sonar_right_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 40 float robot_sonar_left_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 41 float robot_sonar_left_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 42 float estimated_width_indice_in_orthonormal_x(int i);
Ludwigfr 25:572c9e9a8809 43 float estimated_height_indice_in_orthonormal_y(int j);
AurelienBernier 8:109314be5b68 44
Ludwigfr 22:ebb37a249b5f 45 const float pi=3.14159;
Ludwigfr 22:ebb37a249b5f 46 //spec of the sonar
Ludwigfr 22:ebb37a249b5f 47 //TODO MEASURE THE DISTANCE on X and Y of the robot frame, between each sonar and the center of the robot and add it to calculus in updateSonarValues
geotsam 24:8f4b820d8de8 48 const float RANGE_SONAR=50;//cm
geotsam 24:8f4b820d8de8 49 const float RANGE_SONAR_MIN=10;//Rmin cm
geotsam 24:8f4b820d8de8 50 const float INCERTITUDE_SONAR=10;//cm
geotsam 24:8f4b820d8de8 51 const float ANGLE_SONAR=pi/3;//Omega rad
geotsam 24:8f4b820d8de8 52 const float SECURITY_DISTANCE=150;//mm
Ludwigfr 22:ebb37a249b5f 53
Ludwigfr 22:ebb37a249b5f 54 //those distance and angle are approximation in need of measurements
geotsam 24:8f4b820d8de8 55 const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees
geotsam 24:8f4b820d8de8 56 const float DISTANCE_SONAR_LEFT_X=4;
geotsam 24:8f4b820d8de8 57 const float DISTANCE_SONAR_LEFT_Y=4;
Ludwigfr 22:ebb37a249b5f 58
geotsam 24:8f4b820d8de8 59 const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees
geotsam 24:8f4b820d8de8 60 const float DISTANCE_SONAR_RIGHT_X=-4;
geotsam 24:8f4b820d8de8 61 const float DISTANCE_SONAR_RIGHT_Y=4;
AurelienBernier 11:e641aa08c92e 62
Ludwigfr 22:ebb37a249b5f 63 const float ANGLE_FRONT_TO_FRONT=0;
Ludwigfr 22:ebb37a249b5f 64 const float DISTANCE_SONAR_FRONT_X=0;
Ludwigfr 22:ebb37a249b5f 65 const float DISTANCE_SONAR_FRONT_Y=5;
Ludwigfr 22:ebb37a249b5f 66
Ludwigfr 22:ebb37a249b5f 67 //TODO adjust the size of the map for computation time (25*25?)
geotsam 24:8f4b820d8de8 68 const float WIDTH_ARENA=120;//cm
geotsam 24:8f4b820d8de8 69 const float HEIGHT_ARENA=90;//cm
geotsam 24:8f4b820d8de8 70
geotsam 24:8f4b820d8de8 71 //const int SIZE_MAP=25;
geotsam 24:8f4b820d8de8 72 const int NB_CELL_WIDTH=24;
geotsam 24:8f4b820d8de8 73 const int NB_CELL_HEIGHT=18;
Ludwigfr 22:ebb37a249b5f 74
Ludwigfr 22:ebb37a249b5f 75 //position and orientation of the robot when put on the map (ODOMETRY doesn't know those)
geotsam 24:8f4b820d8de8 76 const float DEFAULT_X=WIDTH_ARENA/2;
geotsam 24:8f4b820d8de8 77 const float DEFAULT_Y=HEIGHT_ARENA/2;
Ludwigfr 22:ebb37a249b5f 78 const float DEFAULT_THETA=pi/2;
Ludwigfr 22:ebb37a249b5f 79
geotsam 24:8f4b820d8de8 80 //used to create the map 250 represent the 250cm of the square where the robot is tested
geotsam 24:8f4b820d8de8 81 //float sizeCell=250/(float)SIZE_MAP;
geotsam 24:8f4b820d8de8 82 float sizeCellX=WIDTH_ARENA/(float)NB_CELL_WIDTH;
geotsam 24:8f4b820d8de8 83 float sizeCellY=HEIGHT_ARENA/(float)NB_CELL_HEIGHT;
geotsam 24:8f4b820d8de8 84
geotsam 24:8f4b820d8de8 85 float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell
geotsam 24:8f4b820d8de8 86 float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT];
Ludwigfr 22:ebb37a249b5f 87
Ludwigfr 22:ebb37a249b5f 88 //Diameter of a wheel and distance between the 2
Ludwigfr 22:ebb37a249b5f 89 const float RADIUS_WHEELS=3.25;
Ludwigfr 22:ebb37a249b5f 90 const float DISTANCE_WHEELS=7.2;
Ludwigfr 22:ebb37a249b5f 91
geotsam 24:8f4b820d8de8 92 const int MAX_SPEED=500;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
AurelienBernier 8:109314be5b68 93
geotsam 24:8f4b820d8de8 94 bool tooClose=false;
geotsam 24:8f4b820d8de8 95 bool turnFromObstacle=false;
AurelienBernier 8:109314be5b68 96
geotsam 0:8bffb51cc345 97 int main(){
geotsam 17:caf393b63e27 98
geotsam 13:41f75c132135 99 i2c1.frequency(100000);
AurelienBernier 2:ea61e801e81f 100 initRobot(); //Initializing the robot
geotsam 0:8bffb51cc345 101 pc.baud(9600); // baud for the pc communication
geotsam 0:8bffb51cc345 102
Ludwigfr 22:ebb37a249b5f 103 measure_always_on();//TODO check if needed
geotsam 24:8f4b820d8de8 104 wait(0.5);
AurelienBernier 19:dbc5fbad4975 105
Ludwigfr 22:ebb37a249b5f 106 fill_initial_log_values();
geotsam 13:41f75c132135 107
geotsam 24:8f4b820d8de8 108 theta=DEFAULT_THETA;
geotsam 24:8f4b820d8de8 109 X=DEFAULT_X;
geotsam 24:8f4b820d8de8 110 Y=DEFAULT_Y;
geotsam 24:8f4b820d8de8 111
geotsam 24:8f4b820d8de8 112 for (int i = 0; i<20; i++) {
Ludwigfr 22:ebb37a249b5f 113 randomize_and_map();
geotsam 24:8f4b820d8de8 114 wait(2);
geotsam 24:8f4b820d8de8 115 print_final_map();
geotsam 17:caf393b63e27 116 }
AurelienBernier 8:109314be5b68 117
AurelienBernier 8:109314be5b68 118 }
AurelienBernier 8:109314be5b68 119
Ludwigfr 22:ebb37a249b5f 120 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 22:ebb37a249b5f 121 void fill_initial_log_values(){
Ludwigfr 22:ebb37a249b5f 122 //Fill map, we know the border are occupied
geotsam 24:8f4b820d8de8 123 for (int i = 0; i<NB_CELL_WIDTH; i++) {
geotsam 24:8f4b820d8de8 124 for (int j = 0; j<NB_CELL_HEIGHT; j++) {
geotsam 24:8f4b820d8de8 125 if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1)
Ludwigfr 22:ebb37a249b5f 126 initialLogValues[i][j] = proba_to_log(1);
Ludwigfr 22:ebb37a249b5f 127 else
Ludwigfr 22:ebb37a249b5f 128 initialLogValues[i][j] = proba_to_log(0.5);
AurelienBernier 21:62154d644531 129 }
Ludwigfr 22:ebb37a249b5f 130 }
AurelienBernier 8:109314be5b68 131 }
AurelienBernier 8:109314be5b68 132
Ludwigfr 22:ebb37a249b5f 133 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 22:ebb37a249b5f 134 void randomize_and_map() {
geotsam 24:8f4b820d8de8 135 //TODO check that it's aurelien's work
geotsam 24:8f4b820d8de8 136 float target_x = (rand()%(int)(WIDTH_ARENA*10))/10;//for decimal precision
geotsam 24:8f4b820d8de8 137 float target_y = (rand()%(int)(HEIGHT_ARENA*10))/10;
Ludwigfr 22:ebb37a249b5f 138 float target_angle = ((float)(rand()%31416)-15708)/10000.0;
geotsam 24:8f4b820d8de8 139
geotsam 24:8f4b820d8de8 140 //TODO comment that
geotsam 24:8f4b820d8de8 141 //pc.printf("\n\r targ_X=%f", target_x);
geotsam 24:8f4b820d8de8 142 //pc.printf("\n\r targ_Y=%f", target_y);
geotsam 24:8f4b820d8de8 143 //pc.printf("\n\r targ_Angle=%f", target_angle);
geotsam 24:8f4b820d8de8 144
Ludwigfr 22:ebb37a249b5f 145 go_to_point_with_angle(target_x, target_y, target_angle);
AurelienBernier 19:dbc5fbad4975 146 }
AurelienBernier 19:dbc5fbad4975 147
Ludwigfr 22:ebb37a249b5f 148 //go the the given position while updating the map
Ludwigfr 22:ebb37a249b5f 149 //TODO clean this procedure it's ugly as hell and too long
Ludwigfr 22:ebb37a249b5f 150 void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
geotsam 24:8f4b820d8de8 151 if(tooClose){
geotsam 24:8f4b820d8de8 152 target_x=X;
geotsam 24:8f4b820d8de8 153 target_y=Y;
geotsam 24:8f4b820d8de8 154 target_angle=theta+pi/2;
geotsam 24:8f4b820d8de8 155 target_angle = atan(sin(target_angle)/cos(target_angle));
geotsam 24:8f4b820d8de8 156 pc.printf("\n\rShould just turn now, new target_angle=%f\n\r", target_angle);
geotsam 24:8f4b820d8de8 157 }
geotsam 24:8f4b820d8de8 158
geotsam 24:8f4b820d8de8 159 //TODO ugly old stuff
geotsam 24:8f4b820d8de8 160 float alpha; //angle error
geotsam 24:8f4b820d8de8 161 float rho; //distance from target
geotsam 24:8f4b820d8de8 162 float beta;
geotsam 24:8f4b820d8de8 163 float kRho=12, ka=30, kb=-13; //Kappa values
geotsam 24:8f4b820d8de8 164 float linear, angular, angular_left, angular_right;
geotsam 24:8f4b820d8de8 165 float dt=0.5;
geotsam 24:8f4b820d8de8 166 float temp;
geotsam 24:8f4b820d8de8 167 float d2;
geotsam 24:8f4b820d8de8 168 Timer t;
geotsam 24:8f4b820d8de8 169
geotsam 24:8f4b820d8de8 170 int speed=MAX_SPEED; // Max speed at beggining of movement
geotsam 24:8f4b820d8de8 171
geotsam 24:8f4b820d8de8 172 float leftMm;
geotsam 24:8f4b820d8de8 173 float frontMm;
geotsam 24:8f4b820d8de8 174 float rightMm;
geotsam 24:8f4b820d8de8 175
geotsam 24:8f4b820d8de8 176 alpha = atan2((target_y-Y),(target_x-X))-theta;
geotsam 24:8f4b820d8de8 177 alpha = atan(sin(alpha)/cos(alpha));
geotsam 24:8f4b820d8de8 178 rho = dist(X, Y, target_x, target_y);
geotsam 24:8f4b820d8de8 179
geotsam 24:8f4b820d8de8 180 beta = -alpha-theta+target_angle;
geotsam 24:8f4b820d8de8 181 //beta = atan(sin(beta)/cos(beta));
geotsam 24:8f4b820d8de8 182
geotsam 24:8f4b820d8de8 183 do {
geotsam 24:8f4b820d8de8 184 //pc.printf("\n\n\r entered while");
AurelienBernier 2:ea61e801e81f 185
AurelienBernier 6:afde4b08166b 186 //Timer stuff
AurelienBernier 6:afde4b08166b 187 dt = t.read();
AurelienBernier 6:afde4b08166b 188 t.reset();
AurelienBernier 6:afde4b08166b 189 t.start();
AurelienBernier 6:afde4b08166b 190
geotsam 14:d58f2bdbf42e 191 //Updating X,Y and theta with the odometry values
geotsam 14:d58f2bdbf42e 192 Odometria();
geotsam 24:8f4b820d8de8 193
geotsam 24:8f4b820d8de8 194 leftMm = get_distance_left_sensor();
geotsam 24:8f4b820d8de8 195 frontMm = get_distance_front_sensor();
geotsam 24:8f4b820d8de8 196 rightMm = get_distance_right_sensor();
geotsam 24:8f4b820d8de8 197
geotsam 24:8f4b820d8de8 198 //pc.printf("\n\r leftMm=%f", leftMm);
geotsam 24:8f4b820d8de8 199 //pc.printf("\n\r frontMm=%f", frontMm);
geotsam 24:8f4b820d8de8 200 //pc.printf("\n\r rightMm=%f", rightMm);
Ludwigfr 22:ebb37a249b5f 201
geotsam 24:8f4b820d8de8 202 update_sonar_values(leftMm, frontMm, rightMm);
geotsam 24:8f4b820d8de8 203
geotsam 24:8f4b820d8de8 204 if ((leftMm < SECURITY_DISTANCE || frontMm < SECURITY_DISTANCE || rightMm < SECURITY_DISTANCE) && turnFromObstacle==false) {
AurelienBernier 21:62154d644531 205 tooClose = true;
geotsam 24:8f4b820d8de8 206 turnFromObstacle = true;
geotsam 24:8f4b820d8de8 207 pc.printf("\n\r TOO CLOSE \n\r");
geotsam 24:8f4b820d8de8 208 leftMotor(1,0);
geotsam 24:8f4b820d8de8 209 rightMotor(1,0);
geotsam 24:8f4b820d8de8 210 Odometria();
geotsam 24:8f4b820d8de8 211 go_to_point_with_angle(X, Y, rad_angle_check(theta+pi));
geotsam 14:d58f2bdbf42e 212 break;
AurelienBernier 11:e641aa08c92e 213 }
AurelienBernier 11:e641aa08c92e 214
geotsam 24:8f4b820d8de8 215 alpha = atan2((target_y-Y),(target_x-X))-theta;
geotsam 24:8f4b820d8de8 216 alpha = atan(sin(alpha)/cos(alpha));
geotsam 24:8f4b820d8de8 217 rho = dist(X, Y, target_x, target_y);
geotsam 24:8f4b820d8de8 218 d2 = rho;
geotsam 24:8f4b820d8de8 219 beta = -alpha-theta+target_angle;
geotsam 24:8f4b820d8de8 220
geotsam 24:8f4b820d8de8 221 //Computing angle error and distance towards the target value
geotsam 24:8f4b820d8de8 222 rho += dt*(-kRho*cos(alpha)*rho);
geotsam 24:8f4b820d8de8 223 temp = alpha;
geotsam 24:8f4b820d8de8 224 alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
geotsam 24:8f4b820d8de8 225 beta += dt*(-kRho*sin(temp));
geotsam 24:8f4b820d8de8 226 //pc.printf("\n\r d2=%f", d2);
geotsam 24:8f4b820d8de8 227 //pc.printf("\n\r dt=%f", dt);
geotsam 0:8bffb51cc345 228
geotsam 24:8f4b820d8de8 229 //Computing linear and angular velocities
geotsam 24:8f4b820d8de8 230 if(alpha>=-1.5708 && alpha<=1.5708){
geotsam 24:8f4b820d8de8 231 linear=kRho*rho;
geotsam 24:8f4b820d8de8 232 angular=ka*alpha+kb*beta;
geotsam 24:8f4b820d8de8 233 }
geotsam 24:8f4b820d8de8 234 else{
geotsam 24:8f4b820d8de8 235 linear=-kRho*rho;
geotsam 24:8f4b820d8de8 236 angular=-ka*alpha-kb*beta;
geotsam 24:8f4b820d8de8 237 }
geotsam 24:8f4b820d8de8 238 angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
geotsam 24:8f4b820d8de8 239 angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
geotsam 24:8f4b820d8de8 240
geotsam 24:8f4b820d8de8 241 pc.printf("\n\r a_r=%f", angular_right);
geotsam 24:8f4b820d8de8 242 pc.printf("\n\r a_l=%f", angular_left);
geotsam 24:8f4b820d8de8 243
geotsam 24:8f4b820d8de8 244 //Slowing down at the end for more precision
geotsam 24:8f4b820d8de8 245 //if (d2<25) {
geotsam 24:8f4b820d8de8 246 // speed = d2*30;
geotsam 24:8f4b820d8de8 247 // }
geotsam 24:8f4b820d8de8 248
geotsam 24:8f4b820d8de8 249 //Normalize speed for motors
geotsam 24:8f4b820d8de8 250 if(angular_left>angular_right) {
geotsam 24:8f4b820d8de8 251 angular_right=speed*angular_right/angular_left;
geotsam 24:8f4b820d8de8 252 angular_left=speed;
geotsam 24:8f4b820d8de8 253 } else {
geotsam 24:8f4b820d8de8 254 angular_left=speed*angular_left/angular_right;
geotsam 24:8f4b820d8de8 255 angular_right=speed;
geotsam 24:8f4b820d8de8 256 }
geotsam 24:8f4b820d8de8 257
geotsam 24:8f4b820d8de8 258 //pc.printf("\n\r X=%f", X);
geotsam 24:8f4b820d8de8 259 //pc.printf("\n\r Y=%f", Y);
geotsam 24:8f4b820d8de8 260
geotsam 24:8f4b820d8de8 261 pc.printf("\n\r a_r=%f", angular_right);
geotsam 24:8f4b820d8de8 262 pc.printf("\n\r a_l=%f", angular_left);
geotsam 0:8bffb51cc345 263
AurelienBernier 2:ea61e801e81f 264 //Updating motor velocities
AurelienBernier 1:f0807d5c5a4b 265 leftMotor(1,angular_left);
AurelienBernier 1:f0807d5c5a4b 266 rightMotor(1,angular_right);
geotsam 0:8bffb51cc345 267
AurelienBernier 7:c94070f9af78 268 wait(0.2);
AurelienBernier 6:afde4b08166b 269 //Timer stuff
AurelienBernier 6:afde4b08166b 270 t.stop();
geotsam 24:8f4b820d8de8 271 } while(d2>1 && (abs(target_angle-theta)>0.01) && tooClose==false);
geotsam 24:8f4b820d8de8 272
geotsam 24:8f4b820d8de8 273 //Stop at the end
geotsam 24:8f4b820d8de8 274 leftMotor(1,0);
geotsam 24:8f4b820d8de8 275 rightMotor(1,0);
geotsam 24:8f4b820d8de8 276
geotsam 24:8f4b820d8de8 277 if(turnFromObstacle){
geotsam 24:8f4b820d8de8 278 turnFromObstacle=false;
geotsam 24:8f4b820d8de8 279 tooClose=false;
geotsam 24:8f4b820d8de8 280 }
Ludwigfr 22:ebb37a249b5f 281 }
Ludwigfr 22:ebb37a249b5f 282
Ludwigfr 22:ebb37a249b5f 283 //Updates sonar values
geotsam 24:8f4b820d8de8 284 void update_sonar_values(float leftMm, float frontMm, float rightMm){
Ludwigfr 22:ebb37a249b5f 285
Ludwigfr 22:ebb37a249b5f 286 float currProba;
Ludwigfr 25:572c9e9a8809 287 float i_in_orthonormal;
Ludwigfr 25:572c9e9a8809 288 float j_in_orthonormal;
geotsam 24:8f4b820d8de8 289 for(int i=0;i<NB_CELL_WIDTH;i++){
geotsam 24:8f4b820d8de8 290 for(int j=0;j<NB_CELL_HEIGHT;j++){
geotsam 24:8f4b820d8de8 291 //check if the point A(x,y) in the world frame is within the range of the sonar (which has the coordinates xs, ys in the world frame)
geotsam 24:8f4b820d8de8 292 //check that again
Ludwigfr 22:ebb37a249b5f 293 //compute for front sonar
Ludwigfr 25:572c9e9a8809 294 i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i);
Ludwigfr 25:572c9e9a8809 295 j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j);
Ludwigfr 25:572c9e9a8809 296
Ludwigfr 25:572c9e9a8809 297 currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_front_x_in_orthonormal_x(),robot_sonar_front_y_in_orthonormal_y(),ANGLE_FRONT_TO_FRONT,frontMm/10);
Ludwigfr 22:ebb37a249b5f 298 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];//map is filled as map[0][0] get the data for the point closest to the origin
Ludwigfr 22:ebb37a249b5f 299 //compute for right sonar
Ludwigfr 25:572c9e9a8809 300 currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_right_x_in_orthonormal_x(),robot_sonar_right_y_in_orthonormal_y(),ANGLE_FRONT_TO_RIGHT,rightMm/10);
Ludwigfr 22:ebb37a249b5f 301 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
Ludwigfr 22:ebb37a249b5f 302 //compute for left sonar
Ludwigfr 25:572c9e9a8809 303 currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_left_x_in_orthonormal_x(),robot_sonar_left_y_in_orthonormal_y(),ANGLE_FRONT_TO_LEFT,leftMm/10);
Ludwigfr 22:ebb37a249b5f 304 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
Ludwigfr 22:ebb37a249b5f 305 }
Ludwigfr 22:ebb37a249b5f 306 }
Ludwigfr 22:ebb37a249b5f 307 }
Ludwigfr 22:ebb37a249b5f 308
Ludwigfr 25:572c9e9a8809 309 //ODOMETRIA MUST HAVE BEEN CALLED
Ludwigfr 22:ebb37a249b5f 310 //function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left pi/3, right -pi/3) returns the probability it's occupied/empty [0;1]
Ludwigfr 22:ebb37a249b5f 311 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){
Ludwigfr 22:ebb37a249b5f 312
Ludwigfr 22:ebb37a249b5f 313 float alpha=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
geotsam 24:8f4b820d8de8 314 float alphaBeforeAdjustment=alpha-theta-angleFromSonarPosition;
Ludwigfr 22:ebb37a249b5f 315 alpha=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
Ludwigfr 22:ebb37a249b5f 316 float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
Ludwigfr 22:ebb37a249b5f 317
Ludwigfr 22:ebb37a249b5f 318 //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
Ludwigfr 22:ebb37a249b5f 319 //check if absolute difference between the angles is no more than Omega/2
Ludwigfr 22:ebb37a249b5f 320 if( distancePointToSonar < (RANGE_SONAR)&& (alpha <= ANGLE_SONAR/2 || alpha >= rad_angle_check(-ANGLE_SONAR/2))){
Ludwigfr 22:ebb37a249b5f 321 if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
Ludwigfr 22:ebb37a249b5f 322 //point before obstacle, probably empty
Ludwigfr 22:ebb37a249b5f 323 /*****************************************************************************/
Ludwigfr 22:ebb37a249b5f 324 float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 22:ebb37a249b5f 325 float Er;
Ludwigfr 22:ebb37a249b5f 326 if(distancePointToSonar < RANGE_SONAR_MIN){
Ludwigfr 22:ebb37a249b5f 327 //point before minimum sonar range
Ludwigfr 22:ebb37a249b5f 328 Er=0.f;
Ludwigfr 22:ebb37a249b5f 329 }else{
Ludwigfr 22:ebb37a249b5f 330 //point after minimum sonar range
Ludwigfr 22:ebb37a249b5f 331 Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
Ludwigfr 22:ebb37a249b5f 332 }
Ludwigfr 22:ebb37a249b5f 333 /*****************************************************************************/
Ludwigfr 22:ebb37a249b5f 334 return (1.f-Er*Ea)/2.f;
Ludwigfr 22:ebb37a249b5f 335 }else{
Ludwigfr 22:ebb37a249b5f 336 //probably occupied
Ludwigfr 22:ebb37a249b5f 337 /*****************************************************************************/
Ludwigfr 22:ebb37a249b5f 338 float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 22:ebb37a249b5f 339 float Or;
Ludwigfr 22:ebb37a249b5f 340 if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
Ludwigfr 22:ebb37a249b5f 341 //point between distanceObstacleDetected +- INCERTITUDE_SONAR
Ludwigfr 22:ebb37a249b5f 342 Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
Ludwigfr 22:ebb37a249b5f 343 }else{
Ludwigfr 22:ebb37a249b5f 344 //point after in range of the sonar but after the zone detected
Ludwigfr 22:ebb37a249b5f 345 Or=0;
Ludwigfr 22:ebb37a249b5f 346 }
Ludwigfr 22:ebb37a249b5f 347 /*****************************************************************************/
Ludwigfr 22:ebb37a249b5f 348 return (1+Or*Oa)/2;
Ludwigfr 22:ebb37a249b5f 349 }
Ludwigfr 22:ebb37a249b5f 350 }else{
Ludwigfr 25:572c9e9a8809 351 //not checked by the sonar
Ludwigfr 22:ebb37a249b5f 352 return 0.5;
AurelienBernier 19:dbc5fbad4975 353 }
Ludwigfr 22:ebb37a249b5f 354 }
Ludwigfr 22:ebb37a249b5f 355
Ludwigfr 25:572c9e9a8809 356 void print_final_map() {
Ludwigfr 22:ebb37a249b5f 357 float currProba;
geotsam 24:8f4b820d8de8 358 pc.printf("\n\r");
geotsam 24:8f4b820d8de8 359 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
geotsam 24:8f4b820d8de8 360 for (int x= 0; x<NB_CELL_WIDTH; x++) {
geotsam 24:8f4b820d8de8 361 currProba=log_to_proba(map[x][y]);
geotsam 24:8f4b820d8de8 362 if ( currProba < 0.5) {
geotsam 24:8f4b820d8de8 363 pc.printf(" 0 ");
Ludwigfr 22:ebb37a249b5f 364 } else {
Ludwigfr 22:ebb37a249b5f 365 if(currProba==0.5)
geotsam 24:8f4b820d8de8 366 pc.printf(" . ");
Ludwigfr 22:ebb37a249b5f 367 else
geotsam 24:8f4b820d8de8 368 pc.printf(" + ");
Ludwigfr 22:ebb37a249b5f 369 }
Ludwigfr 22:ebb37a249b5f 370 }
geotsam 24:8f4b820d8de8 371 pc.printf("\n\r");
Ludwigfr 22:ebb37a249b5f 372 }
Ludwigfr 22:ebb37a249b5f 373 }
Ludwigfr 22:ebb37a249b5f 374
Ludwigfr 25:572c9e9a8809 375 void print_final_map_with_robot_position() {
geotsam 24:8f4b820d8de8 376 float currProba;
Ludwigfr 25:572c9e9a8809 377 Odometria();
Ludwigfr 25:572c9e9a8809 378 float Xrobot=robot_center_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 379 float Yrobot=robot_center_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 380
Ludwigfr 25:572c9e9a8809 381 float heightIndiceInOrthonormal;
Ludwigfr 25:572c9e9a8809 382 float widthIndiceInOrthonormal;
Ludwigfr 25:572c9e9a8809 383
Ludwigfr 25:572c9e9a8809 384 float heightMalus=-(3*sizeCellX/2);
Ludwigfr 25:572c9e9a8809 385 float heightBonus=sizeCellX/2;
Ludwigfr 25:572c9e9a8809 386
Ludwigfr 25:572c9e9a8809 387 float widthMalus=-(3*sizeCellY/2);
Ludwigfr 25:572c9e9a8809 388 float widthBonus=sizeCellY/2;
Ludwigfr 25:572c9e9a8809 389
geotsam 24:8f4b820d8de8 390 pc.printf("\n\r");
geotsam 24:8f4b820d8de8 391 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
geotsam 24:8f4b820d8de8 392 for (int x= 0; x<NB_CELL_WIDTH; x++) {
Ludwigfr 25:572c9e9a8809 393 heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
Ludwigfr 25:572c9e9a8809 394 widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
Ludwigfr 25:572c9e9a8809 395 if(Xrobot >= (heightIndiceInOrthonormal+heightMalus) && Xrobot <= (heightIndiceInOrthonormal+heightBonus) && Yrobot >= (widthIndiceInOrthonormal+widthMalus) && Yrobot <= (widthIndiceInOrthonormal+widthBonus))
geotsam 24:8f4b820d8de8 396 pc.printf(" R ");
Ludwigfr 25:572c9e9a8809 397 else{
Ludwigfr 25:572c9e9a8809 398 currProba=log_to_proba(map[x][y]);
Ludwigfr 25:572c9e9a8809 399 if ( currProba < 0.5)
Ludwigfr 25:572c9e9a8809 400 pc.printf(" 0 ");
Ludwigfr 25:572c9e9a8809 401 else{
Ludwigfr 25:572c9e9a8809 402 if(currProba==0.5)
Ludwigfr 25:572c9e9a8809 403 pc.printf(" . ");
Ludwigfr 25:572c9e9a8809 404 else
Ludwigfr 25:572c9e9a8809 405 pc.printf(" + ");
Ludwigfr 25:572c9e9a8809 406 }
geotsam 24:8f4b820d8de8 407 }
geotsam 24:8f4b820d8de8 408 }
geotsam 24:8f4b820d8de8 409 pc.printf("\n\r");
geotsam 24:8f4b820d8de8 410 }
geotsam 24:8f4b820d8de8 411 }
Ludwigfr 22:ebb37a249b5f 412
Ludwigfr 22:ebb37a249b5f 413
Ludwigfr 22:ebb37a249b5f 414 //MATHS heavy functions
Ludwigfr 22:ebb37a249b5f 415 /**********************************************************************/
Ludwigfr 22:ebb37a249b5f 416 //Distance computation function
Ludwigfr 22:ebb37a249b5f 417 float dist(float robot_x, float robot_y, float target_x, float target_y){
Ludwigfr 22:ebb37a249b5f 418 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
Ludwigfr 22:ebb37a249b5f 419 }
Ludwigfr 22:ebb37a249b5f 420
geotsam 24:8f4b820d8de8 421 //returns the probability [0,1] that the cell is occupied from the log valAue lt
Ludwigfr 22:ebb37a249b5f 422 float log_to_proba(float lt){
Ludwigfr 22:ebb37a249b5f 423 return 1-1/(1+exp(lt));
Ludwigfr 22:ebb37a249b5f 424 }
Ludwigfr 22:ebb37a249b5f 425
Ludwigfr 22:ebb37a249b5f 426 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 22:ebb37a249b5f 427 float proba_to_log(float p){
Ludwigfr 22:ebb37a249b5f 428 return log(p/(1-p));
Ludwigfr 22:ebb37a249b5f 429 }
Ludwigfr 22:ebb37a249b5f 430
Ludwigfr 22:ebb37a249b5f 431 //returns the new log value
Ludwigfr 22:ebb37a249b5f 432 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){
Ludwigfr 22:ebb37a249b5f 433 return previousLogValue+proba_to_log(currentProbability)-originalLogvalue;
Ludwigfr 22:ebb37a249b5f 434 }
Ludwigfr 22:ebb37a249b5f 435
Ludwigfr 22:ebb37a249b5f 436 //makes the angle inAngle between 0 and 2pi
Ludwigfr 22:ebb37a249b5f 437 float rad_angle_check(float inAngle){
Ludwigfr 22:ebb37a249b5f 438 //cout<<"before :"<<inAngle;
Ludwigfr 22:ebb37a249b5f 439 if(inAngle > 0){
Ludwigfr 22:ebb37a249b5f 440 while(inAngle > (2*pi))
Ludwigfr 22:ebb37a249b5f 441 inAngle-=2*pi;
Ludwigfr 22:ebb37a249b5f 442 }else{
Ludwigfr 22:ebb37a249b5f 443 while(inAngle < 0)
Ludwigfr 22:ebb37a249b5f 444 inAngle+=2*pi;
Ludwigfr 22:ebb37a249b5f 445 }
Ludwigfr 22:ebb37a249b5f 446 //cout<<" after :"<<inAngle<<endl;
Ludwigfr 22:ebb37a249b5f 447 return inAngle;
Ludwigfr 22:ebb37a249b5f 448 }
Ludwigfr 22:ebb37a249b5f 449
Ludwigfr 22:ebb37a249b5f 450 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 22:ebb37a249b5f 451 float compute_angle_between_vectors(float x, float y,float xs,float ys){
Ludwigfr 22:ebb37a249b5f 452 //alpha angle between ->x and ->SA
Ludwigfr 22:ebb37a249b5f 453 //vector S to A ->SA
Ludwigfr 22:ebb37a249b5f 454 float vSAx=x-xs;
Ludwigfr 22:ebb37a249b5f 455 float vSAy=y-ys;
Ludwigfr 22:ebb37a249b5f 456 //norme SA
Ludwigfr 22:ebb37a249b5f 457 float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
Ludwigfr 22:ebb37a249b5f 458 //vector ->x (1,0)
Ludwigfr 22:ebb37a249b5f 459 float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
Ludwigfr 22:ebb37a249b5f 460 //vector ->y (0,1)
Ludwigfr 22:ebb37a249b5f 461 float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
Ludwigfr 22:ebb37a249b5f 462 if (sinAlpha < 0)
Ludwigfr 22:ebb37a249b5f 463 return -acos(cosAlpha);
Ludwigfr 22:ebb37a249b5f 464 else
Ludwigfr 22:ebb37a249b5f 465 return acos(cosAlpha);
Ludwigfr 25:572c9e9a8809 466 }
Ludwigfr 25:572c9e9a8809 467
Ludwigfr 25:572c9e9a8809 468 float robot_center_x_in_orthonormal_x(){
Ludwigfr 25:572c9e9a8809 469 return Y;
Ludwigfr 25:572c9e9a8809 470 }
Ludwigfr 25:572c9e9a8809 471
Ludwigfr 25:572c9e9a8809 472 float robot_center_y_in_orthonormal_y(){
Ludwigfr 25:572c9e9a8809 473 return NB_CELL_WIDTH*sizeCellX-X;
Ludwigfr 25:572c9e9a8809 474 }
Ludwigfr 25:572c9e9a8809 475
Ludwigfr 25:572c9e9a8809 476 float robot_sonar_front_x_in_orthonormal_x(){
Ludwigfr 25:572c9e9a8809 477 return Y+DISTANCE_SONAR_FRONT_Y;
Ludwigfr 25:572c9e9a8809 478 }
Ludwigfr 25:572c9e9a8809 479 float robot_sonar_front_y_in_orthonormal_y(){
Ludwigfr 25:572c9e9a8809 480 return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_FRONT_X;
Ludwigfr 25:572c9e9a8809 481 }
Ludwigfr 25:572c9e9a8809 482
Ludwigfr 25:572c9e9a8809 483 float robot_sonar_right_x_in_orthonormal_x(){
Ludwigfr 25:572c9e9a8809 484 return Y+DISTANCE_SONAR_RIGHT_Y;
Ludwigfr 25:572c9e9a8809 485 }
Ludwigfr 25:572c9e9a8809 486 float robot_sonar_right_y_in_orthonormal_y(){
Ludwigfr 25:572c9e9a8809 487 return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_RIGHT_X;
Ludwigfr 25:572c9e9a8809 488 }
Ludwigfr 25:572c9e9a8809 489
Ludwigfr 25:572c9e9a8809 490 float robot_sonar_left_x_in_orthonormal_x(){
Ludwigfr 25:572c9e9a8809 491 return Y+DISTANCE_SONAR_LEFT_Y;
Ludwigfr 25:572c9e9a8809 492 }
Ludwigfr 25:572c9e9a8809 493 float robot_sonar_left_y_in_orthonormal_y(){
Ludwigfr 25:572c9e9a8809 494 return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_LEFT_X;
Ludwigfr 25:572c9e9a8809 495 }
Ludwigfr 25:572c9e9a8809 496
Ludwigfr 25:572c9e9a8809 497 float estimated_width_indice_in_orthonormal_x(int i){
Ludwigfr 25:572c9e9a8809 498 return sizeCellX/2+i*sizeCellX;
Ludwigfr 25:572c9e9a8809 499 }
Ludwigfr 25:572c9e9a8809 500 float estimated_height_indice_in_orthonormal_y(int j){
Ludwigfr 25:572c9e9a8809 501 return sizeCellY/2+j*sizeCellY;
geotsam 24:8f4b820d8de8 502 }