Aurélien Bernier Levalois / Mbed 2 deprecated Mapping

Mapping for TP2

Dependencies:   ISR_Mini-explorer mbed

Fork of GoToPointWithAngle by Georgios Tsamis

main.cpp@27:07bde633af72, 2017-04-24 (annotated)

Committer:
Ludwigfr
Date:
Mon Apr 24 11:55:55 2017 +0000
Revision:
27:07bde633af72
Parent:
26:b020cf253059
Child:
28:f884979a02fa
changed and adjusted stuff, print map with robot work, modified go to point for it to do a pi flip if in danger zone but it s not working :/

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