Aurélien Bernier Levalois / Mbed 2 deprecated Mapping

Mapping for TP2

Dependencies:   ISR_Mini-explorer mbed

Fork of GoToPointWithAngle by Georgios Tsamis

main.cpp@21:ebb37a249b5f, 2017-04-18 (annotated)

Committer:
Ludwigfr
Date:
Tue Apr 18 17:48:26 2017 +0000
Revision:
21:ebb37a249b5f
Parent:
20:62154d644531
Child:
23:901fc468b8a7
Still TODOs waiting to be closed

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 21:ebb37a249b5f 4
Ludwigfr 21:ebb37a249b5f 5 using namespace std;
AurelienBernier 4:8c56c3ba6e54 6
Ludwigfr 21:ebb37a249b5f 7 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 21:ebb37a249b5f 8 void fill_initial_log_values();
Ludwigfr 21:ebb37a249b5f 9 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 21:ebb37a249b5f 10 void randomize_and_map();
Ludwigfr 21:ebb37a249b5f 11 //go the the given position while updating the map
Ludwigfr 21:ebb37a249b5f 12 void go_to_point_with_angle(float target_x, float target_y, float target_angle);
Ludwigfr 21:ebb37a249b5f 13 //Updates sonar values
Ludwigfr 21:ebb37a249b5f 14 void update_sonar_values();
Ludwigfr 21: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 21:ebb37a249b5f 16 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
Ludwigfr 21:ebb37a249b5f 17 //print the map
Ludwigfr 21:ebb37a249b5f 18 void print_final_map();
geotsam 0:8bffb51cc345 19
Ludwigfr 21:ebb37a249b5f 20 //MATHS heavy functions
Ludwigfr 21:ebb37a249b5f 21 float dist(float robot_x, float robot_y, float target_x, float target_y);
Ludwigfr 21:ebb37a249b5f 22 //returns the probability [0,1] that the cell is occupied from the log value lt
Ludwigfr 21:ebb37a249b5f 23 float log_to_proba(float lt);
Ludwigfr 21:ebb37a249b5f 24 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 21:ebb37a249b5f 25 float proba_to_log(float p);
Ludwigfr 21:ebb37a249b5f 26 //returns the new log value
Ludwigfr 21:ebb37a249b5f 27 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue );
Ludwigfr 21:ebb37a249b5f 28 //makes the angle inAngle between 0 and 2pi
Ludwigfr 21:ebb37a249b5f 29 float rad_angle_check(float inAngle);
Ludwigfr 21:ebb37a249b5f 30 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 21:ebb37a249b5f 31 float compute_angle_between_vectors(float x, float y,float xs,float ys);
AurelienBernier 8:109314be5b68 32
Ludwigfr 21:ebb37a249b5f 33 const float pi=3.14159;
Ludwigfr 21:ebb37a249b5f 34 //spec of the sonar
Ludwigfr 21:ebb37a249b5f 35 //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
Ludwigfr 21:ebb37a249b5f 36 const float RANGE_SONAR=50;//RADIUS_WHEELS
Ludwigfr 21:ebb37a249b5f 37 const float RANGE_SONAR_MIN=10;//Rmin
Ludwigfr 21:ebb37a249b5f 38 const float INCERTITUDE_SONAR=10;
Ludwigfr 21:ebb37a249b5f 39 const float ANGLE_SONAR=pi/3;//Omega
Ludwigfr 21:ebb37a249b5f 40
Ludwigfr 21:ebb37a249b5f 41 //those distance and angle are approximation in need of measurements
Ludwigfr 21:ebb37a249b5f 42 const float ANGLE_FRONT_TO_LEFT=pi/3;
Ludwigfr 21:ebb37a249b5f 43 const float DISTANCE_SONAR_LEFT_X=5;
Ludwigfr 21:ebb37a249b5f 44 const float DISTANCE_SONAR_LEFT_Y=5;
Ludwigfr 21:ebb37a249b5f 45 float leftMm;
Ludwigfr 21:ebb37a249b5f 46
Ludwigfr 21:ebb37a249b5f 47 const float ANGLE_FRONT_TO_RIGHT=-pi/3;
Ludwigfr 21:ebb37a249b5f 48 const float DISTANCE_SONAR_RIGHT_X=-5;
Ludwigfr 21:ebb37a249b5f 49 const float DISTANCE_SONAR_RIGHT_Y=5;
Ludwigfr 21:ebb37a249b5f 50 float rightMm;
AurelienBernier 11:e641aa08c92e 51
Ludwigfr 21:ebb37a249b5f 52 const float ANGLE_FRONT_TO_FRONT=0;
Ludwigfr 21:ebb37a249b5f 53 const float DISTANCE_SONAR_FRONT_X=0;
Ludwigfr 21:ebb37a249b5f 54 const float DISTANCE_SONAR_FRONT_Y=5;
Ludwigfr 21:ebb37a249b5f 55 float frontMm;
Ludwigfr 21:ebb37a249b5f 56
Ludwigfr 21:ebb37a249b5f 57 //TODO adjust the size of the map for computation time (25*25?)
Ludwigfr 21:ebb37a249b5f 58 const int SIZE_MAP=25;
Ludwigfr 21:ebb37a249b5f 59
Ludwigfr 21:ebb37a249b5f 60 //position and orientation of the robot when put on the map (ODOMETRY doesn't know those)
Ludwigfr 21:ebb37a249b5f 61 const float DEFAULT_X=0;
Ludwigfr 21:ebb37a249b5f 62 const float DEFAULT_Y=0;
Ludwigfr 21:ebb37a249b5f 63 const float DEFAULT_THETA=pi/2;
Ludwigfr 21:ebb37a249b5f 64
Ludwigfr 21:ebb37a249b5f 65 //used to create the map 250 represent the 250cm of the table where the robot is tested
Ludwigfr 21:ebb37a249b5f 66 float sizeCell=250/(float)SIZE_MAP;
Ludwigfr 21:ebb37a249b5f 67 float map[SIZE_MAP][SIZE_MAP];//contains the log values for each cell
Ludwigfr 21:ebb37a249b5f 68 float initialLogValues[SIZE_MAP][SIZE_MAP];
Ludwigfr 21:ebb37a249b5f 69
Ludwigfr 21:ebb37a249b5f 70 //Diameter of a wheel and distance between the 2
Ludwigfr 21:ebb37a249b5f 71 const float RADIUS_WHEELS=3.25;
Ludwigfr 21:ebb37a249b5f 72 const float DISTANCE_WHEELS=7.2;
Ludwigfr 21:ebb37a249b5f 73
AurelienBernier 8:109314be5b68 74 float alpha; //angle error
AurelienBernier 8:109314be5b68 75 float rho; //distance from target
AurelienBernier 8:109314be5b68 76 float beta;
AurelienBernier 11:e641aa08c92e 77 float kRho=12, ka=30, kb=-13; //Kappa values
AurelienBernier 8:109314be5b68 78 float linear, angular, angular_left, angular_right;
AurelienBernier 8:109314be5b68 79 float dt=0.5;
AurelienBernier 8:109314be5b68 80 float temp;
AurelienBernier 8:109314be5b68 81 float d2;
Ludwigfr 21:ebb37a249b5f 82 Timer t;
AurelienBernier 8:109314be5b68 83
AurelienBernier 8:109314be5b68 84 int speed=999; // Max speed at beggining of movement
AurelienBernier 8:109314be5b68 85
geotsam 0:8bffb51cc345 86 int main(){
geotsam 17:caf393b63e27 87
geotsam 13:41f75c132135 88 i2c1.frequency(100000);
AurelienBernier 2:ea61e801e81f 89 initRobot(); //Initializing the robot
geotsam 0:8bffb51cc345 90 pc.baud(9600); // baud for the pc communication
geotsam 0:8bffb51cc345 91
Ludwigfr 21:ebb37a249b5f 92 measure_always_on();//TODO check if needed
AurelienBernier 18:dbc5fbad4975 93
Ludwigfr 21:ebb37a249b5f 94 fill_initial_log_values();
geotsam 13:41f75c132135 95
AurelienBernier 20:62154d644531 96 for (int i = 0; i<25; i++) {
Ludwigfr 21:ebb37a249b5f 97 randomize_and_map();
geotsam 17:caf393b63e27 98 }
AurelienBernier 8:109314be5b68 99 //Stop at the end
AurelienBernier 8:109314be5b68 100 leftMotor(1,0);
AurelienBernier 8:109314be5b68 101 rightMotor(1,0);
AurelienBernier 8:109314be5b68 102
AurelienBernier 20:62154d644531 103 //pc.printf("\n\r %f -- arrived!", rho);
Ludwigfr 21:ebb37a249b5f 104 print_final_map();
AurelienBernier 8:109314be5b68 105 }
AurelienBernier 8:109314be5b68 106
Ludwigfr 21:ebb37a249b5f 107 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 21:ebb37a249b5f 108 void fill_initial_log_values(){
Ludwigfr 21:ebb37a249b5f 109 //Fill map, we know the border are occupied
Ludwigfr 21:ebb37a249b5f 110 for (int i = 0; i<SIZE_MAP; i++) {
Ludwigfr 21:ebb37a249b5f 111 for (int j = 0; j<SIZE_MAP; j++) {
Ludwigfr 21:ebb37a249b5f 112 if(j==0 || j==SIZE_MAP-1 || i==0 || i==SIZE_MAP-1)
Ludwigfr 21:ebb37a249b5f 113 initialLogValues[i][j] = proba_to_log(1);
Ludwigfr 21:ebb37a249b5f 114 else
Ludwigfr 21:ebb37a249b5f 115 initialLogValues[i][j] = proba_to_log(0.5);
AurelienBernier 20:62154d644531 116 }
Ludwigfr 21:ebb37a249b5f 117 }
AurelienBernier 8:109314be5b68 118 }
AurelienBernier 8:109314be5b68 119
Ludwigfr 21:ebb37a249b5f 120 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 21:ebb37a249b5f 121 void randomize_and_map() {
Ludwigfr 21:ebb37a249b5f 122 float target_x = (rand()%2500)/10;//for decimal precision
Ludwigfr 21:ebb37a249b5f 123 float target_y = (rand()%2500)/10;
Ludwigfr 21:ebb37a249b5f 124 float target_angle = ((float)(rand()%31416)-15708)/10000.0;
Ludwigfr 21:ebb37a249b5f 125 /*
AurelienBernier 18:dbc5fbad4975 126 pc.printf("\n\r targ_X=%f", target_x);
AurelienBernier 18:dbc5fbad4975 127 pc.printf("\n\r targ_Y=%f", target_y);
AurelienBernier 18:dbc5fbad4975 128 pc.printf("\n\r targ_Angle=%f", target_angle);
Ludwigfr 21:ebb37a249b5f 129 */
Ludwigfr 21:ebb37a249b5f 130 go_to_point_with_angle(target_x, target_y, target_angle);
AurelienBernier 18:dbc5fbad4975 131 }
AurelienBernier 18:dbc5fbad4975 132
Ludwigfr 21:ebb37a249b5f 133 //go the the given position while updating the map
Ludwigfr 21:ebb37a249b5f 134 //TODO clean this procedure it's ugly as hell and too long
Ludwigfr 21:ebb37a249b5f 135 void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
Ludwigfr 21:ebb37a249b5f 136 bool tooClose=false;
AurelienBernier 8:109314be5b68 137 do {
geotsam 0:8bffb51cc345 138 pc.printf("\n\n\r entered while");
AurelienBernier 2:ea61e801e81f 139
AurelienBernier 6:afde4b08166b 140 //Timer stuff
AurelienBernier 6:afde4b08166b 141 dt = t.read();
AurelienBernier 6:afde4b08166b 142 t.reset();
AurelienBernier 6:afde4b08166b 143 t.start();
AurelienBernier 6:afde4b08166b 144
geotsam 15:d58f2bdbf42e 145 //Updating X,Y and theta with the odometry values
geotsam 15:d58f2bdbf42e 146 Odometria();
geotsam 15:d58f2bdbf42e 147
Ludwigfr 21:ebb37a249b5f 148 update_sonar_values();
Ludwigfr 21:ebb37a249b5f 149
AurelienBernier 11:e641aa08c92e 150 if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
AurelienBernier 20:62154d644531 151 tooClose = true;
geotsam 15:d58f2bdbf42e 152 break;
AurelienBernier 11:e641aa08c92e 153 }
AurelienBernier 11:e641aa08c92e 154
AurelienBernier 4:8c56c3ba6e54 155 alpha = atan2((target_y-Y),(target_x-X))-theta;
AurelienBernier 4:8c56c3ba6e54 156 alpha = atan(sin(alpha)/cos(alpha));
AurelienBernier 4:8c56c3ba6e54 157 rho = dist(X, Y, target_x, target_y);
AurelienBernier 6:afde4b08166b 158 d2 = rho;
AurelienBernier 18:dbc5fbad4975 159 beta = -alpha-theta+target_angle;
AurelienBernier 6:afde4b08166b 160
AurelienBernier 2:ea61e801e81f 161 //Computing angle error and distance towards the target value
AurelienBernier 4:8c56c3ba6e54 162 rho += dt*(-kRho*cos(alpha)*rho);
AurelienBernier 4:8c56c3ba6e54 163 temp = alpha;
AurelienBernier 6:afde4b08166b 164 alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
AurelienBernier 6:afde4b08166b 165 beta += dt*(-kRho*sin(temp));
AurelienBernier 6:afde4b08166b 166 pc.printf("\n\r d2=%f", d2);
AurelienBernier 6:afde4b08166b 167 pc.printf("\n\r dt=%f", dt);
geotsam 0:8bffb51cc345 168
AurelienBernier 2:ea61e801e81f 169 //Computing linear and angular velocities
AurelienBernier 4:8c56c3ba6e54 170 if(alpha>=-1.5708 && alpha<=1.5708){
AurelienBernier 4:8c56c3ba6e54 171 linear=kRho*rho;
AurelienBernier 4:8c56c3ba6e54 172 angular=ka*alpha+kb*beta;
geotsam 3:1e0f4cb93eda 173 }
geotsam 3:1e0f4cb93eda 174 else{
AurelienBernier 4:8c56c3ba6e54 175 linear=-kRho*rho;
AurelienBernier 4:8c56c3ba6e54 176 angular=-ka*alpha-kb*beta;
geotsam 3:1e0f4cb93eda 177 }
Ludwigfr 21:ebb37a249b5f 178 angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
Ludwigfr 21:ebb37a249b5f 179 angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
geotsam 0:8bffb51cc345 180
AurelienBernier 2:ea61e801e81f 181 //Slowing down at the end for more precision
AurelienBernier 6:afde4b08166b 182 if (d2<25) {
AurelienBernier 6:afde4b08166b 183 speed = d2*30;
geotsam 0:8bffb51cc345 184 }
AurelienBernier 2:ea61e801e81f 185
AurelienBernier 2:ea61e801e81f 186 //Normalize speed for motors
geotsam 0:8bffb51cc345 187 if(angular_left>angular_right) {
geotsam 0:8bffb51cc345 188 angular_right=speed*angular_right/angular_left;
geotsam 0:8bffb51cc345 189 angular_left=speed;
geotsam 0:8bffb51cc345 190 } else {
geotsam 0:8bffb51cc345 191 angular_left=speed*angular_left/angular_right;
geotsam 0:8bffb51cc345 192 angular_right=speed;
geotsam 0:8bffb51cc345 193 }
geotsam 0:8bffb51cc345 194
geotsam 0:8bffb51cc345 195 pc.printf("\n\r X=%f", X);
geotsam 0:8bffb51cc345 196 pc.printf("\n\r Y=%f", Y);
AurelienBernier 14:44ab4626f1ad 197 pc.printf("\n\r leftMm=%f", leftMm);
AurelienBernier 14:44ab4626f1ad 198 pc.printf("\n\r frontMm=%f", frontMm);
AurelienBernier 14:44ab4626f1ad 199 pc.printf("\n\r rightMm=%f", rightMm);
geotsam 0:8bffb51cc345 200
AurelienBernier 2:ea61e801e81f 201 //Updating motor velocities
AurelienBernier 1:f0807d5c5a4b 202 leftMotor(1,angular_left);
AurelienBernier 1:f0807d5c5a4b 203 rightMotor(1,angular_right);
geotsam 0:8bffb51cc345 204
AurelienBernier 7:c94070f9af78 205 wait(0.2);
AurelienBernier 6:afde4b08166b 206 //Timer stuff
AurelienBernier 6:afde4b08166b 207 t.stop();
Ludwigfr 21:ebb37a249b5f 208 } while(d2>1 && tooClose==false);
Ludwigfr 21:ebb37a249b5f 209 }
Ludwigfr 21:ebb37a249b5f 210
Ludwigfr 21:ebb37a249b5f 211 //Updates sonar values
Ludwigfr 21:ebb37a249b5f 212 void update_sonar_values(){
Ludwigfr 21:ebb37a249b5f 213
Ludwigfr 21:ebb37a249b5f 214 float leftMm = get_distance_left_sensor();
Ludwigfr 21:ebb37a249b5f 215 float frontMm = get_distance_front_sensor();
Ludwigfr 21:ebb37a249b5f 216 float rightMm = get_distance_right_sensor();
Ludwigfr 21:ebb37a249b5f 217
Ludwigfr 21:ebb37a249b5f 218 float currProba;
Ludwigfr 21:ebb37a249b5f 219 for(int i=0;i<SIZE_MAP;i++){
Ludwigfr 21:ebb37a249b5f 220 for(int j=0;j<SIZE_MAP;j++){
Ludwigfr 21:ebb37a249b5f 221 //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)
Ludwigfr 21:ebb37a249b5f 222
Ludwigfr 21:ebb37a249b5f 223 //compute for front sonar
Ludwigfr 21:ebb37a249b5f 224 currProba=compute_probability_t(sizeCell/2+i*sizeCell,sizeCell/2+j*sizeCell,SIZE_MAP*sizeCell-X+DEFAULT_X+DISTANCE_SONAR_FRONT_X,Y+DEFAULT_Y+DISTANCE_SONAR_FRONT_Y,ANGLE_FRONT_TO_FRONT,frontMm/10);
Ludwigfr 21:ebb37a249b5f 225 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 21:ebb37a249b5f 226 //compute for right sonar
Ludwigfr 21:ebb37a249b5f 227 currProba=compute_probability_t(sizeCell/2+i*sizeCell,sizeCell/2+j*sizeCell,SIZE_MAP*sizeCell-X+DEFAULT_X+DISTANCE_SONAR_RIGHT_X,Y+DEFAULT_Y+DISTANCE_SONAR_RIGHT_Y,ANGLE_FRONT_TO_RIGHT,rightMm/10);
Ludwigfr 21:ebb37a249b5f 228 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
Ludwigfr 21:ebb37a249b5f 229 //compute for left sonar
Ludwigfr 21:ebb37a249b5f 230 currProba=compute_probability_t(sizeCell/2+i*sizeCell,sizeCell/2+j*sizeCell,SIZE_MAP*sizeCell-X+DEFAULT_X+DISTANCE_SONAR_LEFT_X,Y+DEFAULT_Y+DISTANCE_SONAR_LEFT_Y,ANGLE_FRONT_TO_LEFT,leftMm/10);
Ludwigfr 21:ebb37a249b5f 231 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
Ludwigfr 21:ebb37a249b5f 232 }
Ludwigfr 21:ebb37a249b5f 233 }
Ludwigfr 21:ebb37a249b5f 234 }
Ludwigfr 21:ebb37a249b5f 235
Ludwigfr 21:ebb37a249b5f 236 //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 21:ebb37a249b5f 237 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){
Ludwigfr 21:ebb37a249b5f 238
Ludwigfr 21:ebb37a249b5f 239 float alpha=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
Ludwigfr 21:ebb37a249b5f 240 float alphaBeforeAdjustment=alpha-theta-DEFAULT_THETA-angleFromSonarPosition;
Ludwigfr 21:ebb37a249b5f 241 alpha=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
Ludwigfr 21:ebb37a249b5f 242 float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
Ludwigfr 21:ebb37a249b5f 243
Ludwigfr 21:ebb37a249b5f 244 //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
Ludwigfr 21:ebb37a249b5f 245 //check if absolute difference between the angles is no more than Omega/2
Ludwigfr 21:ebb37a249b5f 246 if( distancePointToSonar < (RANGE_SONAR)&& (alpha <= ANGLE_SONAR/2 || alpha >= rad_angle_check(-ANGLE_SONAR/2))){
Ludwigfr 21:ebb37a249b5f 247 if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
Ludwigfr 21:ebb37a249b5f 248 //point before obstacle, probably empty
Ludwigfr 21:ebb37a249b5f 249 /*****************************************************************************/
Ludwigfr 21:ebb37a249b5f 250 float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 21:ebb37a249b5f 251 float Er;
Ludwigfr 21:ebb37a249b5f 252 if(distancePointToSonar < RANGE_SONAR_MIN){
Ludwigfr 21:ebb37a249b5f 253 //point before minimum sonar range
Ludwigfr 21:ebb37a249b5f 254 Er=0.f;
Ludwigfr 21:ebb37a249b5f 255 }else{
Ludwigfr 21:ebb37a249b5f 256 //point after minimum sonar range
Ludwigfr 21:ebb37a249b5f 257 Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
Ludwigfr 21:ebb37a249b5f 258 }
Ludwigfr 21:ebb37a249b5f 259 /*****************************************************************************/
Ludwigfr 21:ebb37a249b5f 260 return (1.f-Er*Ea)/2.f;
Ludwigfr 21:ebb37a249b5f 261 }else{
Ludwigfr 21:ebb37a249b5f 262 //probably occupied
Ludwigfr 21:ebb37a249b5f 263 /*****************************************************************************/
Ludwigfr 21:ebb37a249b5f 264 float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 21:ebb37a249b5f 265 float Or;
Ludwigfr 21:ebb37a249b5f 266 if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
Ludwigfr 21:ebb37a249b5f 267 //point between distanceObstacleDetected +- INCERTITUDE_SONAR
Ludwigfr 21:ebb37a249b5f 268 Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
Ludwigfr 21:ebb37a249b5f 269 }else{
Ludwigfr 21:ebb37a249b5f 270 //point after in range of the sonar but after the zone detected
Ludwigfr 21:ebb37a249b5f 271 Or=0;
Ludwigfr 21:ebb37a249b5f 272 }
Ludwigfr 21:ebb37a249b5f 273 /*****************************************************************************/
Ludwigfr 21:ebb37a249b5f 274 return (1+Or*Oa)/2;
Ludwigfr 21:ebb37a249b5f 275 }
Ludwigfr 21:ebb37a249b5f 276 }else{
Ludwigfr 21:ebb37a249b5f 277 //not in range of the sonar
Ludwigfr 21:ebb37a249b5f 278 return 0.5;
AurelienBernier 18:dbc5fbad4975 279 }
Ludwigfr 21:ebb37a249b5f 280 }
Ludwigfr 21:ebb37a249b5f 281
Ludwigfr 21:ebb37a249b5f 282 void print_final_map() {
Ludwigfr 21:ebb37a249b5f 283 float currProba;
Ludwigfr 21:ebb37a249b5f 284 for (int x = 0; x<SIZE_MAP; x++) {
Ludwigfr 21:ebb37a249b5f 285 for (int y = 0; y<SIZE_MAP; y++) {
Ludwigfr 21:ebb37a249b5f 286 currProba=log_to_proba(map[SIZE_MAP-1-x][y]);
Ludwigfr 21:ebb37a249b5f 287 if ( currProba < 0.5) {//by default [0][0] would be top left, we want it bottom left
Ludwigfr 21:ebb37a249b5f 288 pc.printf("0");
Ludwigfr 21:ebb37a249b5f 289 } else {
Ludwigfr 21:ebb37a249b5f 290 if(currProba==0.5)
Ludwigfr 21:ebb37a249b5f 291 pc.printf(".");
Ludwigfr 21:ebb37a249b5f 292 else
Ludwigfr 21:ebb37a249b5f 293 pc.printf("+");
Ludwigfr 21:ebb37a249b5f 294 }
Ludwigfr 21:ebb37a249b5f 295 }
Ludwigfr 21:ebb37a249b5f 296 }
Ludwigfr 21:ebb37a249b5f 297 }
Ludwigfr 21:ebb37a249b5f 298
Ludwigfr 21:ebb37a249b5f 299
Ludwigfr 21:ebb37a249b5f 300
Ludwigfr 21:ebb37a249b5f 301
Ludwigfr 21:ebb37a249b5f 302 //MATHS heavy functions
Ludwigfr 21:ebb37a249b5f 303 /**********************************************************************/
Ludwigfr 21:ebb37a249b5f 304 //Distance computation function
Ludwigfr 21:ebb37a249b5f 305 float dist(float robot_x, float robot_y, float target_x, float target_y){
Ludwigfr 21:ebb37a249b5f 306 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
Ludwigfr 21:ebb37a249b5f 307 }
Ludwigfr 21:ebb37a249b5f 308
Ludwigfr 21:ebb37a249b5f 309 //returns the probability [0,1] that the cell is occupied from the log value lt
Ludwigfr 21:ebb37a249b5f 310 float log_to_proba(float lt){
Ludwigfr 21:ebb37a249b5f 311 return 1-1/(1+exp(lt));
Ludwigfr 21:ebb37a249b5f 312 }
Ludwigfr 21:ebb37a249b5f 313
Ludwigfr 21:ebb37a249b5f 314 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 21:ebb37a249b5f 315 float proba_to_log(float p){
Ludwigfr 21:ebb37a249b5f 316 return log(p/(1-p));
Ludwigfr 21:ebb37a249b5f 317 }
Ludwigfr 21:ebb37a249b5f 318
Ludwigfr 21:ebb37a249b5f 319 //returns the new log value
Ludwigfr 21:ebb37a249b5f 320 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){
Ludwigfr 21:ebb37a249b5f 321 return previousLogValue+proba_to_log(currentProbability)-originalLogvalue;
Ludwigfr 21:ebb37a249b5f 322 }
Ludwigfr 21:ebb37a249b5f 323
Ludwigfr 21:ebb37a249b5f 324 //makes the angle inAngle between 0 and 2pi
Ludwigfr 21:ebb37a249b5f 325 float rad_angle_check(float inAngle){
Ludwigfr 21:ebb37a249b5f 326 //cout<<"before :"<<inAngle;
Ludwigfr 21:ebb37a249b5f 327 if(inAngle > 0){
Ludwigfr 21:ebb37a249b5f 328 while(inAngle > (2*pi))
Ludwigfr 21:ebb37a249b5f 329 inAngle-=2*pi;
Ludwigfr 21:ebb37a249b5f 330 }else{
Ludwigfr 21:ebb37a249b5f 331 while(inAngle < 0)
Ludwigfr 21:ebb37a249b5f 332 inAngle+=2*pi;
Ludwigfr 21:ebb37a249b5f 333 }
Ludwigfr 21:ebb37a249b5f 334 //cout<<" after :"<<inAngle<<endl;
Ludwigfr 21:ebb37a249b5f 335 return inAngle;
Ludwigfr 21:ebb37a249b5f 336 }
Ludwigfr 21:ebb37a249b5f 337
Ludwigfr 21:ebb37a249b5f 338 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 21:ebb37a249b5f 339 float compute_angle_between_vectors(float x, float y,float xs,float ys){
Ludwigfr 21:ebb37a249b5f 340 //alpha angle between ->x and ->SA
Ludwigfr 21:ebb37a249b5f 341 //vector S to A ->SA
Ludwigfr 21:ebb37a249b5f 342 float vSAx=x-xs;
Ludwigfr 21:ebb37a249b5f 343 float vSAy=y-ys;
Ludwigfr 21:ebb37a249b5f 344 //norme SA
Ludwigfr 21:ebb37a249b5f 345 float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
Ludwigfr 21:ebb37a249b5f 346 //vector ->x (1,0)
Ludwigfr 21:ebb37a249b5f 347 float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
Ludwigfr 21:ebb37a249b5f 348 //vector ->y (0,1)
Ludwigfr 21:ebb37a249b5f 349 float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
Ludwigfr 21:ebb37a249b5f 350 if (sinAlpha < 0)
Ludwigfr 21:ebb37a249b5f 351 return -acos(cosAlpha);
Ludwigfr 21:ebb37a249b5f 352 else
Ludwigfr 21:ebb37a249b5f 353 return acos(cosAlpha);
Ludwigfr 21:ebb37a249b5f 354 }
Ludwigfr 21:ebb37a249b5f 355