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@40:f5e212d9f900, 2017-05-18 (annotated)

Committer:
Ludwigfr
Date:
Thu May 18 18:48:47 2017 +0000
Revision:
40:f5e212d9f900
Parent:
39:dd8326ec75ce
Child:
41:39157b310975
tweaked a few thing, maybe check the math beetween compute_line and go_to_line

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