Georgios Tsamis / Mbed 2 deprecated OneFileToRuleThemAll

All the lab works are here!

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

main.cpp@42:ab25bffdc32b, 2017-05-22 (annotated)

Committer:
Ludwigfr
Date:
Mon May 22 13:25:20 2017 +0000
Revision:
42:ab25bffdc32b
Parent:
41:39157b310975
Child:
43:ffd5a6d4dd48
changed the code to  not rely on global variables because it's heresy

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
geotsam 34:128fc7aed957 5 using namespace std;
AurelienBernier 4:8c56c3ba6e54 6
Ludwigfr 39:dd8326ec75ce 7 void initialise_parameters();
Ludwigfr 22:ebb37a249b5f 8 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 22:ebb37a249b5f 9 void fill_initial_log_values();
Ludwigfr 22:ebb37a249b5f 10 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 22:ebb37a249b5f 11 void randomize_and_map();
geotsam 29:224e9e686f7b 12 //make the robot do a pi/2 flip
geotsam 29:224e9e686f7b 13 void do_half_flip();
Ludwigfr 22:ebb37a249b5f 14 //go the the given position while updating the map
Ludwigfr 22:ebb37a249b5f 15 void go_to_point_with_angle(float target_x, float target_y, float target_angle);
Ludwigfr 22:ebb37a249b5f 16 //Updates sonar values
geotsam 24:8f4b820d8de8 17 void update_sonar_values(float leftMm, float frontMm, float rightMm);
Ludwigfr 22:ebb37a249b5f 18 //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 19 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
Ludwigfr 22:ebb37a249b5f 20 //print the map
Ludwigfr 22:ebb37a249b5f 21 void print_final_map();
Ludwigfr 25:572c9e9a8809 22 //print the map with the robot marked on it
Ludwigfr 25:572c9e9a8809 23 void print_final_map_with_robot_position();
Ludwigfr 39:dd8326ec75ce 24 //print the map with the robot and the target marked on it
Ludwigfr 39:dd8326ec75ce 25 void print_final_map_with_robot_position_and_target();
Ludwigfr 42:ab25bffdc32b 26 //go to a given line by updating angularLeft and angularRight
Ludwigfr 42:ab25bffdc32b 27 void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c);
geotsam 33:78139f82ea74 28 //calculate virtual force field and move
Ludwigfr 42:ab25bffdc32b 29 void vff(bool* reached);
geotsam 0:8bffb51cc345 30
Ludwigfr 22:ebb37a249b5f 31 //MATHS heavy functions
Ludwigfr 22:ebb37a249b5f 32 float dist(float robot_x, float robot_y, float target_x, float target_y);
Ludwigfr 22:ebb37a249b5f 33 //returns the probability [0,1] that the cell is occupied from the log value lt
Ludwigfr 22:ebb37a249b5f 34 float log_to_proba(float lt);
Ludwigfr 22:ebb37a249b5f 35 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 22:ebb37a249b5f 36 float proba_to_log(float p);
Ludwigfr 22:ebb37a249b5f 37 //returns the new log value
Ludwigfr 22:ebb37a249b5f 38 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue );
Ludwigfr 22:ebb37a249b5f 39 //makes the angle inAngle between 0 and 2pi
Ludwigfr 22:ebb37a249b5f 40 float rad_angle_check(float inAngle);
Ludwigfr 22:ebb37a249b5f 41 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 22:ebb37a249b5f 42 float compute_angle_between_vectors(float x, float y,float xs,float ys);
Ludwigfr 39:dd8326ec75ce 43 float x_robot_in_orthonormal_x(float x, float y);
Ludwigfr 39:dd8326ec75ce 44 float y_robot_in_orthonormal_y(float x, float y);
Ludwigfr 25:572c9e9a8809 45 float robot_center_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 46 float robot_center_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 47 float robot_sonar_front_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 48 float robot_sonar_front_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 49 float robot_sonar_right_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 50 float robot_sonar_right_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 51 float robot_sonar_left_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 52 float robot_sonar_left_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 53 float estimated_width_indice_in_orthonormal_x(int i);
Ludwigfr 25:572c9e9a8809 54 float estimated_height_indice_in_orthonormal_y(int j);
Ludwigfr 42:ab25bffdc32b 55 //update angleError,distanceFromTarget,d2, beta
Ludwigfr 42:ab25bffdc32b 56 void compute_angles_and_distance(float target_x, float target_y, float target_angle,float dt,float* angleError,float* distanceFromTarget,float* d2,float* beta);
Ludwigfr 42:ab25bffdc32b 57 //update angularLeft and angularRight
Ludwigfr 42:ab25bffdc32b 58 void compute_linear_angular_velocities(float angleError,float distanceFromTarget,float beta, float* angularLeft, float* angularRight);
Ludwigfr 32:d51928b58645 59 //update foceX and forceY if necessary
Ludwigfr 32:d51928b58645 60 void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho );
Ludwigfr 32:d51928b58645 61 //compute the X and Y force
Ludwigfr 39:dd8326ec75ce 62 void compute_forceX_and_forceY(float* forceX, float* forceY);
Ludwigfr 42:ab25bffdc32b 63 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c
Ludwigfr 42:ab25bffdc32b 64 void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c);
AurelienBernier 8:109314be5b68 65
Ludwigfr 22:ebb37a249b5f 66 const float pi=3.14159;
Ludwigfr 32:d51928b58645 67
Ludwigfr 32:d51928b58645 68 //CONSTANT FORCE FIELD
geotsam 34:128fc7aed957 69 const float FORCE_CONSTANT_REPULSION=50;//TODO tweak it
Ludwigfr 32:d51928b58645 70 const float FORCE_CONSTANT_ATTRACTION=10;//TODO tweak it
Ludwigfr 32:d51928b58645 71 const float RANGE_FORCE=50;//TODO tweak it
Ludwigfr 32:d51928b58645 72
Ludwigfr 22:ebb37a249b5f 73 //spec of the sonar
Ludwigfr 22:ebb37a249b5f 74 //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 75 const float RANGE_SONAR=50;//cm
geotsam 24:8f4b820d8de8 76 const float RANGE_SONAR_MIN=10;//Rmin cm
geotsam 24:8f4b820d8de8 77 const float INCERTITUDE_SONAR=10;//cm
geotsam 24:8f4b820d8de8 78 const float ANGLE_SONAR=pi/3;//Omega rad
Ludwigfr 22:ebb37a249b5f 79
Ludwigfr 27:07bde633af72 80 //those distance and angle are approximation in need of measurements, in the orthonormal frame
geotsam 24:8f4b820d8de8 81 const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees
Ludwigfr 27:07bde633af72 82 const float DISTANCE_SONAR_LEFT_X=-4;
geotsam 24:8f4b820d8de8 83 const float DISTANCE_SONAR_LEFT_Y=4;
Ludwigfr 22:ebb37a249b5f 84
geotsam 24:8f4b820d8de8 85 const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees
Ludwigfr 27:07bde633af72 86 const float DISTANCE_SONAR_RIGHT_X=4;
geotsam 24:8f4b820d8de8 87 const float DISTANCE_SONAR_RIGHT_Y=4;
AurelienBernier 11:e641aa08c92e 88
Ludwigfr 22:ebb37a249b5f 89 const float ANGLE_FRONT_TO_FRONT=0;
Ludwigfr 22:ebb37a249b5f 90 const float DISTANCE_SONAR_FRONT_X=0;
Ludwigfr 22:ebb37a249b5f 91 const float DISTANCE_SONAR_FRONT_Y=5;
Ludwigfr 22:ebb37a249b5f 92
Ludwigfr 22:ebb37a249b5f 93 //TODO adjust the size of the map for computation time (25*25?)
Ludwigfr 32:d51928b58645 94 const float WIDTH_ARENA=120;//cm
Ludwigfr 32:d51928b58645 95 const float HEIGHT_ARENA=90;//cm
geotsam 24:8f4b820d8de8 96
geotsam 24:8f4b820d8de8 97 //const int SIZE_MAP=25;
Ludwigfr 32:d51928b58645 98 const int NB_CELL_WIDTH=24;
Ludwigfr 32:d51928b58645 99 const int NB_CELL_HEIGHT=18;
Ludwigfr 22:ebb37a249b5f 100
Ludwigfr 27:07bde633af72 101 //position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot frame
Ludwigfr 31:352be78e1aad 102 //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
Ludwigfr 35:c8f224ab153f 103 //const float DEFAULT_X=HEIGHT_ARENA/2;
Ludwigfr 35:c8f224ab153f 104 //const float DEFAULT_Y=WIDTH_ARENA/2;
Ludwigfr 40:f5e212d9f900 105 const float DEFAULT_X=20;//lower right
Ludwigfr 40:f5e212d9f900 106 const float DEFAULT_Y=20;//lower right
Ludwigfr 27:07bde633af72 107 const float DEFAULT_THETA=0;
Ludwigfr 22:ebb37a249b5f 108
geotsam 24:8f4b820d8de8 109 //used to create the map 250 represent the 250cm of the square where the robot is tested
geotsam 24:8f4b820d8de8 110 //float sizeCell=250/(float)SIZE_MAP;
Ludwigfr 27:07bde633af72 111 float sizeCellWidth=WIDTH_ARENA/(float)NB_CELL_WIDTH;
Ludwigfr 27:07bde633af72 112 float sizeCellHeight=HEIGHT_ARENA/(float)NB_CELL_HEIGHT;
geotsam 24:8f4b820d8de8 113
geotsam 24:8f4b820d8de8 114 float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell
geotsam 24:8f4b820d8de8 115 float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT];
Ludwigfr 22:ebb37a249b5f 116
Ludwigfr 22:ebb37a249b5f 117 //Diameter of a wheel and distance between the 2
Ludwigfr 22:ebb37a249b5f 118 const float RADIUS_WHEELS=3.25;
Ludwigfr 22:ebb37a249b5f 119 const float DISTANCE_WHEELS=7.2;
Ludwigfr 22:ebb37a249b5f 120
Ludwigfr 39:dd8326ec75ce 121 const int MAX_SPEED=200;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
Ludwigfr 39:dd8326ec75ce 122
Ludwigfr 39:dd8326ec75ce 123 //TODO all those global variables are making me sad
Ludwigfr 42:ab25bffdc32b 124 const float KRHO=12, KA=30, KB=-13, KV=200, KH=200; //Kappa values
geotsam 33:78139f82ea74 125
Ludwigfr 39:dd8326ec75ce 126 //those target are in comparaison to the robot (for exemple a robot in 50,50 with a taget of 0,0 would not need to move)
Ludwigfr 40:f5e212d9f900 127 const float targetX=HEIGHT_ARENA-X-20;//this is in the robot frame top left
Ludwigfr 40:f5e212d9f900 128 const float targetY=WIDTH_ARENA+Y-20;//this is in the robot frame top left
Ludwigfr 39:dd8326ec75ce 129 float targetX_orhto=0;
Ludwigfr 39:dd8326ec75ce 130 float targetY_orhto=0;
geotsam 33:78139f82ea74 131
geotsam 0:8bffb51cc345 132 int main(){
Ludwigfr 40:f5e212d9f900 133 initialise_parameters();
geotsam 17:caf393b63e27 134
Ludwigfr 42:ab25bffdc32b 135 //try to reach the target
Ludwigfr 42:ab25bffdc32b 136 bool reached=false;
Ludwigfr 39:dd8326ec75ce 137 while (!reached) {
Ludwigfr 42:ab25bffdc32b 138 vff(&reached);
Ludwigfr 39:dd8326ec75ce 139 print_final_map_with_robot_position_and_target();
Ludwigfr 39:dd8326ec75ce 140 }
Ludwigfr 39:dd8326ec75ce 141 //Stop at the end
Ludwigfr 39:dd8326ec75ce 142 leftMotor(1,0);
Ludwigfr 39:dd8326ec75ce 143 rightMotor(1,0);
Ludwigfr 40:f5e212d9f900 144
Ludwigfr 40:f5e212d9f900 145 //print the map forever
Ludwigfr 39:dd8326ec75ce 146 while(1){
Ludwigfr 39:dd8326ec75ce 147 print_final_map_with_robot_position_and_target();
Ludwigfr 39:dd8326ec75ce 148 }
Ludwigfr 39:dd8326ec75ce 149
Ludwigfr 39:dd8326ec75ce 150 }
Ludwigfr 39:dd8326ec75ce 151
Ludwigfr 39:dd8326ec75ce 152 void initialise_parameters(){
geotsam 13:41f75c132135 153 i2c1.frequency(100000);
AurelienBernier 2:ea61e801e81f 154 initRobot(); //Initializing the robot
geotsam 0:8bffb51cc345 155 pc.baud(9600); // baud for the pc communication
geotsam 0:8bffb51cc345 156
Ludwigfr 22:ebb37a249b5f 157 measure_always_on();//TODO check if needed
geotsam 24:8f4b820d8de8 158 wait(0.5);
Ludwigfr 39:dd8326ec75ce 159 //fill the map with the initial log values
Ludwigfr 22:ebb37a249b5f 160 fill_initial_log_values();
geotsam 13:41f75c132135 161
Ludwigfr 31:352be78e1aad 162 theta=DEFAULT_THETA;
Ludwigfr 35:c8f224ab153f 163 X=DEFAULT_X;
Ludwigfr 35:c8f224ab153f 164 Y=DEFAULT_Y;
Ludwigfr 39:dd8326ec75ce 165 targetX_orhto=x_robot_in_orthonormal_x(targetX,targetY);
Ludwigfr 39:dd8326ec75ce 166 targetY_orhto=y_robot_in_orthonormal_y(targetX,targetY);
AurelienBernier 8:109314be5b68 167 }
AurelienBernier 8:109314be5b68 168
Ludwigfr 22:ebb37a249b5f 169 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 22:ebb37a249b5f 170 void fill_initial_log_values(){
Ludwigfr 31:352be78e1aad 171 //Fill map, we know the border are occupied
geotsam 24:8f4b820d8de8 172 for (int i = 0; i<NB_CELL_WIDTH; i++) {
geotsam 24:8f4b820d8de8 173 for (int j = 0; j<NB_CELL_HEIGHT; j++) {
geotsam 24:8f4b820d8de8 174 if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1)
Ludwigfr 22:ebb37a249b5f 175 initialLogValues[i][j] = proba_to_log(1);
Ludwigfr 22:ebb37a249b5f 176 else
Ludwigfr 22:ebb37a249b5f 177 initialLogValues[i][j] = proba_to_log(0.5);
AurelienBernier 21:62154d644531 178 }
Ludwigfr 22:ebb37a249b5f 179 }
AurelienBernier 8:109314be5b68 180 }
AurelienBernier 8:109314be5b68 181
Ludwigfr 22:ebb37a249b5f 182 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 22:ebb37a249b5f 183 void randomize_and_map() {
geotsam 24:8f4b820d8de8 184 //TODO check that it's aurelien's work
Ludwigfr 27:07bde633af72 185 float target_x = (rand()%(int)(HEIGHT_ARENA*10))/10;//for decimal precision
Ludwigfr 27:07bde633af72 186 float target_y = (rand()%(int)(WIDTH_ARENA*10))/10;
geotsam 30:95d8d3e2b81b 187 float target_angle = 2*((float)(rand()%31416)-15708)/10000.0;
geotsam 24:8f4b820d8de8 188
geotsam 24:8f4b820d8de8 189 //TODO comment that
geotsam 24:8f4b820d8de8 190 //pc.printf("\n\r targ_X=%f", target_x);
geotsam 24:8f4b820d8de8 191 //pc.printf("\n\r targ_Y=%f", target_y);
geotsam 24:8f4b820d8de8 192 //pc.printf("\n\r targ_Angle=%f", target_angle);
geotsam 24:8f4b820d8de8 193
Ludwigfr 22:ebb37a249b5f 194 go_to_point_with_angle(target_x, target_y, target_angle);
AurelienBernier 19:dbc5fbad4975 195 }
AurelienBernier 19:dbc5fbad4975 196
geotsam 29:224e9e686f7b 197
geotsam 29:224e9e686f7b 198 void do_half_flip(){
Ludwigfr 28:f884979a02fa 199 Odometria();
geotsam 29:224e9e686f7b 200 float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
geotsam 29:224e9e686f7b 201 if(theta_plus_h_pi > pi)
geotsam 29:224e9e686f7b 202 theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
geotsam 29:224e9e686f7b 203 leftMotor(0,100);
geotsam 29:224e9e686f7b 204 rightMotor(1,100);
geotsam 29:224e9e686f7b 205 while(abs(theta_plus_h_pi-theta)>0.05){
Ludwigfr 28:f884979a02fa 206 Odometria();
geotsam 29:224e9e686f7b 207 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
Ludwigfr 28:f884979a02fa 208 }
Ludwigfr 28:f884979a02fa 209 leftMotor(1,0);
Ludwigfr 28:f884979a02fa 210 rightMotor(1,0);
Ludwigfr 28:f884979a02fa 211 }
Ludwigfr 28:f884979a02fa 212
Ludwigfr 22:ebb37a249b5f 213 //go the the given position while updating the map
Ludwigfr 22:ebb37a249b5f 214 //TODO clean this procedure it's ugly as hell and too long
Ludwigfr 22:ebb37a249b5f 215 void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
Ludwigfr 28:f884979a02fa 216 Odometria();
Ludwigfr 42:ab25bffdc32b 217 float angleError = atan2((target_y-Y),(target_x-X))-theta;
Ludwigfr 42:ab25bffdc32b 218 angleError = atan(sin(angleError)/cos(angleError));
Ludwigfr 42:ab25bffdc32b 219 float distanceFromTarget = dist(X, Y, target_x, target_y);
Ludwigfr 42:ab25bffdc32b 220 float beta = -angleError-theta+target_angle;
geotsam 24:8f4b820d8de8 221 //beta = atan(sin(beta)/cos(beta));
Ludwigfr 27:07bde633af72 222 bool keep_going=true;
Ludwigfr 40:f5e212d9f900 223 float leftMm;
Ludwigfr 40:f5e212d9f900 224 float frontMm;
Ludwigfr 40:f5e212d9f900 225 float rightMm;
Ludwigfr 42:ab25bffdc32b 226 float angularLeft=0;
Ludwigfr 42:ab25bffdc32b 227 float angularRight=0;
Ludwigfr 42:ab25bffdc32b 228 Timer t;
Ludwigfr 42:ab25bffdc32b 229 float dt=0.5;//TODO better name please
Ludwigfr 42:ab25bffdc32b 230 float d2;//TODO better name please
geotsam 24:8f4b820d8de8 231 do {
AurelienBernier 6:afde4b08166b 232 //Timer stuff
AurelienBernier 6:afde4b08166b 233 dt = t.read();
AurelienBernier 6:afde4b08166b 234 t.reset();
AurelienBernier 6:afde4b08166b 235 t.start();
AurelienBernier 6:afde4b08166b 236
geotsam 14:d58f2bdbf42e 237 //Updating X,Y and theta with the odometry values
geotsam 14:d58f2bdbf42e 238 Odometria();
geotsam 24:8f4b820d8de8 239 leftMm = get_distance_left_sensor();
geotsam 24:8f4b820d8de8 240 frontMm = get_distance_front_sensor();
geotsam 24:8f4b820d8de8 241 rightMm = get_distance_right_sensor();
geotsam 24:8f4b820d8de8 242
geotsam 24:8f4b820d8de8 243 //pc.printf("\n\r leftMm=%f", leftMm);
geotsam 24:8f4b820d8de8 244 //pc.printf("\n\r frontMm=%f", frontMm);
geotsam 24:8f4b820d8de8 245 //pc.printf("\n\r rightMm=%f", rightMm);
Ludwigfr 27:07bde633af72 246
Ludwigfr 27:07bde633af72 247 //if in dangerzone
geotsam 29:224e9e686f7b 248 if(frontMm < 120 || leftMm <120 || rightMm <120){
geotsam 24:8f4b820d8de8 249 leftMotor(1,0);
geotsam 24:8f4b820d8de8 250 rightMotor(1,0);
Ludwigfr 27:07bde633af72 251 update_sonar_values(leftMm, frontMm, rightMm);
geotsam 29:224e9e686f7b 252 //TODO Giorgos maybe you can also test the do_half_flip() function
geotsam 24:8f4b820d8de8 253 Odometria();
Ludwigfr 27:07bde633af72 254 //do a flip TODO
Ludwigfr 27:07bde633af72 255 keep_going=false;
geotsam 29:224e9e686f7b 256 do_half_flip();
Ludwigfr 27:07bde633af72 257 }else{
Ludwigfr 27:07bde633af72 258 //if not in danger zone continue as usual
Ludwigfr 27:07bde633af72 259 update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 42:ab25bffdc32b 260 compute_angles_and_distance(target_x, target_y, target_angle,dt,&angleError,&distanceFromTarget,&d2,&beta);//Compute the angles and the distance from target
Ludwigfr 42:ab25bffdc32b 261 compute_linear_angular_velocities(angleError,distanceFromTarget,beta,&angularLeft,&angularRight); //Using the angles and distance, compute the velocities needed (linear & angular)
Ludwigfr 42:ab25bffdc32b 262
Ludwigfr 42:ab25bffdc32b 263 //Normalize speed for motors
Ludwigfr 42:ab25bffdc32b 264 if(angularLeft>angularRight) {
Ludwigfr 42:ab25bffdc32b 265 angularRight=MAX_SPEED*angularRight/angularLeft;
Ludwigfr 42:ab25bffdc32b 266 angularLeft=MAX_SPEED;
Ludwigfr 42:ab25bffdc32b 267 } else {
Ludwigfr 42:ab25bffdc32b 268 angularLeft=MAX_SPEED*angularLeft/angularRight;
Ludwigfr 42:ab25bffdc32b 269 angularRight=MAX_SPEED;
Ludwigfr 42:ab25bffdc32b 270 }
Ludwigfr 27:07bde633af72 271 //pc.printf("\n\r X=%f", X);
Ludwigfr 27:07bde633af72 272 //pc.printf("\n\r Y=%f", Y);
Ludwigfr 27:07bde633af72 273
Ludwigfr 42:ab25bffdc32b 274 //pc.printf("\n\r a_r=%f", angularRight);
Ludwigfr 42:ab25bffdc32b 275 //pc.printf("\n\r a_l=%f", angularLeft);
Ludwigfr 27:07bde633af72 276
Ludwigfr 27:07bde633af72 277 //Updating motor velocities
Ludwigfr 42:ab25bffdc32b 278 leftMotor(1,angularLeft);
Ludwigfr 42:ab25bffdc32b 279 rightMotor(1,angularRight);
Ludwigfr 27:07bde633af72 280
Ludwigfr 27:07bde633af72 281 wait(0.2);
Ludwigfr 27:07bde633af72 282 //Timer stuff
Ludwigfr 27:07bde633af72 283 t.stop();
AurelienBernier 11:e641aa08c92e 284 }
Ludwigfr 27:07bde633af72 285 } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going);
geotsam 24:8f4b820d8de8 286
geotsam 24:8f4b820d8de8 287 //Stop at the end
geotsam 24:8f4b820d8de8 288 leftMotor(1,0);
geotsam 24:8f4b820d8de8 289 rightMotor(1,0);
Ludwigfr 22:ebb37a249b5f 290 }
Ludwigfr 22:ebb37a249b5f 291
Ludwigfr 22:ebb37a249b5f 292 //Updates sonar values
geotsam 24:8f4b820d8de8 293 void update_sonar_values(float leftMm, float frontMm, float rightMm){
Ludwigfr 22:ebb37a249b5f 294 float currProba;
Ludwigfr 25:572c9e9a8809 295 float i_in_orthonormal;
Ludwigfr 25:572c9e9a8809 296 float j_in_orthonormal;
geotsam 24:8f4b820d8de8 297 for(int i=0;i<NB_CELL_WIDTH;i++){
geotsam 24:8f4b820d8de8 298 for(int j=0;j<NB_CELL_HEIGHT;j++){
geotsam 24:8f4b820d8de8 299 //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 300 //check that again
Ludwigfr 22:ebb37a249b5f 301 //compute for front sonar
Ludwigfr 25:572c9e9a8809 302 i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i);
Ludwigfr 25:572c9e9a8809 303 j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j);
Ludwigfr 25:572c9e9a8809 304
Ludwigfr 25:572c9e9a8809 305 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 306 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 307 //compute for right sonar
Ludwigfr 25:572c9e9a8809 308 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 309 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
Ludwigfr 22:ebb37a249b5f 310 //compute for left sonar
Ludwigfr 25:572c9e9a8809 311 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 312 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
Ludwigfr 22:ebb37a249b5f 313 }
Ludwigfr 22:ebb37a249b5f 314 }
Ludwigfr 22:ebb37a249b5f 315 }
Ludwigfr 22:ebb37a249b5f 316
Ludwigfr 25:572c9e9a8809 317 //ODOMETRIA MUST HAVE BEEN CALLED
Ludwigfr 22:ebb37a249b5f 318 //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 319 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){
Ludwigfr 22:ebb37a249b5f 320
Ludwigfr 42:ab25bffdc32b 321 float anglePointToSonar=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
Ludwigfr 42:ab25bffdc32b 322 float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition;
Ludwigfr 42:ab25bffdc32b 323 anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
Ludwigfr 22:ebb37a249b5f 324 float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
Ludwigfr 22:ebb37a249b5f 325
Ludwigfr 22:ebb37a249b5f 326 //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
Ludwigfr 22:ebb37a249b5f 327 //check if absolute difference between the angles is no more than Omega/2
Ludwigfr 42:ab25bffdc32b 328 if( distancePointToSonar < (RANGE_SONAR)&& (anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= rad_angle_check(-ANGLE_SONAR/2))){
Ludwigfr 22:ebb37a249b5f 329 if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
Ludwigfr 22:ebb37a249b5f 330 //point before obstacle, probably empty
Ludwigfr 22:ebb37a249b5f 331 /*****************************************************************************/
Ludwigfr 22:ebb37a249b5f 332 float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 22:ebb37a249b5f 333 float Er;
Ludwigfr 22:ebb37a249b5f 334 if(distancePointToSonar < RANGE_SONAR_MIN){
Ludwigfr 22:ebb37a249b5f 335 //point before minimum sonar range
Ludwigfr 22:ebb37a249b5f 336 Er=0.f;
Ludwigfr 22:ebb37a249b5f 337 }else{
Ludwigfr 22:ebb37a249b5f 338 //point after minimum sonar range
Ludwigfr 22:ebb37a249b5f 339 Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
Ludwigfr 22:ebb37a249b5f 340 }
Ludwigfr 22:ebb37a249b5f 341 /*****************************************************************************/
Ludwigfr 22:ebb37a249b5f 342 return (1.f-Er*Ea)/2.f;
Ludwigfr 22:ebb37a249b5f 343 }else{
Ludwigfr 22:ebb37a249b5f 344 //probably occupied
Ludwigfr 22:ebb37a249b5f 345 /*****************************************************************************/
Ludwigfr 22:ebb37a249b5f 346 float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 22:ebb37a249b5f 347 float Or;
Ludwigfr 22:ebb37a249b5f 348 if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
Ludwigfr 22:ebb37a249b5f 349 //point between distanceObstacleDetected +- INCERTITUDE_SONAR
Ludwigfr 22:ebb37a249b5f 350 Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
Ludwigfr 22:ebb37a249b5f 351 }else{
Ludwigfr 22:ebb37a249b5f 352 //point after in range of the sonar but after the zone detected
Ludwigfr 22:ebb37a249b5f 353 Or=0;
Ludwigfr 22:ebb37a249b5f 354 }
Ludwigfr 22:ebb37a249b5f 355 /*****************************************************************************/
Ludwigfr 22:ebb37a249b5f 356 return (1+Or*Oa)/2;
Ludwigfr 22:ebb37a249b5f 357 }
Ludwigfr 22:ebb37a249b5f 358 }else{
Ludwigfr 25:572c9e9a8809 359 //not checked by the sonar
Ludwigfr 22:ebb37a249b5f 360 return 0.5;
AurelienBernier 19:dbc5fbad4975 361 }
Ludwigfr 22:ebb37a249b5f 362 }
Ludwigfr 22:ebb37a249b5f 363
Ludwigfr 25:572c9e9a8809 364 void print_final_map() {
Ludwigfr 22:ebb37a249b5f 365 float currProba;
geotsam 24:8f4b820d8de8 366 pc.printf("\n\r");
geotsam 24:8f4b820d8de8 367 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
geotsam 24:8f4b820d8de8 368 for (int x= 0; x<NB_CELL_WIDTH; x++) {
geotsam 24:8f4b820d8de8 369 currProba=log_to_proba(map[x][y]);
geotsam 24:8f4b820d8de8 370 if ( currProba < 0.5) {
geotsam 29:224e9e686f7b 371 pc.printf(" ");
Ludwigfr 22:ebb37a249b5f 372 } else {
Ludwigfr 22:ebb37a249b5f 373 if(currProba==0.5)
geotsam 24:8f4b820d8de8 374 pc.printf(" . ");
Ludwigfr 22:ebb37a249b5f 375 else
geotsam 29:224e9e686f7b 376 pc.printf(" X ");
Ludwigfr 22:ebb37a249b5f 377 }
Ludwigfr 22:ebb37a249b5f 378 }
geotsam 24:8f4b820d8de8 379 pc.printf("\n\r");
Ludwigfr 22:ebb37a249b5f 380 }
Ludwigfr 22:ebb37a249b5f 381 }
Ludwigfr 22:ebb37a249b5f 382
Ludwigfr 25:572c9e9a8809 383 void print_final_map_with_robot_position() {
geotsam 24:8f4b820d8de8 384 float currProba;
Ludwigfr 25:572c9e9a8809 385 Odometria();
Ludwigfr 25:572c9e9a8809 386 float Xrobot=robot_center_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 387 float Yrobot=robot_center_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 388
Ludwigfr 25:572c9e9a8809 389 float heightIndiceInOrthonormal;
Ludwigfr 25:572c9e9a8809 390 float widthIndiceInOrthonormal;
Ludwigfr 25:572c9e9a8809 391
Ludwigfr 27:07bde633af72 392 float widthMalus=-(3*sizeCellWidth/2);
Ludwigfr 27:07bde633af72 393 float widthBonus=sizeCellWidth/2;
Ludwigfr 25:572c9e9a8809 394
Ludwigfr 27:07bde633af72 395 float heightMalus=-(3*sizeCellHeight/2);
Ludwigfr 27:07bde633af72 396 float heightBonus=sizeCellHeight/2;
Ludwigfr 25:572c9e9a8809 397
geotsam 24:8f4b820d8de8 398 pc.printf("\n\r");
geotsam 24:8f4b820d8de8 399 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
geotsam 24:8f4b820d8de8 400 for (int x= 0; x<NB_CELL_WIDTH; x++) {
Ludwigfr 25:572c9e9a8809 401 heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
Ludwigfr 25:572c9e9a8809 402 widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
Ludwigfr 27:07bde633af72 403 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 27:07bde633af72 404 pc.printf(" R ");
Ludwigfr 25:572c9e9a8809 405 else{
Ludwigfr 25:572c9e9a8809 406 currProba=log_to_proba(map[x][y]);
Ludwigfr 25:572c9e9a8809 407 if ( currProba < 0.5)
geotsam 29:224e9e686f7b 408 pc.printf(" ");
Ludwigfr 25:572c9e9a8809 409 else{
Ludwigfr 25:572c9e9a8809 410 if(currProba==0.5)
Ludwigfr 27:07bde633af72 411 pc.printf(" . ");
Ludwigfr 25:572c9e9a8809 412 else
geotsam 29:224e9e686f7b 413 pc.printf(" X ");
Ludwigfr 25:572c9e9a8809 414 }
geotsam 24:8f4b820d8de8 415 }
geotsam 24:8f4b820d8de8 416 }
geotsam 24:8f4b820d8de8 417 pc.printf("\n\r");
geotsam 24:8f4b820d8de8 418 }
geotsam 24:8f4b820d8de8 419 }
Ludwigfr 22:ebb37a249b5f 420
Ludwigfr 39:dd8326ec75ce 421 void print_final_map_with_robot_position_and_target() {
Ludwigfr 39:dd8326ec75ce 422 float currProba;
Ludwigfr 39:dd8326ec75ce 423 Odometria();
Ludwigfr 39:dd8326ec75ce 424 float Xrobot=robot_center_x_in_orthonormal_x();
Ludwigfr 39:dd8326ec75ce 425 float Yrobot=robot_center_y_in_orthonormal_y();
Ludwigfr 39:dd8326ec75ce 426
Ludwigfr 39:dd8326ec75ce 427 float heightIndiceInOrthonormal;
Ludwigfr 39:dd8326ec75ce 428 float widthIndiceInOrthonormal;
Ludwigfr 39:dd8326ec75ce 429
Ludwigfr 39:dd8326ec75ce 430 float widthMalus=-(3*sizeCellWidth/2);
Ludwigfr 39:dd8326ec75ce 431 float widthBonus=sizeCellWidth/2;
Ludwigfr 39:dd8326ec75ce 432
Ludwigfr 39:dd8326ec75ce 433 float heightMalus=-(3*sizeCellHeight/2);
Ludwigfr 39:dd8326ec75ce 434 float heightBonus=sizeCellHeight/2;
Ludwigfr 39:dd8326ec75ce 435
Ludwigfr 39:dd8326ec75ce 436 pc.printf("\n\r");
Ludwigfr 39:dd8326ec75ce 437 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
Ludwigfr 39:dd8326ec75ce 438 for (int x= 0; x<NB_CELL_WIDTH; x++) {
Ludwigfr 39:dd8326ec75ce 439 heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
Ludwigfr 39:dd8326ec75ce 440 widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
Ludwigfr 39:dd8326ec75ce 441 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 39:dd8326ec75ce 442 pc.printf(" R ");
Ludwigfr 39:dd8326ec75ce 443 else{
Ludwigfr 39:dd8326ec75ce 444 if(targetY_orhto >= (heightIndiceInOrthonormal+heightMalus) && targetY_orhto <= (heightIndiceInOrthonormal+heightBonus) && targetX_orhto >= (widthIndiceInOrthonormal+widthMalus) && targetX_orhto <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 39:dd8326ec75ce 445 pc.printf(" T ");
Ludwigfr 39:dd8326ec75ce 446 else{
Ludwigfr 39:dd8326ec75ce 447 currProba=log_to_proba(map[x][y]);
Ludwigfr 39:dd8326ec75ce 448 if ( currProba < 0.5)
Ludwigfr 39:dd8326ec75ce 449 pc.printf(" ");
Ludwigfr 39:dd8326ec75ce 450 else{
Ludwigfr 39:dd8326ec75ce 451 if(currProba==0.5)
Ludwigfr 39:dd8326ec75ce 452 pc.printf(" . ");
Ludwigfr 39:dd8326ec75ce 453 else
Ludwigfr 39:dd8326ec75ce 454 pc.printf(" X ");
Ludwigfr 39:dd8326ec75ce 455 }
Ludwigfr 39:dd8326ec75ce 456 }
Ludwigfr 39:dd8326ec75ce 457 }
Ludwigfr 39:dd8326ec75ce 458 }
Ludwigfr 39:dd8326ec75ce 459 pc.printf("\n\r");
Ludwigfr 39:dd8326ec75ce 460 }
Ludwigfr 39:dd8326ec75ce 461 }
Ludwigfr 39:dd8326ec75ce 462
Ludwigfr 22:ebb37a249b5f 463 //MATHS heavy functions
Ludwigfr 22:ebb37a249b5f 464 /**********************************************************************/
Ludwigfr 22:ebb37a249b5f 465 //Distance computation function
Ludwigfr 22:ebb37a249b5f 466 float dist(float robot_x, float robot_y, float target_x, float target_y){
Ludwigfr 22:ebb37a249b5f 467 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
Ludwigfr 22:ebb37a249b5f 468 }
Ludwigfr 22:ebb37a249b5f 469
geotsam 24:8f4b820d8de8 470 //returns the probability [0,1] that the cell is occupied from the log valAue lt
Ludwigfr 22:ebb37a249b5f 471 float log_to_proba(float lt){
Ludwigfr 22:ebb37a249b5f 472 return 1-1/(1+exp(lt));
Ludwigfr 22:ebb37a249b5f 473 }
Ludwigfr 22:ebb37a249b5f 474
Ludwigfr 22:ebb37a249b5f 475 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 22:ebb37a249b5f 476 float proba_to_log(float p){
Ludwigfr 22:ebb37a249b5f 477 return log(p/(1-p));
Ludwigfr 22:ebb37a249b5f 478 }
Ludwigfr 22:ebb37a249b5f 479
Ludwigfr 22:ebb37a249b5f 480 //returns the new log value
Ludwigfr 22:ebb37a249b5f 481 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){
Ludwigfr 22:ebb37a249b5f 482 return previousLogValue+proba_to_log(currentProbability)-originalLogvalue;
Ludwigfr 22:ebb37a249b5f 483 }
Ludwigfr 22:ebb37a249b5f 484
Ludwigfr 22:ebb37a249b5f 485 //makes the angle inAngle between 0 and 2pi
Ludwigfr 22:ebb37a249b5f 486 float rad_angle_check(float inAngle){
Ludwigfr 22:ebb37a249b5f 487 //cout<<"before :"<<inAngle;
Ludwigfr 22:ebb37a249b5f 488 if(inAngle > 0){
Ludwigfr 22:ebb37a249b5f 489 while(inAngle > (2*pi))
Ludwigfr 22:ebb37a249b5f 490 inAngle-=2*pi;
Ludwigfr 22:ebb37a249b5f 491 }else{
Ludwigfr 22:ebb37a249b5f 492 while(inAngle < 0)
Ludwigfr 22:ebb37a249b5f 493 inAngle+=2*pi;
Ludwigfr 22:ebb37a249b5f 494 }
Ludwigfr 22:ebb37a249b5f 495 //cout<<" after :"<<inAngle<<endl;
Ludwigfr 22:ebb37a249b5f 496 return inAngle;
Ludwigfr 22:ebb37a249b5f 497 }
Ludwigfr 22:ebb37a249b5f 498
Ludwigfr 22:ebb37a249b5f 499 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 22:ebb37a249b5f 500 float compute_angle_between_vectors(float x, float y,float xs,float ys){
Ludwigfr 22:ebb37a249b5f 501 //alpha angle between ->x and ->SA
Ludwigfr 22:ebb37a249b5f 502 //vector S to A ->SA
Ludwigfr 22:ebb37a249b5f 503 float vSAx=x-xs;
Ludwigfr 22:ebb37a249b5f 504 float vSAy=y-ys;
Ludwigfr 22:ebb37a249b5f 505 //norme SA
Ludwigfr 22:ebb37a249b5f 506 float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
Ludwigfr 22:ebb37a249b5f 507 //vector ->x (1,0)
Ludwigfr 22:ebb37a249b5f 508 float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
Ludwigfr 22:ebb37a249b5f 509 //vector ->y (0,1)
Ludwigfr 22:ebb37a249b5f 510 float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
Ludwigfr 22:ebb37a249b5f 511 if (sinAlpha < 0)
Ludwigfr 22:ebb37a249b5f 512 return -acos(cosAlpha);
Ludwigfr 22:ebb37a249b5f 513 else
Ludwigfr 22:ebb37a249b5f 514 return acos(cosAlpha);
Ludwigfr 25:572c9e9a8809 515 }
Ludwigfr 38:3c9f8cbf5250 516 /*
Ludwigfr 25:572c9e9a8809 517
Ludwigfr 38:3c9f8cbf5250 518
Ludwigfr 38:3c9f8cbf5250 519 Robot frame: orthonormal frame:
Ludwigfr 38:3c9f8cbf5250 520 ^ ^
Ludwigfr 38:3c9f8cbf5250 521 |x |y
Ludwigfr 38:3c9f8cbf5250 522 <- R O ->
Ludwigfr 38:3c9f8cbf5250 523 y x
Ludwigfr 38:3c9f8cbf5250 524 */
Ludwigfr 38:3c9f8cbf5250 525 //Odometria must bu up to date
Ludwigfr 36:c806c568720a 526 float x_robot_in_orthonormal_x(float x, float y){
Ludwigfr 38:3c9f8cbf5250 527 return robot_center_x_in_orthonormal_x()-y;
Ludwigfr 36:c806c568720a 528 }
Ludwigfr 36:c806c568720a 529
Ludwigfr 38:3c9f8cbf5250 530 //Odometria must bu up to date
Ludwigfr 36:c806c568720a 531 float y_robot_in_orthonormal_y(float x, float y){
Ludwigfr 38:3c9f8cbf5250 532 return robot_center_y_in_orthonormal_y()+x;
Ludwigfr 36:c806c568720a 533 }
Ludwigfr 36:c806c568720a 534
Ludwigfr 25:572c9e9a8809 535 float robot_center_x_in_orthonormal_x(){
Ludwigfr 27:07bde633af72 536 return NB_CELL_WIDTH*sizeCellWidth-Y;
Ludwigfr 25:572c9e9a8809 537 }
Ludwigfr 25:572c9e9a8809 538
Ludwigfr 25:572c9e9a8809 539 float robot_center_y_in_orthonormal_y(){
Ludwigfr 27:07bde633af72 540 return X;
Ludwigfr 25:572c9e9a8809 541 }
Ludwigfr 25:572c9e9a8809 542
Ludwigfr 25:572c9e9a8809 543 float robot_sonar_front_x_in_orthonormal_x(){
Ludwigfr 27:07bde633af72 544 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_FRONT_X;
Ludwigfr 25:572c9e9a8809 545 }
Ludwigfr 25:572c9e9a8809 546 float robot_sonar_front_y_in_orthonormal_y(){
Ludwigfr 27:07bde633af72 547 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_FRONT_Y;
Ludwigfr 25:572c9e9a8809 548 }
Ludwigfr 25:572c9e9a8809 549
Ludwigfr 25:572c9e9a8809 550 float robot_sonar_right_x_in_orthonormal_x(){
Ludwigfr 27:07bde633af72 551 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_RIGHT_X;
Ludwigfr 25:572c9e9a8809 552 }
Ludwigfr 25:572c9e9a8809 553 float robot_sonar_right_y_in_orthonormal_y(){
Ludwigfr 27:07bde633af72 554 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_RIGHT_Y;
Ludwigfr 25:572c9e9a8809 555 }
Ludwigfr 25:572c9e9a8809 556
Ludwigfr 25:572c9e9a8809 557 float robot_sonar_left_x_in_orthonormal_x(){
Ludwigfr 27:07bde633af72 558 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_LEFT_X;
Ludwigfr 25:572c9e9a8809 559 }
Ludwigfr 25:572c9e9a8809 560 float robot_sonar_left_y_in_orthonormal_y(){
Ludwigfr 27:07bde633af72 561 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_LEFT_Y;
Ludwigfr 25:572c9e9a8809 562 }
Ludwigfr 25:572c9e9a8809 563
Ludwigfr 25:572c9e9a8809 564 float estimated_width_indice_in_orthonormal_x(int i){
Ludwigfr 27:07bde633af72 565 return sizeCellWidth/2+i*sizeCellWidth;
Ludwigfr 25:572c9e9a8809 566 }
Ludwigfr 25:572c9e9a8809 567 float estimated_height_indice_in_orthonormal_y(int j){
Ludwigfr 27:07bde633af72 568 return sizeCellHeight/2+j*sizeCellHeight;
geotsam 26:b020cf253059 569 }
geotsam 26:b020cf253059 570
Ludwigfr 42:ab25bffdc32b 571 //update angleError,distanceFromTarget,d2, beta
Ludwigfr 42:ab25bffdc32b 572 void compute_angles_and_distance(float target_x, float target_y, float target_angle,float dt,float* angleError,float* distanceFromTarget,float* d2,float* beta){
Ludwigfr 42:ab25bffdc32b 573 *angleError = atan2((target_y-Y),(target_x-X))-theta;
Ludwigfr 42:ab25bffdc32b 574 *angleError = atan(sin(*angleError)/cos(*angleError));
Ludwigfr 42:ab25bffdc32b 575 *distanceFromTarget = dist(X, Y, target_x, target_y);
Ludwigfr 42:ab25bffdc32b 576 *d2 = *distanceFromTarget;
Ludwigfr 42:ab25bffdc32b 577 *beta = -*angleError-theta+target_angle;
geotsam 26:b020cf253059 578
geotsam 26:b020cf253059 579 //Computing angle error and distance towards the target value
Ludwigfr 42:ab25bffdc32b 580 *distanceFromTarget += dt*(-KRHO*cos(*angleError)**distanceFromTarget);
Ludwigfr 42:ab25bffdc32b 581 float temp = *angleError;
Ludwigfr 42:ab25bffdc32b 582 *angleError += dt*(KRHO*sin(*angleError)-KA**angleError-KB**beta);
Ludwigfr 42:ab25bffdc32b 583 *beta += dt*(-KRHO*sin(temp));
Ludwigfr 27:07bde633af72 584 //pc.printf("\n\r d2=%f", d2);
Ludwigfr 27:07bde633af72 585 //pc.printf("\n\r dt=%f", dt);
geotsam 26:b020cf253059 586 }
geotsam 26:b020cf253059 587
Ludwigfr 42:ab25bffdc32b 588 //update angularLeft and angularRight
Ludwigfr 42:ab25bffdc32b 589 void compute_linear_angular_velocities(float angleError,float distanceFromTarget,float beta,float* angularLeft, float* angularRight){
geotsam 26:b020cf253059 590 //Computing linear and angular velocities
Ludwigfr 42:ab25bffdc32b 591 float linear;
Ludwigfr 42:ab25bffdc32b 592 float angular;
Ludwigfr 42:ab25bffdc32b 593 if(angleError>=-1.5708 && angleError<=1.5708){
Ludwigfr 42:ab25bffdc32b 594 linear=KRHO*distanceFromTarget;
Ludwigfr 42:ab25bffdc32b 595 angular=KA*angleError+KB*beta;
geotsam 26:b020cf253059 596 }
geotsam 26:b020cf253059 597 else{
Ludwigfr 42:ab25bffdc32b 598 linear=-KRHO*distanceFromTarget;
Ludwigfr 42:ab25bffdc32b 599 angular=-KA*angleError-KB*beta;
geotsam 26:b020cf253059 600 }
Ludwigfr 42:ab25bffdc32b 601 *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
Ludwigfr 42:ab25bffdc32b 602 *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
Ludwigfr 32:d51928b58645 603 }
Ludwigfr 32:d51928b58645 604
Ludwigfr 32:d51928b58645 605 void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){
Ludwigfr 32:d51928b58645 606 //get the coordonate of the map and the robot in the ortonormal frame
Ludwigfr 32:d51928b58645 607 float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice);
Ludwigfr 32:d51928b58645 608 float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice);
Ludwigfr 32:d51928b58645 609 //compute the distance beetween the cell and the robot
Ludwigfr 32:d51928b58645 610 float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2));
Ludwigfr 32:d51928b58645 611 //check if the cell is in range
Ludwigfr 32:d51928b58645 612 if(distanceCellToRobot <= (range)) {
Ludwigfr 32:d51928b58645 613 float probaCell=log_to_proba(map[widthIndice][heightIndice]);
Ludwigfr 32:d51928b58645 614 float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3);
Ludwigfr 32:d51928b58645 615 float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3);
Ludwigfr 32:d51928b58645 616 *forceX+=xForceComputed;
Ludwigfr 32:d51928b58645 617 *forceY+=yForceComputed;
Ludwigfr 32:d51928b58645 618 }
Ludwigfr 32:d51928b58645 619 }
Ludwigfr 32:d51928b58645 620
Ludwigfr 38:3c9f8cbf5250 621 //compute the force on X and Y
Ludwigfr 39:dd8326ec75ce 622 void compute_forceX_and_forceY(float* forceX, float* forceY){
Ludwigfr 38:3c9f8cbf5250 623 //we put the position of the robot in an orthonormal frame
Ludwigfr 32:d51928b58645 624 float xRobotOrtho=robot_center_x_in_orthonormal_x();
Ludwigfr 32:d51928b58645 625 float yRobotOrtho=robot_center_y_in_orthonormal_y();
Ludwigfr 39:dd8326ec75ce 626
geotsam 37:462d19bb221f 627 float forceRepulsionComputedX=0;
geotsam 37:462d19bb221f 628 float forceRepulsionComputedY=0;
Ludwigfr 38:3c9f8cbf5250 629 //for each cell of the map we compute a force of repulsion
Ludwigfr 32:d51928b58645 630 for(int i=0;i<NB_CELL_WIDTH;i++){
Ludwigfr 32:d51928b58645 631 for(int j=0;j<NB_CELL_HEIGHT;j++){
geotsam 37:462d19bb221f 632 updateForce(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho);
Ludwigfr 32:d51928b58645 633 }
Ludwigfr 32:d51928b58645 634 }
geotsam 34:128fc7aed957 635 //update with attraction force
geotsam 37:462d19bb221f 636 *forceX=-forceRepulsionComputedX;
geotsam 37:462d19bb221f 637 *forceY=-forceRepulsionComputedY;
Ludwigfr 40:f5e212d9f900 638 float distanceTargetRobot=sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2));
Ludwigfr 40:f5e212d9f900 639 if(distanceTargetRobot != 0){
Ludwigfr 40:f5e212d9f900 640 *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX_orhto-xRobotOrtho)/distanceTargetRobot;
Ludwigfr 40:f5e212d9f900 641 *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY_orhto-yRobotOrtho)/distanceTargetRobot;
Ludwigfr 40:f5e212d9f900 642 }
geotsam 34:128fc7aed957 643 float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
Ludwigfr 40:f5e212d9f900 644 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
Ludwigfr 36:c806c568720a 645 *forceX=*forceX/amplitude;
Ludwigfr 36:c806c568720a 646 *forceY=*forceY/amplitude;
Ludwigfr 36:c806c568720a 647 }
geotsam 33:78139f82ea74 648 }
geotsam 33:78139f82ea74 649
Ludwigfr 42:ab25bffdc32b 650 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c
Ludwigfr 42:ab25bffdc32b 651 void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c){
Ludwigfr 42:ab25bffdc32b 652 *line_a=forceY;
Ludwigfr 42:ab25bffdc32b 653 *line_b=forceX;
Ludwigfr 40:f5e212d9f900 654 //TODO check in what referentiel it needs to go
Ludwigfr 40:f5e212d9f900 655 float xRobotOrtho=robot_center_x_in_orthonormal_x();
Ludwigfr 40:f5e212d9f900 656 float yRobotOrtho=robot_center_y_in_orthonormal_y();
Ludwigfr 42:ab25bffdc32b 657 *line_c=forceX*yRobotOrtho-forceY*xRobotOrtho;
geotsam 33:78139f82ea74 658 }
geotsam 33:78139f82ea74 659
Ludwigfr 42:ab25bffdc32b 660 void vff(bool* reached){
Ludwigfr 42:ab25bffdc32b 661 float line_a;
Ludwigfr 42:ab25bffdc32b 662 float line_b;
Ludwigfr 42:ab25bffdc32b 663 float line_c;
geotsam 33:78139f82ea74 664 //Updating X,Y and theta with the odometry values
geotsam 37:462d19bb221f 665 float forceX=0;
geotsam 37:462d19bb221f 666 float forceY=0;
Ludwigfr 38:3c9f8cbf5250 667 //we update the odometrie
geotsam 33:78139f82ea74 668 Odometria();
Ludwigfr 38:3c9f8cbf5250 669 //we check the sensors
Ludwigfr 40:f5e212d9f900 670 float leftMm = get_distance_left_sensor();
Ludwigfr 40:f5e212d9f900 671 float frontMm = get_distance_front_sensor();
Ludwigfr 40:f5e212d9f900 672 float rightMm = get_distance_right_sensor();
Ludwigfr 42:ab25bffdc32b 673 float angularRight=0;
Ludwigfr 42:ab25bffdc32b 674 float angularLeft=0;
Ludwigfr 38:3c9f8cbf5250 675 //update the probabilities values
geotsam 33:78139f82ea74 676 update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 38:3c9f8cbf5250 677 //we compute the force on X and Y
Ludwigfr 39:dd8326ec75ce 678 compute_forceX_and_forceY(&forceX, &forceY);
Ludwigfr 38:3c9f8cbf5250 679 //we compute a new ine
Ludwigfr 42:ab25bffdc32b 680 calculate_line(forceX, forceY, X, Y,&line_a,&line_b,&line_c);
Ludwigfr 42:ab25bffdc32b 681 go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c);
Ludwigfr 42:ab25bffdc32b 682 //Normalize speed for motors
Ludwigfr 42:ab25bffdc32b 683 if(angularLeft>angularRight) {
Ludwigfr 42:ab25bffdc32b 684 angularRight=MAX_SPEED*angularRight/angularLeft;
Ludwigfr 42:ab25bffdc32b 685 angularLeft=MAX_SPEED;
Ludwigfr 42:ab25bffdc32b 686 }
Ludwigfr 42:ab25bffdc32b 687 else {
Ludwigfr 42:ab25bffdc32b 688 angularLeft=MAX_SPEED*angularLeft/angularRight;
Ludwigfr 42:ab25bffdc32b 689 angularRight=MAX_SPEED;
Ludwigfr 42:ab25bffdc32b 690 }
geotsam 34:128fc7aed957 691 pc.printf("\r\n forceX=%f", forceX);
geotsam 34:128fc7aed957 692 pc.printf("\r\n forceY=%f", forceY);
geotsam 34:128fc7aed957 693 pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c);
geotsam 33:78139f82ea74 694
geotsam 33:78139f82ea74 695 //Updating motor velocities
Ludwigfr 42:ab25bffdc32b 696 leftMotor(1,angularLeft);
Ludwigfr 42:ab25bffdc32b 697 rightMotor(1,angularRight);
geotsam 34:128fc7aed957 698
Ludwigfr 42:ab25bffdc32b 699 pc.printf("\r\n angR=%f", angularRight);
Ludwigfr 42:ab25bffdc32b 700 pc.printf("\r\n angL=%f", angularLeft);
geotsam 34:128fc7aed957 701 pc.printf("\r\n dist=%f", dist(X,Y,targetX,targetY));
geotsam 33:78139f82ea74 702
Ludwigfr 40:f5e212d9f900 703 //wait(0.1);
geotsam 33:78139f82ea74 704 Odometria();
geotsam 34:128fc7aed957 705 if(dist(X,Y,targetX,targetY)<10)
Ludwigfr 42:ab25bffdc32b 706 *reached=true;
geotsam 33:78139f82ea74 707 }
geotsam 33:78139f82ea74 708
Ludwigfr 42:ab25bffdc32b 709 void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c){
Ludwigfr 42:ab25bffdc32b 710 float lineAngle;
Ludwigfr 42:ab25bffdc32b 711 float angleError;
Ludwigfr 42:ab25bffdc32b 712 float linear;
Ludwigfr 42:ab25bffdc32b 713 float angular;
geotsam 33:78139f82ea74 714 if(line_b!=0){
Ludwigfr 42:ab25bffdc32b 715 lineAngle=atan(-line_a/line_b);
geotsam 33:78139f82ea74 716 }
geotsam 33:78139f82ea74 717 else{
Ludwigfr 42:ab25bffdc32b 718 lineAngle=1.5708;
geotsam 33:78139f82ea74 719 }
geotsam 33:78139f82ea74 720
geotsam 33:78139f82ea74 721 //Computing angle error
Ludwigfr 42:ab25bffdc32b 722 angleError = lineAngle-theta;
Ludwigfr 42:ab25bffdc32b 723 angleError = atan(sin(angleError)/cos(angleError));
geotsam 33:78139f82ea74 724
geotsam 33:78139f82ea74 725 //Calculating velocities
Ludwigfr 42:ab25bffdc32b 726 linear=KV*(3.1416);
Ludwigfr 42:ab25bffdc32b 727 angular=KH*angleError;
Ludwigfr 42:ab25bffdc32b 728 *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
Ludwigfr 42:ab25bffdc32b 729 *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
geotsam 24:8f4b820d8de8 730 }