Mapping for TP2

Dependencies:   ISR_Mini-explorer mbed

Fork of GoToPointWithAngle by Georgios Tsamis

Committer:
Ludwigfr
Date:
Sat Apr 29 07:29:30 2017 +0000
Revision:
31:352be78e1aad
Parent:
30:95d8d3e2b81b
I guess this code is fine for putting in the report (I tweaked the variables for a 250x250cm arena and a 20x20cells map

Who changed what in which revision?

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