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@51:b863fad80574, 2017-06-07 (annotated)

Committer:
Ludwigfr
Date:
Wed Jun 07 16:25:54 2017 +0000
Revision:
51:b863fad80574
Parent:
50:a970cf7889d3
Child:
52:54b3fe68a4f2
I dont understand how GoToPoint is supposed to work.

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 51:b863fad80574 7 //update angularLeft and angularRight
Ludwigfr 51:b863fad80574 8 float get_error_angles(float x, float y,float theta);
Ludwigfr 51:b863fad80574 9 void test_procedure_Lab_2();
Ludwigfr 51:b863fad80574 10 void procedure_Lab_3();
Ludwigfr 51:b863fad80574 11 void procedure_Lab_2();
Ludwigfr 51:b863fad80574 12 void turn_to_target(float angleToTarget);
Ludwigfr 39:dd8326ec75ce 13 void initialise_parameters();
Ludwigfr 22:ebb37a249b5f 14 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 22:ebb37a249b5f 15 void fill_initial_log_values();
Ludwigfr 22:ebb37a249b5f 16 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 22:ebb37a249b5f 17 void randomize_and_map();
geotsam 29:224e9e686f7b 18 //make the robot do a pi/2 flip
geotsam 29:224e9e686f7b 19 void do_half_flip();
Ludwigfr 22:ebb37a249b5f 20 //go the the given position while updating the map
Ludwigfr 22:ebb37a249b5f 21 void go_to_point_with_angle(float target_x, float target_y, float target_angle);
Ludwigfr 51:b863fad80574 22 void compute_angles_and_distance(float target_x, float target_y, float target_angle);
Ludwigfr 51:b863fad80574 23 void compute_linear_angular_velocities();
Ludwigfr 43:ffd5a6d4dd48 24 //Updates sonar values
geotsam 24:8f4b820d8de8 25 void update_sonar_values(float leftMm, float frontMm, float rightMm);
Ludwigfr 22:ebb37a249b5f 26 //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 27 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
Ludwigfr 22:ebb37a249b5f 28 //print the map
Ludwigfr 22:ebb37a249b5f 29 void print_final_map();
Ludwigfr 25:572c9e9a8809 30 //print the map with the robot marked on it
Ludwigfr 25:572c9e9a8809 31 void print_final_map_with_robot_position();
Ludwigfr 39:dd8326ec75ce 32 //print the map with the robot and the target marked on it
Ludwigfr 39:dd8326ec75ce 33 void print_final_map_with_robot_position_and_target();
Ludwigfr 42:ab25bffdc32b 34 //go to a given line by updating angularLeft and angularRight
Ludwigfr 42:ab25bffdc32b 35 void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c);
geotsam 33:78139f82ea74 36 //calculate virtual force field and move
Ludwigfr 42:ab25bffdc32b 37 void vff(bool* reached);
Ludwigfr 43:ffd5a6d4dd48 38 void test_got_to_line(bool* reached);
geotsam 0:8bffb51cc345 39
Ludwigfr 22:ebb37a249b5f 40 //MATHS heavy functions
Ludwigfr 22:ebb37a249b5f 41 float dist(float robot_x, float robot_y, float target_x, float target_y);
Ludwigfr 22:ebb37a249b5f 42 //returns the probability [0,1] that the cell is occupied from the log value lt
Ludwigfr 22:ebb37a249b5f 43 float log_to_proba(float lt);
Ludwigfr 22:ebb37a249b5f 44 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 22:ebb37a249b5f 45 float proba_to_log(float p);
Ludwigfr 22:ebb37a249b5f 46 //returns the new log value
Ludwigfr 22:ebb37a249b5f 47 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue );
Ludwigfr 22:ebb37a249b5f 48 //makes the angle inAngle between 0 and 2pi
Ludwigfr 22:ebb37a249b5f 49 float rad_angle_check(float inAngle);
Ludwigfr 22:ebb37a249b5f 50 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 22:ebb37a249b5f 51 float compute_angle_between_vectors(float x, float y,float xs,float ys);
Ludwigfr 39:dd8326ec75ce 52 float x_robot_in_orthonormal_x(float x, float y);
Ludwigfr 39:dd8326ec75ce 53 float y_robot_in_orthonormal_y(float x, float y);
Ludwigfr 25:572c9e9a8809 54 float robot_center_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 55 float robot_center_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 56 float robot_sonar_front_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 57 float robot_sonar_front_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 58 float robot_sonar_right_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 59 float robot_sonar_right_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 60 float robot_sonar_left_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 61 float robot_sonar_left_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 62 float estimated_width_indice_in_orthonormal_x(int i);
Ludwigfr 25:572c9e9a8809 63 float estimated_height_indice_in_orthonormal_y(int j);
Ludwigfr 32:d51928b58645 64 //update foceX and forceY if necessary
Ludwigfr 43:ffd5a6d4dd48 65 void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho );
Ludwigfr 32:d51928b58645 66 //compute the X and Y force
Ludwigfr 39:dd8326ec75ce 67 void compute_forceX_and_forceY(float* forceX, float* forceY);
Ludwigfr 42:ab25bffdc32b 68 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c
Ludwigfr 42:ab25bffdc32b 69 void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c);
Ludwigfr 43:ffd5a6d4dd48 70 //return 1 if positiv, -1 if negativ
Ludwigfr 43:ffd5a6d4dd48 71 float sign1(float value);
Ludwigfr 43:ffd5a6d4dd48 72 //return 1 if positiv, 0 if negativ
Ludwigfr 43:ffd5a6d4dd48 73 int sign2(float value);
Ludwigfr 45:fb07065a64a9 74 //set target in ortho space, in reference to the robot (so if the robot is in the middle, you want to him to go 10cm down and 15 right, set_target_to(15,-10)
Ludwigfr 43:ffd5a6d4dd48 75 void set_target_to(float x, float y);
Ludwigfr 44:e2bd06f94dc0 76 void try_to_reach_target();
Ludwigfr 51:b863fad80574 77 void test_map_sonar();
AurelienBernier 8:109314be5b68 78
Ludwigfr 50:a970cf7889d3 79 //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 50:a970cf7889d3 80 float targetX;//this is in the robot space top left
Ludwigfr 50:a970cf7889d3 81 float targetY;//this is in the robot space top left
Ludwigfr 50:a970cf7889d3 82 //Ortho but for the map (i.e x and Y are > 0)
Ludwigfr 50:a970cf7889d3 83 float targetXOrhto;
Ludwigfr 50:a970cf7889d3 84 float targetYOrhto;
Ludwigfr 50:a970cf7889d3 85 float targetXOrhtoNotMap;
Ludwigfr 50:a970cf7889d3 86 float targetYOrhtoNotMap;
Ludwigfr 50:a970cf7889d3 87
Ludwigfr 22:ebb37a249b5f 88 const float pi=3.14159;
Ludwigfr 32:d51928b58645 89
Ludwigfr 22:ebb37a249b5f 90 //spec of the sonar
Ludwigfr 44:e2bd06f94dc0 91 //TODO MEASURE THE DISTANCE on X and Y of the robot space, between each sonar and the center of the robot and add it to calculus in updateSonarValues
geotsam 24:8f4b820d8de8 92 const float RANGE_SONAR=50;//cm
geotsam 24:8f4b820d8de8 93 const float RANGE_SONAR_MIN=10;//Rmin cm
geotsam 24:8f4b820d8de8 94 const float INCERTITUDE_SONAR=10;//cm
geotsam 24:8f4b820d8de8 95 const float ANGLE_SONAR=pi/3;//Omega rad
Ludwigfr 22:ebb37a249b5f 96
Ludwigfr 44:e2bd06f94dc0 97 //those distance and angle are approximation in need of measurements, in the orthonormal space
geotsam 24:8f4b820d8de8 98 const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees
Ludwigfr 27:07bde633af72 99 const float DISTANCE_SONAR_LEFT_X=-4;
geotsam 24:8f4b820d8de8 100 const float DISTANCE_SONAR_LEFT_Y=4;
Ludwigfr 22:ebb37a249b5f 101
geotsam 24:8f4b820d8de8 102 const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees
Ludwigfr 27:07bde633af72 103 const float DISTANCE_SONAR_RIGHT_X=4;
geotsam 24:8f4b820d8de8 104 const float DISTANCE_SONAR_RIGHT_Y=4;
AurelienBernier 11:e641aa08c92e 105
Ludwigfr 22:ebb37a249b5f 106 const float ANGLE_FRONT_TO_FRONT=0;
Ludwigfr 22:ebb37a249b5f 107 const float DISTANCE_SONAR_FRONT_X=0;
Ludwigfr 22:ebb37a249b5f 108 const float DISTANCE_SONAR_FRONT_Y=5;
Ludwigfr 22:ebb37a249b5f 109
Ludwigfr 22:ebb37a249b5f 110 //TODO adjust the size of the map for computation time (25*25?)
Ludwigfr 51:b863fad80574 111 const float WIDTH_ARENA=200;//cm
Ludwigfr 51:b863fad80574 112 const float HEIGHT_ARENA=200;//cm
geotsam 24:8f4b820d8de8 113
geotsam 24:8f4b820d8de8 114 //const int SIZE_MAP=25;
Ludwigfr 51:b863fad80574 115 const int NB_CELL_WIDTH=20;
Ludwigfr 51:b863fad80574 116 const int NB_CELL_HEIGHT=20;
Ludwigfr 22:ebb37a249b5f 117
geotsam 24:8f4b820d8de8 118 //used to create the map 250 represent the 250cm of the square where the robot is tested
geotsam 24:8f4b820d8de8 119 //float sizeCell=250/(float)SIZE_MAP;
Ludwigfr 27:07bde633af72 120 float sizeCellWidth=WIDTH_ARENA/(float)NB_CELL_WIDTH;
Ludwigfr 27:07bde633af72 121 float sizeCellHeight=HEIGHT_ARENA/(float)NB_CELL_HEIGHT;
geotsam 24:8f4b820d8de8 122
geotsam 24:8f4b820d8de8 123 float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell
geotsam 24:8f4b820d8de8 124 float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT];
Ludwigfr 22:ebb37a249b5f 125
Ludwigfr 22:ebb37a249b5f 126 //Diameter of a wheel and distance between the 2
Ludwigfr 22:ebb37a249b5f 127 const float RADIUS_WHEELS=3.25;
Ludwigfr 22:ebb37a249b5f 128 const float DISTANCE_WHEELS=7.2;
Ludwigfr 22:ebb37a249b5f 129
Ludwigfr 47:f0fe58571e4a 130 //position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot space
Ludwigfr 47:f0fe58571e4a 131 //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 51:b863fad80574 132 const float DEFAULT_X=20;
Ludwigfr 47:f0fe58571e4a 133 const float DEFAULT_Y=WIDTH_ARENA/2;
Ludwigfr 47:f0fe58571e4a 134 //const float DEFAULT_X=20;//lower right
Ludwigfr 47:f0fe58571e4a 135 //const float DEFAULT_Y=20;//lower right
Ludwigfr 47:f0fe58571e4a 136 const float DEFAULT_THETA=0;
Ludwigfr 47:f0fe58571e4a 137
Ludwigfr 51:b863fad80574 138 const int MAX_SPEED=50;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
Ludwigfr 39:dd8326ec75ce 139
Ludwigfr 47:f0fe58571e4a 140 const float KRHO=12, KA=30, KB=-13, KV=200, KH=200; //Kappa values
Ludwigfr 43:ffd5a6d4dd48 141
Ludwigfr 43:ffd5a6d4dd48 142 //CONSTANT FORCE FIELD
Ludwigfr 51:b863fad80574 143 const float FORCE_CONSTANT_REPULSION=10000;//TODO tweak it
Ludwigfr 51:b863fad80574 144 const float FORCE_CONSTANT_ATTRACTION=1;//TODO tweak it
Ludwigfr 51:b863fad80574 145 const float RANGE_FORCE=30;//TODO tweak it
Ludwigfr 51:b863fad80574 146
Ludwigfr 51:b863fad80574 147 float alpha; //angle error
Ludwigfr 51:b863fad80574 148 float rho; //distance from target
Ludwigfr 51:b863fad80574 149 float beta;
Ludwigfr 51:b863fad80574 150 float linear, angular, angular_left, angular_right;
Ludwigfr 51:b863fad80574 151 float dt=0.5;
Ludwigfr 51:b863fad80574 152 float temp;
Ludwigfr 51:b863fad80574 153 float d2;
Ludwigfr 51:b863fad80574 154 Timer t;
geotsam 33:78139f82ea74 155
geotsam 0:8bffb51cc345 156 int main(){
Ludwigfr 44:e2bd06f94dc0 157 initialise_parameters();
Ludwigfr 51:b863fad80574 158 procedure_Lab_3();
Ludwigfr 51:b863fad80574 159 }
Ludwigfr 51:b863fad80574 160
Ludwigfr 51:b863fad80574 161 void procedure_Lab_3(){
Ludwigfr 51:b863fad80574 162 //try to reach the target is ortho space
Ludwigfr 51:b863fad80574 163 set_target_to(0,80);//
Ludwigfr 44:e2bd06f94dc0 164 print_final_map_with_robot_position_and_target();
Ludwigfr 44:e2bd06f94dc0 165 try_to_reach_target();
Ludwigfr 47:f0fe58571e4a 166 //set_target_to(0,20);//lower right
Ludwigfr 47:f0fe58571e4a 167 //print_final_map_with_robot_position_and_target();
Ludwigfr 47:f0fe58571e4a 168 //try_to_reach_target();
Ludwigfr 47:f0fe58571e4a 169 //set_target_to(-20,-20);//up left
Ludwigfr 47:f0fe58571e4a 170 //print_final_map_with_robot_position_and_target();
Ludwigfr 47:f0fe58571e4a 171 //try_to_reach_target();
Ludwigfr 44:e2bd06f94dc0 172 //print the map forever
Ludwigfr 44:e2bd06f94dc0 173 while(1){
Ludwigfr 44:e2bd06f94dc0 174 print_final_map_with_robot_position_and_target();
Ludwigfr 44:e2bd06f94dc0 175 }
Ludwigfr 44:e2bd06f94dc0 176 }
Ludwigfr 44:e2bd06f94dc0 177
Ludwigfr 51:b863fad80574 178 void test_map_sonar(){
Ludwigfr 51:b863fad80574 179 float leftMm;
Ludwigfr 51:b863fad80574 180 float frontMm;
Ludwigfr 51:b863fad80574 181 float rightMm;
Ludwigfr 51:b863fad80574 182 X=20;
Ludwigfr 51:b863fad80574 183 Y=WIDTH_ARENA/2;
Ludwigfr 51:b863fad80574 184 theta=0;
Ludwigfr 51:b863fad80574 185 while(1){
Ludwigfr 51:b863fad80574 186 leftMm = get_distance_left_sensor();
Ludwigfr 51:b863fad80574 187 frontMm = get_distance_front_sensor();
Ludwigfr 51:b863fad80574 188 rightMm = get_distance_right_sensor();
Ludwigfr 51:b863fad80574 189 pc.printf("%f, %f, %f",leftMm,frontMm,rightMm);
Ludwigfr 51:b863fad80574 190 //update the probabilities values
Ludwigfr 51:b863fad80574 191 update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 51:b863fad80574 192 print_final_map();
Ludwigfr 51:b863fad80574 193 }
Ludwigfr 51:b863fad80574 194 }
Ludwigfr 51:b863fad80574 195
Ludwigfr 51:b863fad80574 196 void test_procedure_Lab_2(){
Ludwigfr 51:b863fad80574 197 X=HEIGHT_ARENA/2;
Ludwigfr 51:b863fad80574 198 Y=WIDTH_ARENA/2;
Ludwigfr 51:b863fad80574 199 set_target_to(-100,50);
Ludwigfr 51:b863fad80574 200 print_final_map_with_robot_position_and_target();
Ludwigfr 51:b863fad80574 201 go_to_point_with_angle(targetX, targetY, pi/2);
Ludwigfr 51:b863fad80574 202 print_final_map_with_robot_position_and_target();
Ludwigfr 51:b863fad80574 203
Ludwigfr 51:b863fad80574 204 }
Ludwigfr 51:b863fad80574 205 void procedure_Lab_2(){
Ludwigfr 51:b863fad80574 206 for(int i=0;i<25;i++){
Ludwigfr 51:b863fad80574 207 randomize_and_map();
Ludwigfr 51:b863fad80574 208 print_final_map_with_robot_position();
Ludwigfr 51:b863fad80574 209 wait(2);
Ludwigfr 51:b863fad80574 210 }
Ludwigfr 51:b863fad80574 211 }
Ludwigfr 51:b863fad80574 212
Ludwigfr 51:b863fad80574 213
Ludwigfr 44:e2bd06f94dc0 214 void try_to_reach_target(){
Ludwigfr 51:b863fad80574 215 //atan2 gives the angle beetween pi and -pi
Ludwigfr 51:b863fad80574 216 float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap);
Ludwigfr 51:b863fad80574 217 turn_to_target(angleToTarget);
Ludwigfr 44:e2bd06f94dc0 218 bool reached=false;
Ludwigfr 44:e2bd06f94dc0 219 int print=0;
Ludwigfr 39:dd8326ec75ce 220 while (!reached) {
Ludwigfr 43:ffd5a6d4dd48 221 vff(&reached);
Ludwigfr 43:ffd5a6d4dd48 222 //test_got_to_line(&reached);
Ludwigfr 51:b863fad80574 223 if(print==10){
Ludwigfr 44:e2bd06f94dc0 224 leftMotor(1,0);
Ludwigfr 44:e2bd06f94dc0 225 rightMotor(1,0);
Ludwigfr 51:b863fad80574 226 /*
Ludwigfr 44:e2bd06f94dc0 227 pc.printf("\r\n theta=%f", theta);
Ludwigfr 44:e2bd06f94dc0 228 pc.printf("\r\n IN ORTHO:");
Ludwigfr 44:e2bd06f94dc0 229 pc.printf("\r\n X Robot=%f", robot_center_x_in_orthonormal_x());
Ludwigfr 44:e2bd06f94dc0 230 pc.printf("\r\n Y Robot=%f", robot_center_y_in_orthonormal_y());
Ludwigfr 44:e2bd06f94dc0 231 pc.printf("\r\n X Target=%f", targetXOrhto);
Ludwigfr 44:e2bd06f94dc0 232 pc.printf("\r\n Y Target=%f", targetYOrhto);
Ludwigfr 51:b863fad80574 233 */
Ludwigfr 44:e2bd06f94dc0 234 print_final_map_with_robot_position_and_target();
Ludwigfr 44:e2bd06f94dc0 235 print=0;
Ludwigfr 44:e2bd06f94dc0 236 }else
Ludwigfr 44:e2bd06f94dc0 237 print+=1;
Ludwigfr 39:dd8326ec75ce 238 }
Ludwigfr 39:dd8326ec75ce 239 //Stop at the end
Ludwigfr 39:dd8326ec75ce 240 leftMotor(1,0);
Ludwigfr 39:dd8326ec75ce 241 rightMotor(1,0);
Ludwigfr 44:e2bd06f94dc0 242 pc.printf("\r\n target reached");
Ludwigfr 47:f0fe58571e4a 243 wait(3);//
Ludwigfr 39:dd8326ec75ce 244 }
Ludwigfr 39:dd8326ec75ce 245
Ludwigfr 45:fb07065a64a9 246 //target in ortho space
Ludwigfr 43:ffd5a6d4dd48 247 void set_target_to(float x, float y){
Ludwigfr 45:fb07065a64a9 248 targetX=y;
Ludwigfr 45:fb07065a64a9 249 targetY=-x;
Ludwigfr 43:ffd5a6d4dd48 250 targetXOrhto=x_robot_in_orthonormal_x(targetX,targetY);
Ludwigfr 43:ffd5a6d4dd48 251 targetYOrhto=y_robot_in_orthonormal_y(targetX,targetY);
Ludwigfr 47:f0fe58571e4a 252 targetXOrhtoNotMap=x;
Ludwigfr 47:f0fe58571e4a 253 targetYOrhtoNotMap=y;
Ludwigfr 43:ffd5a6d4dd48 254 }
Ludwigfr 43:ffd5a6d4dd48 255
Ludwigfr 39:dd8326ec75ce 256 void initialise_parameters(){
geotsam 13:41f75c132135 257 i2c1.frequency(100000);
AurelienBernier 2:ea61e801e81f 258 initRobot(); //Initializing the robot
geotsam 0:8bffb51cc345 259 pc.baud(9600); // baud for the pc communication
geotsam 0:8bffb51cc345 260
Ludwigfr 22:ebb37a249b5f 261 measure_always_on();//TODO check if needed
geotsam 24:8f4b820d8de8 262 wait(0.5);
Ludwigfr 39:dd8326ec75ce 263 //fill the map with the initial log values
Ludwigfr 22:ebb37a249b5f 264 fill_initial_log_values();
geotsam 13:41f75c132135 265
Ludwigfr 31:352be78e1aad 266 theta=DEFAULT_THETA;
Ludwigfr 35:c8f224ab153f 267 X=DEFAULT_X;
Ludwigfr 35:c8f224ab153f 268 Y=DEFAULT_Y;
AurelienBernier 8:109314be5b68 269 }
AurelienBernier 8:109314be5b68 270
Ludwigfr 22:ebb37a249b5f 271 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 22:ebb37a249b5f 272 void fill_initial_log_values(){
Ludwigfr 31:352be78e1aad 273 //Fill map, we know the border are occupied
geotsam 24:8f4b820d8de8 274 for (int i = 0; i<NB_CELL_WIDTH; i++) {
geotsam 24:8f4b820d8de8 275 for (int j = 0; j<NB_CELL_HEIGHT; j++) {
geotsam 24:8f4b820d8de8 276 if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1)
Ludwigfr 22:ebb37a249b5f 277 initialLogValues[i][j] = proba_to_log(1);
Ludwigfr 22:ebb37a249b5f 278 else
Ludwigfr 22:ebb37a249b5f 279 initialLogValues[i][j] = proba_to_log(0.5);
AurelienBernier 21:62154d644531 280 }
Ludwigfr 22:ebb37a249b5f 281 }
AurelienBernier 8:109314be5b68 282 }
AurelienBernier 8:109314be5b68 283
Ludwigfr 22:ebb37a249b5f 284 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 22:ebb37a249b5f 285 void randomize_and_map() {
geotsam 24:8f4b820d8de8 286 //TODO check that it's aurelien's work
Ludwigfr 51:b863fad80574 287 float movementOnX=rand()%(int)(WIDTH_ARENA/2);
Ludwigfr 51:b863fad80574 288 float movementOnY=rand()%(int)(HEIGHT_ARENA/2);
Ludwigfr 51:b863fad80574 289
Ludwigfr 51:b863fad80574 290 float signOfX=rand()%2;
Ludwigfr 51:b863fad80574 291 if(signOfX < 1)
Ludwigfr 51:b863fad80574 292 signOfX=-1;
Ludwigfr 51:b863fad80574 293 float signOfY=rand()%2;
Ludwigfr 51:b863fad80574 294 if(signOfY < 1)
Ludwigfr 51:b863fad80574 295 signOfY=-1;
Ludwigfr 51:b863fad80574 296
Ludwigfr 51:b863fad80574 297 float x = movementOnX*signOfX;
Ludwigfr 51:b863fad80574 298 float y = movementOnY*signOfY;
geotsam 30:95d8d3e2b81b 299 float target_angle = 2*((float)(rand()%31416)-15708)/10000.0;
Ludwigfr 51:b863fad80574 300 set_target_to(x,y);
Ludwigfr 43:ffd5a6d4dd48 301 go_to_point_with_angle(targetX, targetY, target_angle);
AurelienBernier 19:dbc5fbad4975 302 }
AurelienBernier 19:dbc5fbad4975 303
geotsam 29:224e9e686f7b 304
geotsam 29:224e9e686f7b 305 void do_half_flip(){
Ludwigfr 28:f884979a02fa 306 Odometria();
geotsam 29:224e9e686f7b 307 float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
geotsam 29:224e9e686f7b 308 if(theta_plus_h_pi > pi)
geotsam 29:224e9e686f7b 309 theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
geotsam 29:224e9e686f7b 310 leftMotor(0,100);
geotsam 29:224e9e686f7b 311 rightMotor(1,100);
geotsam 29:224e9e686f7b 312 while(abs(theta_plus_h_pi-theta)>0.05){
Ludwigfr 28:f884979a02fa 313 Odometria();
geotsam 29:224e9e686f7b 314 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
Ludwigfr 28:f884979a02fa 315 }
Ludwigfr 28:f884979a02fa 316 leftMotor(1,0);
Ludwigfr 28:f884979a02fa 317 rightMotor(1,0);
Ludwigfr 28:f884979a02fa 318 }
Ludwigfr 28:f884979a02fa 319
Ludwigfr 25:572c9e9a8809 320 //ODOMETRIA MUST HAVE BEEN CALLED
Ludwigfr 22:ebb37a249b5f 321 //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 322 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){
Ludwigfr 22:ebb37a249b5f 323
Ludwigfr 22:ebb37a249b5f 324 float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
Ludwigfr 50:a970cf7889d3 325 if( distancePointToSonar < RANGE_SONAR){
Ludwigfr 50:a970cf7889d3 326 float anglePointToSonar=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
Ludwigfr 50:a970cf7889d3 327 float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition;
Ludwigfr 50:a970cf7889d3 328 anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
Ludwigfr 50:a970cf7889d3 329
Ludwigfr 50:a970cf7889d3 330 if(alphaBeforeAdjustment>pi)
Ludwigfr 50:a970cf7889d3 331 alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi;
Ludwigfr 50:a970cf7889d3 332 if(alphaBeforeAdjustment<-pi)
Ludwigfr 50:a970cf7889d3 333 alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi;
Ludwigfr 50:a970cf7889d3 334
Ludwigfr 50:a970cf7889d3 335 //float anglePointToSonar2=atan2(y-ys,x-xs)-theta;
Ludwigfr 48:cd3463dcca04 336
Ludwigfr 50:a970cf7889d3 337 //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
Ludwigfr 50:a970cf7889d3 338 //check if absolute difference between the angles is no more than Omega/2
Ludwigfr 50:a970cf7889d3 339 if(anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= rad_angle_check(-ANGLE_SONAR/2)){
Ludwigfr 50:a970cf7889d3 340 if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
Ludwigfr 50:a970cf7889d3 341 //point before obstacle, probably empty
Ludwigfr 50:a970cf7889d3 342 /*****************************************************************************/
Ludwigfr 50:a970cf7889d3 343 float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 50:a970cf7889d3 344 float Er;
Ludwigfr 50:a970cf7889d3 345 if(distancePointToSonar < RANGE_SONAR_MIN){
Ludwigfr 50:a970cf7889d3 346 //point before minimum sonar range
Ludwigfr 50:a970cf7889d3 347 Er=0.f;
Ludwigfr 50:a970cf7889d3 348 }else{
Ludwigfr 50:a970cf7889d3 349 //point after minimum sonar range
Ludwigfr 50:a970cf7889d3 350 Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
Ludwigfr 50:a970cf7889d3 351 }
Ludwigfr 50:a970cf7889d3 352 /*****************************************************************************/
Ludwigfr 50:a970cf7889d3 353 //if((1.f-Er*Ea)/2.f >1 || (1.f-Er*Ea)/2.f < 0)
Ludwigfr 50:a970cf7889d3 354 // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1.f-Er*Ea)/2.f,Er,Ea,alphaBeforeAdjustment);
Ludwigfr 50:a970cf7889d3 355 return (1.f-Er*Ea)/2.f;
Ludwigfr 22:ebb37a249b5f 356 }else{
Ludwigfr 50:a970cf7889d3 357 //probably occupied
Ludwigfr 50:a970cf7889d3 358 /*****************************************************************************/
Ludwigfr 50:a970cf7889d3 359 float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 50:a970cf7889d3 360 float Or;
Ludwigfr 50:a970cf7889d3 361 if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
Ludwigfr 50:a970cf7889d3 362 //point between distanceObstacleDetected +- INCERTITUDE_SONAR
Ludwigfr 50:a970cf7889d3 363 Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
Ludwigfr 50:a970cf7889d3 364 }else{
Ludwigfr 50:a970cf7889d3 365 //point after in range of the sonar but after the zone detected
Ludwigfr 50:a970cf7889d3 366 Or=0;
Ludwigfr 50:a970cf7889d3 367 }
Ludwigfr 50:a970cf7889d3 368 /*****************************************************************************/
Ludwigfr 50:a970cf7889d3 369 //if((1+Or*Oa)/2 >1 || (1+Or*Oa)/2 < 0)
Ludwigfr 50:a970cf7889d3 370 // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1+Or*Oa)/2,Or,Oa,alphaBeforeAdjustment);
Ludwigfr 50:a970cf7889d3 371 return (1+Or*Oa)/2;
Ludwigfr 22:ebb37a249b5f 372 }
Ludwigfr 50:a970cf7889d3 373 }
AurelienBernier 19:dbc5fbad4975 374 }
Ludwigfr 50:a970cf7889d3 375 //not checked by the sonar
Ludwigfr 50:a970cf7889d3 376 return 0.5;
Ludwigfr 22:ebb37a249b5f 377 }
Ludwigfr 22:ebb37a249b5f 378
Ludwigfr 25:572c9e9a8809 379 void print_final_map() {
Ludwigfr 22:ebb37a249b5f 380 float currProba;
geotsam 24:8f4b820d8de8 381 pc.printf("\n\r");
geotsam 24:8f4b820d8de8 382 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
geotsam 24:8f4b820d8de8 383 for (int x= 0; x<NB_CELL_WIDTH; x++) {
geotsam 24:8f4b820d8de8 384 currProba=log_to_proba(map[x][y]);
geotsam 24:8f4b820d8de8 385 if ( currProba < 0.5) {
geotsam 29:224e9e686f7b 386 pc.printf(" ");
Ludwigfr 22:ebb37a249b5f 387 } else {
Ludwigfr 22:ebb37a249b5f 388 if(currProba==0.5)
geotsam 24:8f4b820d8de8 389 pc.printf(" . ");
Ludwigfr 22:ebb37a249b5f 390 else
geotsam 29:224e9e686f7b 391 pc.printf(" X ");
Ludwigfr 22:ebb37a249b5f 392 }
Ludwigfr 22:ebb37a249b5f 393 }
geotsam 24:8f4b820d8de8 394 pc.printf("\n\r");
Ludwigfr 22:ebb37a249b5f 395 }
Ludwigfr 22:ebb37a249b5f 396 }
Ludwigfr 22:ebb37a249b5f 397
Ludwigfr 25:572c9e9a8809 398 void print_final_map_with_robot_position() {
geotsam 24:8f4b820d8de8 399 float currProba;
Ludwigfr 25:572c9e9a8809 400 Odometria();
Ludwigfr 25:572c9e9a8809 401 float Xrobot=robot_center_x_in_orthonormal_x();
Ludwigfr 25:572c9e9a8809 402 float Yrobot=robot_center_y_in_orthonormal_y();
Ludwigfr 25:572c9e9a8809 403
Ludwigfr 25:572c9e9a8809 404 float heightIndiceInOrthonormal;
Ludwigfr 25:572c9e9a8809 405 float widthIndiceInOrthonormal;
Ludwigfr 25:572c9e9a8809 406
Ludwigfr 27:07bde633af72 407 float widthMalus=-(3*sizeCellWidth/2);
Ludwigfr 27:07bde633af72 408 float widthBonus=sizeCellWidth/2;
Ludwigfr 25:572c9e9a8809 409
Ludwigfr 27:07bde633af72 410 float heightMalus=-(3*sizeCellHeight/2);
Ludwigfr 27:07bde633af72 411 float heightBonus=sizeCellHeight/2;
Ludwigfr 25:572c9e9a8809 412
geotsam 24:8f4b820d8de8 413 pc.printf("\n\r");
geotsam 24:8f4b820d8de8 414 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
geotsam 24:8f4b820d8de8 415 for (int x= 0; x<NB_CELL_WIDTH; x++) {
Ludwigfr 25:572c9e9a8809 416 heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
Ludwigfr 25:572c9e9a8809 417 widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
Ludwigfr 27:07bde633af72 418 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 27:07bde633af72 419 pc.printf(" R ");
Ludwigfr 25:572c9e9a8809 420 else{
Ludwigfr 25:572c9e9a8809 421 currProba=log_to_proba(map[x][y]);
Ludwigfr 25:572c9e9a8809 422 if ( currProba < 0.5)
geotsam 29:224e9e686f7b 423 pc.printf(" ");
Ludwigfr 25:572c9e9a8809 424 else{
Ludwigfr 25:572c9e9a8809 425 if(currProba==0.5)
Ludwigfr 27:07bde633af72 426 pc.printf(" . ");
Ludwigfr 25:572c9e9a8809 427 else
geotsam 29:224e9e686f7b 428 pc.printf(" X ");
Ludwigfr 25:572c9e9a8809 429 }
geotsam 24:8f4b820d8de8 430 }
geotsam 24:8f4b820d8de8 431 }
geotsam 24:8f4b820d8de8 432 pc.printf("\n\r");
geotsam 24:8f4b820d8de8 433 }
geotsam 24:8f4b820d8de8 434 }
Ludwigfr 22:ebb37a249b5f 435
Ludwigfr 39:dd8326ec75ce 436 void print_final_map_with_robot_position_and_target() {
Ludwigfr 39:dd8326ec75ce 437 float currProba;
Ludwigfr 39:dd8326ec75ce 438 Odometria();
Ludwigfr 39:dd8326ec75ce 439 float Xrobot=robot_center_x_in_orthonormal_x();
Ludwigfr 39:dd8326ec75ce 440 float Yrobot=robot_center_y_in_orthonormal_y();
Ludwigfr 39:dd8326ec75ce 441
Ludwigfr 39:dd8326ec75ce 442 float heightIndiceInOrthonormal;
Ludwigfr 39:dd8326ec75ce 443 float widthIndiceInOrthonormal;
Ludwigfr 39:dd8326ec75ce 444
Ludwigfr 39:dd8326ec75ce 445 float widthMalus=-(3*sizeCellWidth/2);
Ludwigfr 39:dd8326ec75ce 446 float widthBonus=sizeCellWidth/2;
Ludwigfr 39:dd8326ec75ce 447
Ludwigfr 39:dd8326ec75ce 448 float heightMalus=-(3*sizeCellHeight/2);
Ludwigfr 39:dd8326ec75ce 449 float heightBonus=sizeCellHeight/2;
Ludwigfr 39:dd8326ec75ce 450
Ludwigfr 39:dd8326ec75ce 451 pc.printf("\n\r");
Ludwigfr 39:dd8326ec75ce 452 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
Ludwigfr 39:dd8326ec75ce 453 for (int x= 0; x<NB_CELL_WIDTH; x++) {
Ludwigfr 39:dd8326ec75ce 454 heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
Ludwigfr 39:dd8326ec75ce 455 widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
Ludwigfr 39:dd8326ec75ce 456 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 39:dd8326ec75ce 457 pc.printf(" R ");
Ludwigfr 39:dd8326ec75ce 458 else{
Ludwigfr 43:ffd5a6d4dd48 459 if(targetYOrhto >= (heightIndiceInOrthonormal+heightMalus) && targetYOrhto <= (heightIndiceInOrthonormal+heightBonus) && targetXOrhto >= (widthIndiceInOrthonormal+widthMalus) && targetXOrhto <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 39:dd8326ec75ce 460 pc.printf(" T ");
Ludwigfr 39:dd8326ec75ce 461 else{
Ludwigfr 39:dd8326ec75ce 462 currProba=log_to_proba(map[x][y]);
Ludwigfr 39:dd8326ec75ce 463 if ( currProba < 0.5)
Ludwigfr 39:dd8326ec75ce 464 pc.printf(" ");
Ludwigfr 39:dd8326ec75ce 465 else{
Ludwigfr 39:dd8326ec75ce 466 if(currProba==0.5)
Ludwigfr 39:dd8326ec75ce 467 pc.printf(" . ");
Ludwigfr 39:dd8326ec75ce 468 else
Ludwigfr 39:dd8326ec75ce 469 pc.printf(" X ");
Ludwigfr 39:dd8326ec75ce 470 }
Ludwigfr 39:dd8326ec75ce 471 }
Ludwigfr 39:dd8326ec75ce 472 }
Ludwigfr 39:dd8326ec75ce 473 }
Ludwigfr 39:dd8326ec75ce 474 pc.printf("\n\r");
Ludwigfr 39:dd8326ec75ce 475 }
Ludwigfr 39:dd8326ec75ce 476 }
Ludwigfr 39:dd8326ec75ce 477
Ludwigfr 22:ebb37a249b5f 478 //MATHS heavy functions
Ludwigfr 22:ebb37a249b5f 479 /**********************************************************************/
Ludwigfr 22:ebb37a249b5f 480 //Distance computation function
Ludwigfr 22:ebb37a249b5f 481 float dist(float robot_x, float robot_y, float target_x, float target_y){
Ludwigfr 51:b863fad80574 482 //pc.printf("%f, %f, %f, %f",robot_x,robot_y,target_x,target_y);
Ludwigfr 22:ebb37a249b5f 483 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
Ludwigfr 22:ebb37a249b5f 484 }
Ludwigfr 22:ebb37a249b5f 485
geotsam 24:8f4b820d8de8 486 //returns the probability [0,1] that the cell is occupied from the log valAue lt
Ludwigfr 22:ebb37a249b5f 487 float log_to_proba(float lt){
Ludwigfr 22:ebb37a249b5f 488 return 1-1/(1+exp(lt));
Ludwigfr 22:ebb37a249b5f 489 }
Ludwigfr 22:ebb37a249b5f 490
Ludwigfr 22:ebb37a249b5f 491 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 22:ebb37a249b5f 492 float proba_to_log(float p){
Ludwigfr 22:ebb37a249b5f 493 return log(p/(1-p));
Ludwigfr 22:ebb37a249b5f 494 }
Ludwigfr 22:ebb37a249b5f 495
Ludwigfr 22:ebb37a249b5f 496 //returns the new log value
Ludwigfr 22:ebb37a249b5f 497 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){
Ludwigfr 22:ebb37a249b5f 498 return previousLogValue+proba_to_log(currentProbability)-originalLogvalue;
Ludwigfr 22:ebb37a249b5f 499 }
Ludwigfr 22:ebb37a249b5f 500
Ludwigfr 22:ebb37a249b5f 501 //makes the angle inAngle between 0 and 2pi
Ludwigfr 22:ebb37a249b5f 502 float rad_angle_check(float inAngle){
Ludwigfr 22:ebb37a249b5f 503 //cout<<"before :"<<inAngle;
Ludwigfr 22:ebb37a249b5f 504 if(inAngle > 0){
Ludwigfr 22:ebb37a249b5f 505 while(inAngle > (2*pi))
Ludwigfr 22:ebb37a249b5f 506 inAngle-=2*pi;
Ludwigfr 22:ebb37a249b5f 507 }else{
Ludwigfr 22:ebb37a249b5f 508 while(inAngle < 0)
Ludwigfr 22:ebb37a249b5f 509 inAngle+=2*pi;
Ludwigfr 22:ebb37a249b5f 510 }
Ludwigfr 22:ebb37a249b5f 511 //cout<<" after :"<<inAngle<<endl;
Ludwigfr 22:ebb37a249b5f 512 return inAngle;
Ludwigfr 22:ebb37a249b5f 513 }
Ludwigfr 22:ebb37a249b5f 514
Ludwigfr 22:ebb37a249b5f 515 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 22:ebb37a249b5f 516 float compute_angle_between_vectors(float x, float y,float xs,float ys){
Ludwigfr 22:ebb37a249b5f 517 //alpha angle between ->x and ->SA
Ludwigfr 22:ebb37a249b5f 518 //vector S to A ->SA
Ludwigfr 22:ebb37a249b5f 519 float vSAx=x-xs;
Ludwigfr 22:ebb37a249b5f 520 float vSAy=y-ys;
Ludwigfr 22:ebb37a249b5f 521 //norme SA
Ludwigfr 22:ebb37a249b5f 522 float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
Ludwigfr 22:ebb37a249b5f 523 //vector ->x (1,0)
Ludwigfr 22:ebb37a249b5f 524 float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
Ludwigfr 22:ebb37a249b5f 525 //vector ->y (0,1)
Ludwigfr 22:ebb37a249b5f 526 float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
Ludwigfr 22:ebb37a249b5f 527 if (sinAlpha < 0)
Ludwigfr 22:ebb37a249b5f 528 return -acos(cosAlpha);
Ludwigfr 22:ebb37a249b5f 529 else
Ludwigfr 22:ebb37a249b5f 530 return acos(cosAlpha);
Ludwigfr 25:572c9e9a8809 531 }
Ludwigfr 51:b863fad80574 532
Ludwigfr 51:b863fad80574 533 //return 1 if positiv, -1 if negativ
Ludwigfr 51:b863fad80574 534 float sign1(float value){
Ludwigfr 51:b863fad80574 535 if(value>=0)
Ludwigfr 51:b863fad80574 536 return 1;
Ludwigfr 51:b863fad80574 537 else
Ludwigfr 51:b863fad80574 538 return -1;
Ludwigfr 51:b863fad80574 539 }
Ludwigfr 25:572c9e9a8809 540
Ludwigfr 51:b863fad80574 541 //return 1 if positiv, 0 if negativ
Ludwigfr 51:b863fad80574 542 int sign2(float value){
Ludwigfr 51:b863fad80574 543 if(value>=0)
Ludwigfr 51:b863fad80574 544 return 1;
Ludwigfr 51:b863fad80574 545 else
Ludwigfr 51:b863fad80574 546 return 0;
Ludwigfr 51:b863fad80574 547 }
Ludwigfr 38:3c9f8cbf5250 548
Ludwigfr 51:b863fad80574 549 /*
Ludwigfr 44:e2bd06f94dc0 550 Robot space: orthonormal space:
Ludwigfr 38:3c9f8cbf5250 551 ^ ^
Ludwigfr 38:3c9f8cbf5250 552 |x |y
Ludwigfr 38:3c9f8cbf5250 553 <- R O ->
Ludwigfr 38:3c9f8cbf5250 554 y x
Ludwigfr 38:3c9f8cbf5250 555 */
Ludwigfr 38:3c9f8cbf5250 556 //Odometria must bu up to date
Ludwigfr 36:c806c568720a 557 float x_robot_in_orthonormal_x(float x, float y){
Ludwigfr 38:3c9f8cbf5250 558 return robot_center_x_in_orthonormal_x()-y;
Ludwigfr 36:c806c568720a 559 }
Ludwigfr 36:c806c568720a 560
Ludwigfr 38:3c9f8cbf5250 561 //Odometria must bu up to date
Ludwigfr 36:c806c568720a 562 float y_robot_in_orthonormal_y(float x, float y){
Ludwigfr 38:3c9f8cbf5250 563 return robot_center_y_in_orthonormal_y()+x;
Ludwigfr 36:c806c568720a 564 }
Ludwigfr 36:c806c568720a 565
Ludwigfr 25:572c9e9a8809 566 float robot_center_x_in_orthonormal_x(){
Ludwigfr 27:07bde633af72 567 return NB_CELL_WIDTH*sizeCellWidth-Y;
Ludwigfr 25:572c9e9a8809 568 }
Ludwigfr 25:572c9e9a8809 569
Ludwigfr 25:572c9e9a8809 570 float robot_center_y_in_orthonormal_y(){
Ludwigfr 27:07bde633af72 571 return X;
Ludwigfr 25:572c9e9a8809 572 }
Ludwigfr 25:572c9e9a8809 573
Ludwigfr 25:572c9e9a8809 574 float robot_sonar_front_x_in_orthonormal_x(){
Ludwigfr 27:07bde633af72 575 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_FRONT_X;
Ludwigfr 25:572c9e9a8809 576 }
Ludwigfr 25:572c9e9a8809 577 float robot_sonar_front_y_in_orthonormal_y(){
Ludwigfr 27:07bde633af72 578 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_FRONT_Y;
Ludwigfr 25:572c9e9a8809 579 }
Ludwigfr 25:572c9e9a8809 580
Ludwigfr 25:572c9e9a8809 581 float robot_sonar_right_x_in_orthonormal_x(){
Ludwigfr 27:07bde633af72 582 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_RIGHT_X;
Ludwigfr 25:572c9e9a8809 583 }
Ludwigfr 25:572c9e9a8809 584 float robot_sonar_right_y_in_orthonormal_y(){
Ludwigfr 27:07bde633af72 585 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_RIGHT_Y;
Ludwigfr 25:572c9e9a8809 586 }
Ludwigfr 25:572c9e9a8809 587
Ludwigfr 25:572c9e9a8809 588 float robot_sonar_left_x_in_orthonormal_x(){
Ludwigfr 27:07bde633af72 589 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_LEFT_X;
Ludwigfr 25:572c9e9a8809 590 }
Ludwigfr 25:572c9e9a8809 591 float robot_sonar_left_y_in_orthonormal_y(){
Ludwigfr 27:07bde633af72 592 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_LEFT_Y;
Ludwigfr 25:572c9e9a8809 593 }
Ludwigfr 25:572c9e9a8809 594
Ludwigfr 25:572c9e9a8809 595 float estimated_width_indice_in_orthonormal_x(int i){
Ludwigfr 27:07bde633af72 596 return sizeCellWidth/2+i*sizeCellWidth;
Ludwigfr 25:572c9e9a8809 597 }
Ludwigfr 25:572c9e9a8809 598 float estimated_height_indice_in_orthonormal_y(int j){
Ludwigfr 27:07bde633af72 599 return sizeCellHeight/2+j*sizeCellHeight;
geotsam 26:b020cf253059 600 }
geotsam 26:b020cf253059 601
Ludwigfr 43:ffd5a6d4dd48 602 void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){
Ludwigfr 44:e2bd06f94dc0 603 //get the coordonate of the map and the robot in the ortonormal space
Ludwigfr 32:d51928b58645 604 float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice);
Ludwigfr 32:d51928b58645 605 float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice);
Ludwigfr 32:d51928b58645 606 //compute the distance beetween the cell and the robot
Ludwigfr 32:d51928b58645 607 float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2));
Ludwigfr 32:d51928b58645 608 //check if the cell is in range
Ludwigfr 43:ffd5a6d4dd48 609 if(distanceCellToRobot <= range) {
Ludwigfr 51:b863fad80574 610 /*float anglePointToRobot=compute_angle_between_vectors(xCenterCell,yCenterCell,xRobotOrtho,yRobotOrtho);//angle beetween the point and the sonar
Ludwigfr 51:b863fad80574 611 float alphaBeforeAdjustment=anglePointToRobot-theta;
Ludwigfr 51:b863fad80574 612 anglePointToRobot=rad_angle_check(alphaBeforeAdjustment);
Ludwigfr 51:b863fad80574 613 if(anglePointToRobot <= pi/2 || anglePointToRobot >= rad_angle_check(-pi/2)){
Ludwigfr 51:b863fad80574 614 */
Ludwigfr 51:b863fad80574 615 float probaCell=log_to_proba(map[widthIndice][heightIndice]);
Ludwigfr 51:b863fad80574 616 float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3);
Ludwigfr 51:b863fad80574 617 float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3);
Ludwigfr 51:b863fad80574 618 *forceX+=xForceComputed;
Ludwigfr 51:b863fad80574 619 *forceY+=yForceComputed;
Ludwigfr 51:b863fad80574 620 //}
Ludwigfr 36:c806c568720a 621 }
geotsam 33:78139f82ea74 622 }
geotsam 33:78139f82ea74 623
Ludwigfr 43:ffd5a6d4dd48 624 void test_got_to_line(bool* reached){
Ludwigfr 43:ffd5a6d4dd48 625 float line_a=1;
Ludwigfr 43:ffd5a6d4dd48 626 float line_b=2;
Ludwigfr 43:ffd5a6d4dd48 627 float line_c=-140;
Ludwigfr 43:ffd5a6d4dd48 628 //we update the odometrie
Ludwigfr 43:ffd5a6d4dd48 629 Odometria();
Ludwigfr 43:ffd5a6d4dd48 630 float angularRight=0;
Ludwigfr 43:ffd5a6d4dd48 631 float angularLeft=0;
Ludwigfr 43:ffd5a6d4dd48 632
Ludwigfr 43:ffd5a6d4dd48 633 go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c);
Ludwigfr 48:cd3463dcca04 634 //pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c);
Ludwigfr 43:ffd5a6d4dd48 635
Ludwigfr 43:ffd5a6d4dd48 636 leftMotor(sign2(angularLeft),abs(angularLeft));
Ludwigfr 43:ffd5a6d4dd48 637 rightMotor(sign2(angularRight),abs(angularRight));
Ludwigfr 43:ffd5a6d4dd48 638
Ludwigfr 43:ffd5a6d4dd48 639 pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto));
Ludwigfr 43:ffd5a6d4dd48 640
Ludwigfr 43:ffd5a6d4dd48 641 //wait(0.1);
Ludwigfr 43:ffd5a6d4dd48 642 Odometria();
Ludwigfr 43:ffd5a6d4dd48 643 if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10)
Ludwigfr 43:ffd5a6d4dd48 644 *reached=true;
Ludwigfr 43:ffd5a6d4dd48 645 }
Ludwigfr 42:ab25bffdc32b 646 void vff(bool* reached){
Ludwigfr 42:ab25bffdc32b 647 float line_a;
Ludwigfr 42:ab25bffdc32b 648 float line_b;
Ludwigfr 42:ab25bffdc32b 649 float line_c;
geotsam 33:78139f82ea74 650 //Updating X,Y and theta with the odometry values
geotsam 37:462d19bb221f 651 float forceX=0;
geotsam 37:462d19bb221f 652 float forceY=0;
Ludwigfr 38:3c9f8cbf5250 653 //we update the odometrie
geotsam 33:78139f82ea74 654 Odometria();
Ludwigfr 38:3c9f8cbf5250 655 //we check the sensors
Ludwigfr 40:f5e212d9f900 656 float leftMm = get_distance_left_sensor();
Ludwigfr 40:f5e212d9f900 657 float frontMm = get_distance_front_sensor();
Ludwigfr 40:f5e212d9f900 658 float rightMm = get_distance_right_sensor();
Ludwigfr 42:ab25bffdc32b 659 float angularRight=0;
Ludwigfr 42:ab25bffdc32b 660 float angularLeft=0;
Ludwigfr 38:3c9f8cbf5250 661 //update the probabilities values
geotsam 33:78139f82ea74 662 update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 38:3c9f8cbf5250 663 //we compute the force on X and Y
Ludwigfr 39:dd8326ec75ce 664 compute_forceX_and_forceY(&forceX, &forceY);
Ludwigfr 38:3c9f8cbf5250 665 //we compute a new ine
Ludwigfr 42:ab25bffdc32b 666 calculate_line(forceX, forceY, X, Y,&line_a,&line_b,&line_c);
Ludwigfr 42:ab25bffdc32b 667 go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c);
geotsam 33:78139f82ea74 668
geotsam 33:78139f82ea74 669 //Updating motor velocities
Ludwigfr 43:ffd5a6d4dd48 670
Ludwigfr 43:ffd5a6d4dd48 671 leftMotor(sign2(angularLeft),abs(angularLeft));
Ludwigfr 43:ffd5a6d4dd48 672 rightMotor(sign2(angularRight),abs(angularRight));
geotsam 33:78139f82ea74 673
Ludwigfr 40:f5e212d9f900 674 //wait(0.1);
geotsam 33:78139f82ea74 675 Odometria();
Ludwigfr 43:ffd5a6d4dd48 676 if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10)
Ludwigfr 42:ab25bffdc32b 677 *reached=true;
geotsam 33:78139f82ea74 678 }
geotsam 33:78139f82ea74 679
Ludwigfr 44:e2bd06f94dc0 680 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c
Ludwigfr 44:e2bd06f94dc0 681 void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c){
Ludwigfr 44:e2bd06f94dc0 682 /*
Ludwigfr 44:e2bd06f94dc0 683 in the teachers maths it is
Ludwigfr 44:e2bd06f94dc0 684
Ludwigfr 44:e2bd06f94dc0 685 *line_a=forceY;
Ludwigfr 44:e2bd06f94dc0 686 *line_b=-forceX;
Ludwigfr 44:e2bd06f94dc0 687
Ludwigfr 44:e2bd06f94dc0 688 because a*x+b*y+c=0
Ludwigfr 44:e2bd06f94dc0 689 a impact the vertical and b the horizontal
Ludwigfr 44:e2bd06f94dc0 690 and he has to put them like this because
Ludwigfr 44:e2bd06f94dc0 691 Robot space: orthonormal space:
Ludwigfr 44:e2bd06f94dc0 692 ^ ^
Ludwigfr 44:e2bd06f94dc0 693 |x |y
Ludwigfr 44:e2bd06f94dc0 694 <- R O ->
Ludwigfr 44:e2bd06f94dc0 695 y x
Ludwigfr 44:e2bd06f94dc0 696 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to
Ludwigfr 44:e2bd06f94dc0 697 */
Ludwigfr 44:e2bd06f94dc0 698 *line_a=forceX;
Ludwigfr 44:e2bd06f94dc0 699 *line_b=forceY;
Ludwigfr 44:e2bd06f94dc0 700 //TODO check that
Ludwigfr 44:e2bd06f94dc0 701 //because the line computed always pass by the robot center we dont need lince_c
Ludwigfr 44:e2bd06f94dc0 702 //float xRobotOrtho=robot_center_x_in_orthonormal_x();
Ludwigfr 44:e2bd06f94dc0 703 //float yRobotOrtho=robot_center_y_in_orthonormal_y();
Ludwigfr 44:e2bd06f94dc0 704 //*line_c=forceX*yRobotOrtho+forceY*xRobotOrtho;
Ludwigfr 44:e2bd06f94dc0 705 *line_c=0;
Ludwigfr 51:b863fad80574 706 }
Ludwigfr 51:b863fad80574 707
Ludwigfr 51:b863fad80574 708 /*angleToTarget is obtained through atan2 so it s:
Ludwigfr 51:b863fad80574 709 < 0 if the angle is bettween pi and 2pi on a trigo circle
Ludwigfr 51:b863fad80574 710 > 0 if it is between 0 and pi
Ludwigfr 51:b863fad80574 711 */
Ludwigfr 51:b863fad80574 712 void turn_to_target(float angleToTarget){
Ludwigfr 51:b863fad80574 713 Odometria();
Ludwigfr 51:b863fad80574 714 float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
Ludwigfr 51:b863fad80574 715 if(theta_plus_h_pi > pi)
Ludwigfr 51:b863fad80574 716 theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
Ludwigfr 51:b863fad80574 717 if(angleToTarget>0){
Ludwigfr 51:b863fad80574 718 leftMotor(0,1);
Ludwigfr 51:b863fad80574 719 rightMotor(1,1);
Ludwigfr 51:b863fad80574 720 }else{
Ludwigfr 51:b863fad80574 721 leftMotor(1,1);
Ludwigfr 51:b863fad80574 722 rightMotor(0,1);
Ludwigfr 51:b863fad80574 723 }
Ludwigfr 51:b863fad80574 724 while(abs(angleToTarget-theta_plus_h_pi)>0.05){
Ludwigfr 51:b863fad80574 725 Odometria();
Ludwigfr 51:b863fad80574 726 theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
Ludwigfr 51:b863fad80574 727 if(theta_plus_h_pi > pi)
Ludwigfr 51:b863fad80574 728 theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
Ludwigfr 51:b863fad80574 729 //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
Ludwigfr 51:b863fad80574 730 }
Ludwigfr 51:b863fad80574 731 leftMotor(1,0);
Ludwigfr 51:b863fad80574 732 rightMotor(1,0);
Ludwigfr 51:b863fad80574 733 }
Ludwigfr 51:b863fad80574 734
Ludwigfr 51:b863fad80574 735 //currently line_c is not used
Ludwigfr 51:b863fad80574 736 void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c){
Ludwigfr 51:b863fad80574 737 float lineAngle;
Ludwigfr 51:b863fad80574 738 float angleError;
Ludwigfr 51:b863fad80574 739 float linear;
Ludwigfr 51:b863fad80574 740 float angular;
Ludwigfr 51:b863fad80574 741
Ludwigfr 51:b863fad80574 742 bool direction=true;
Ludwigfr 51:b863fad80574 743
Ludwigfr 51:b863fad80574 744 float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap);
Ludwigfr 51:b863fad80574 745 bool inFront=true;
Ludwigfr 51:b863fad80574 746 if(angleToTarget < 0)//the target is in front
Ludwigfr 51:b863fad80574 747 inFront=false;
Ludwigfr 51:b863fad80574 748
Ludwigfr 51:b863fad80574 749 if(theta > 0){
Ludwigfr 51:b863fad80574 750 //the robot is oriented to the left
Ludwigfr 51:b863fad80574 751 if(theta > pi/2){
Ludwigfr 51:b863fad80574 752 //the robot is oriented lower left
Ludwigfr 51:b863fad80574 753 if(inFront)
Ludwigfr 51:b863fad80574 754 direction=false;//robot and target not oriented the same way
Ludwigfr 51:b863fad80574 755 }else{
Ludwigfr 51:b863fad80574 756 //the robot is oriented upper left
Ludwigfr 51:b863fad80574 757 if(!inFront)
Ludwigfr 51:b863fad80574 758 direction=false;//robot and target not oriented the same way
Ludwigfr 51:b863fad80574 759 }
Ludwigfr 51:b863fad80574 760 }else{
Ludwigfr 51:b863fad80574 761 //the robot is oriented to the right
Ludwigfr 51:b863fad80574 762 if(theta < -pi/2){
Ludwigfr 51:b863fad80574 763 //the robot is oriented lower right
Ludwigfr 51:b863fad80574 764 if(inFront)
Ludwigfr 51:b863fad80574 765 direction=false;//robot and target not oriented the same way
Ludwigfr 51:b863fad80574 766 }else{
Ludwigfr 51:b863fad80574 767 //the robot is oriented upper right
Ludwigfr 51:b863fad80574 768 if(!inFront)
Ludwigfr 51:b863fad80574 769 direction=false;//robot and target not oriented the same way
Ludwigfr 51:b863fad80574 770 }
Ludwigfr 51:b863fad80574 771 }
Ludwigfr 51:b863fad80574 772 //pc.printf("\r\n Target is in front");
Ludwigfr 51:b863fad80574 773
Ludwigfr 51:b863fad80574 774 if(line_b!=0){
Ludwigfr 51:b863fad80574 775 if(!direction)
Ludwigfr 51:b863fad80574 776 lineAngle=atan(-line_a/line_b);
Ludwigfr 51:b863fad80574 777 else
Ludwigfr 51:b863fad80574 778 lineAngle=atan(line_a/-line_b);
Ludwigfr 51:b863fad80574 779 }
Ludwigfr 51:b863fad80574 780 else{
Ludwigfr 51:b863fad80574 781 lineAngle=1.5708;
Ludwigfr 51:b863fad80574 782 }
Ludwigfr 51:b863fad80574 783
Ludwigfr 51:b863fad80574 784 //Computing angle error
Ludwigfr 51:b863fad80574 785 angleError = lineAngle-theta;
Ludwigfr 51:b863fad80574 786 angleError = atan(sin(angleError)/cos(angleError));
Ludwigfr 51:b863fad80574 787
Ludwigfr 51:b863fad80574 788 //Calculating velocities
Ludwigfr 51:b863fad80574 789 linear=KV*(3.1416);
Ludwigfr 51:b863fad80574 790 angular=KH*angleError;
Ludwigfr 51:b863fad80574 791
Ludwigfr 51:b863fad80574 792 *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
Ludwigfr 51:b863fad80574 793 *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
Ludwigfr 51:b863fad80574 794
Ludwigfr 51:b863fad80574 795 float aL=*angularLeft;
Ludwigfr 51:b863fad80574 796 float aR=*angularRight;
Ludwigfr 51:b863fad80574 797 //Normalize speed for motors
Ludwigfr 51:b863fad80574 798 if(abs(*angularLeft)>abs(*angularRight)) {
Ludwigfr 51:b863fad80574 799 *angularRight=MAX_SPEED*abs(aR/aL)*sign1(aR);
Ludwigfr 51:b863fad80574 800 *angularLeft=MAX_SPEED*sign1(aL);
Ludwigfr 51:b863fad80574 801 }
Ludwigfr 51:b863fad80574 802 else {
Ludwigfr 51:b863fad80574 803 *angularLeft=MAX_SPEED*abs(aL/aR)*sign1(aL);
Ludwigfr 51:b863fad80574 804 *angularRight=MAX_SPEED*sign1(aR);
Ludwigfr 51:b863fad80574 805 }
Ludwigfr 51:b863fad80574 806 pc.printf("\r\n X=%f; Y=%f", robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y());
Ludwigfr 51:b863fad80574 807 }
Ludwigfr 51:b863fad80574 808
Ludwigfr 51:b863fad80574 809 //compute the force on X and Y
Ludwigfr 51:b863fad80574 810 void compute_forceX_and_forceY(float* forceX, float* forceY){
Ludwigfr 51:b863fad80574 811 //we put the position of the robot in an orthonormal space
Ludwigfr 51:b863fad80574 812 float xRobotOrtho=robot_center_x_in_orthonormal_x();
Ludwigfr 51:b863fad80574 813 float yRobotOrtho=robot_center_y_in_orthonormal_y();
Ludwigfr 51:b863fad80574 814
Ludwigfr 51:b863fad80574 815 float forceRepulsionComputedX=0;
Ludwigfr 51:b863fad80574 816 float forceRepulsionComputedY=0;
Ludwigfr 51:b863fad80574 817 //for each cell of the map we compute a force of repulsion
Ludwigfr 51:b863fad80574 818 for(int i=0;i<NB_CELL_WIDTH;i++){
Ludwigfr 51:b863fad80574 819 for(int j=0;j<NB_CELL_HEIGHT;j++){
Ludwigfr 51:b863fad80574 820 update_force(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho);
Ludwigfr 51:b863fad80574 821 }
Ludwigfr 51:b863fad80574 822 }
Ludwigfr 51:b863fad80574 823 //update with attraction force
Ludwigfr 51:b863fad80574 824 *forceX=+forceRepulsionComputedX;
Ludwigfr 51:b863fad80574 825 *forceY=+forceRepulsionComputedY;
Ludwigfr 51:b863fad80574 826 float distanceTargetRobot=sqrt(pow(targetXOrhto-xRobotOrtho,2)+pow(targetYOrhto-yRobotOrtho,2));
Ludwigfr 51:b863fad80574 827 if(distanceTargetRobot != 0){
Ludwigfr 51:b863fad80574 828 *forceX-=FORCE_CONSTANT_ATTRACTION*(targetXOrhto-xRobotOrtho)/distanceTargetRobot;
Ludwigfr 51:b863fad80574 829 *forceY-=FORCE_CONSTANT_ATTRACTION*(targetYOrhto-yRobotOrtho)/distanceTargetRobot;
Ludwigfr 51:b863fad80574 830 }
Ludwigfr 51:b863fad80574 831 float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
Ludwigfr 51:b863fad80574 832 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
Ludwigfr 51:b863fad80574 833 *forceX=*forceX/amplitude;
Ludwigfr 51:b863fad80574 834 *forceY=*forceY/amplitude;
Ludwigfr 51:b863fad80574 835 }
Ludwigfr 51:b863fad80574 836 }
Ludwigfr 51:b863fad80574 837
Ludwigfr 51:b863fad80574 838 //x and y passed are TargetNotMap
Ludwigfr 51:b863fad80574 839 float get_error_angles(float x, float y,float theta){
Ludwigfr 51:b863fad80574 840 float angleBeetweenRobotAndTarget=atan2(y,x);
Ludwigfr 51:b863fad80574 841 if(y > 0){
Ludwigfr 51:b863fad80574 842 if(angleBeetweenRobotAndTarget < pi/2)//up right
Ludwigfr 51:b863fad80574 843 angleBeetweenRobotAndTarget=-pi/2+angleBeetweenRobotAndTarget;
Ludwigfr 51:b863fad80574 844 else//up right
Ludwigfr 51:b863fad80574 845 angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-pi/2;
Ludwigfr 51:b863fad80574 846 }else{
Ludwigfr 51:b863fad80574 847 if(angleBeetweenRobotAndTarget > -pi/2)//lower right
Ludwigfr 51:b863fad80574 848 angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-pi/2;
Ludwigfr 51:b863fad80574 849 else//lower left
Ludwigfr 51:b863fad80574 850 angleBeetweenRobotAndTarget=2*pi+angleBeetweenRobotAndTarget-pi/2;
Ludwigfr 51:b863fad80574 851 }
Ludwigfr 51:b863fad80574 852 return angleBeetweenRobotAndTarget-theta;
Ludwigfr 51:b863fad80574 853 }
Ludwigfr 51:b863fad80574 854
Ludwigfr 51:b863fad80574 855 void compute_angles_and_distance(float target_x, float target_y, float target_angle){
Ludwigfr 51:b863fad80574 856 alpha = atan2((target_y-Y),(target_x-X))-theta;
Ludwigfr 51:b863fad80574 857 alpha = atan(sin(alpha)/cos(alpha));
Ludwigfr 51:b863fad80574 858 rho = dist(X, Y, target_x, target_y);
Ludwigfr 51:b863fad80574 859 d2 = rho;
Ludwigfr 51:b863fad80574 860 beta = -alpha-theta+target_angle;
Ludwigfr 51:b863fad80574 861
Ludwigfr 51:b863fad80574 862 //Computing angle error and distance towards the target value
Ludwigfr 51:b863fad80574 863 rho += dt*(-KRHO*cos(alpha)*rho);
Ludwigfr 51:b863fad80574 864 temp = alpha;
Ludwigfr 51:b863fad80574 865 alpha += dt*(KRHO*sin(alpha)-KA*alpha-KB*beta);
Ludwigfr 51:b863fad80574 866 beta += dt*(-KRHO*sin(temp));
Ludwigfr 51:b863fad80574 867 //pc.printf("\n\r d2=%f", d2);
Ludwigfr 51:b863fad80574 868 //pc.printf("\n\r dt=%f", dt);
Ludwigfr 51:b863fad80574 869 }
Ludwigfr 51:b863fad80574 870
Ludwigfr 51:b863fad80574 871 void compute_linear_angular_velocities(){
Ludwigfr 51:b863fad80574 872 //Computing linear and angular velocities
Ludwigfr 51:b863fad80574 873 if(alpha>=-1.5708 && alpha<=1.5708){
Ludwigfr 51:b863fad80574 874 linear=KRHO*rho;
Ludwigfr 51:b863fad80574 875 angular=KA*alpha+KB*beta;
Ludwigfr 51:b863fad80574 876 }
Ludwigfr 51:b863fad80574 877 else{
Ludwigfr 51:b863fad80574 878 linear=-KRHO*rho;
Ludwigfr 51:b863fad80574 879 angular=-KA*alpha-KB*beta;
Ludwigfr 51:b863fad80574 880 }
Ludwigfr 51:b863fad80574 881 angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
Ludwigfr 51:b863fad80574 882 angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
Ludwigfr 51:b863fad80574 883
Ludwigfr 51:b863fad80574 884 //Normalize speed for motors
Ludwigfr 51:b863fad80574 885 if(angular_left>angular_right) {
Ludwigfr 51:b863fad80574 886 angular_right=MAX_SPEED*angular_right/angular_left;
Ludwigfr 51:b863fad80574 887 angular_left=MAX_SPEED;
Ludwigfr 51:b863fad80574 888 } else {
Ludwigfr 51:b863fad80574 889 angular_left=MAX_SPEED*angular_left/angular_right;
Ludwigfr 51:b863fad80574 890 angular_right=MAX_SPEED;
Ludwigfr 51:b863fad80574 891 }
Ludwigfr 51:b863fad80574 892 }
Ludwigfr 51:b863fad80574 893
Ludwigfr 51:b863fad80574 894 //go the the given position while updating the map
Ludwigfr 51:b863fad80574 895 void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
Ludwigfr 51:b863fad80574 896 Odometria();
Ludwigfr 51:b863fad80574 897 alpha = atan2((target_y-Y),(target_x-X))-theta;
Ludwigfr 51:b863fad80574 898 alpha = atan(sin(alpha)/cos(alpha));
Ludwigfr 51:b863fad80574 899 rho = dist(X, Y, target_x, target_y);
Ludwigfr 51:b863fad80574 900 beta = -alpha-theta+target_angle;
Ludwigfr 51:b863fad80574 901 //beta = atan(sin(beta)/cos(beta));
Ludwigfr 51:b863fad80574 902 bool keep_going=true;
Ludwigfr 51:b863fad80574 903 float leftMm;
Ludwigfr 51:b863fad80574 904 float frontMm;
Ludwigfr 51:b863fad80574 905 float rightMm;
Ludwigfr 51:b863fad80574 906 do {
Ludwigfr 51:b863fad80574 907 //Timer stuff
Ludwigfr 51:b863fad80574 908 dt = t.read();
Ludwigfr 51:b863fad80574 909 t.reset();
Ludwigfr 51:b863fad80574 910 t.start();
Ludwigfr 51:b863fad80574 911
Ludwigfr 51:b863fad80574 912 //Updating X,Y and theta with the odometry values
Ludwigfr 51:b863fad80574 913 Odometria();
Ludwigfr 51:b863fad80574 914 leftMm = get_distance_left_sensor();
Ludwigfr 51:b863fad80574 915 frontMm = get_distance_front_sensor();
Ludwigfr 51:b863fad80574 916 rightMm = get_distance_right_sensor();
Ludwigfr 51:b863fad80574 917
Ludwigfr 51:b863fad80574 918 //pc.printf("\n\r leftMm=%f", leftMm);
Ludwigfr 51:b863fad80574 919 //pc.printf("\n\r frontMm=%f", frontMm);
Ludwigfr 51:b863fad80574 920 //pc.printf("\n\r rightMm=%f", rightMm);
Ludwigfr 51:b863fad80574 921
Ludwigfr 51:b863fad80574 922 //if in dangerzone
Ludwigfr 51:b863fad80574 923 if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
Ludwigfr 51:b863fad80574 924 leftMotor(1,0);
Ludwigfr 51:b863fad80574 925 rightMotor(1,0);
Ludwigfr 51:b863fad80574 926 update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 51:b863fad80574 927 //TODO Giorgos maybe you can also test the do_half_flip() function
Ludwigfr 51:b863fad80574 928 Odometria();
Ludwigfr 51:b863fad80574 929 //do a flip TODO
Ludwigfr 51:b863fad80574 930 keep_going=false;
Ludwigfr 51:b863fad80574 931 do_half_flip();
Ludwigfr 51:b863fad80574 932 }else{
Ludwigfr 51:b863fad80574 933 //if not in danger zone continue as usual
Ludwigfr 51:b863fad80574 934 update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 51:b863fad80574 935 compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target
Ludwigfr 51:b863fad80574 936 compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular)
Ludwigfr 51:b863fad80574 937
Ludwigfr 51:b863fad80574 938 //pc.printf("\n\r X=%f", X);
Ludwigfr 51:b863fad80574 939 //pc.printf("\n\r Y=%f", Y);
Ludwigfr 51:b863fad80574 940
Ludwigfr 51:b863fad80574 941 //pc.printf("\n\r a_r=%f", angular_right);
Ludwigfr 51:b863fad80574 942 //pc.printf("\n\r a_l=%f", angular_left);
Ludwigfr 51:b863fad80574 943
Ludwigfr 51:b863fad80574 944 //Updating motor velocities
Ludwigfr 51:b863fad80574 945 leftMotor(1,angular_left);
Ludwigfr 51:b863fad80574 946 rightMotor(1,angular_right);
Ludwigfr 51:b863fad80574 947
Ludwigfr 51:b863fad80574 948 wait(0.2);
Ludwigfr 51:b863fad80574 949 //Timer stuff
Ludwigfr 51:b863fad80574 950 t.stop();
Ludwigfr 51:b863fad80574 951 pc.printf("\n\r dist to target= %f",d2);
Ludwigfr 51:b863fad80574 952 }
Ludwigfr 51:b863fad80574 953 } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going);
Ludwigfr 51:b863fad80574 954
Ludwigfr 51:b863fad80574 955 //Stop at the end
Ludwigfr 51:b863fad80574 956 leftMotor(1,0);
Ludwigfr 51:b863fad80574 957 rightMotor(1,0);
Ludwigfr 51:b863fad80574 958 }
Ludwigfr 51:b863fad80574 959
Ludwigfr 51:b863fad80574 960 //Updates sonar values
Ludwigfr 51:b863fad80574 961 void update_sonar_values(float leftMm, float frontMm, float rightMm){
Ludwigfr 51:b863fad80574 962 if(leftMm==0)
Ludwigfr 51:b863fad80574 963 pc.printf("\n\r leftMm==0");
Ludwigfr 51:b863fad80574 964 if(frontMm==0)
Ludwigfr 51:b863fad80574 965 pc.printf("\n\r frontMm==0");
Ludwigfr 51:b863fad80574 966 if(rightMm==0)
Ludwigfr 51:b863fad80574 967 pc.printf("\n\r rightMm==0");
Ludwigfr 51:b863fad80574 968 float currProba;
Ludwigfr 51:b863fad80574 969 float i_in_orthonormal;
Ludwigfr 51:b863fad80574 970 float j_in_orthonormal;
Ludwigfr 51:b863fad80574 971 for(int i=0;i<NB_CELL_WIDTH;i++){
Ludwigfr 51:b863fad80574 972 for(int j=0;j<NB_CELL_HEIGHT;j++){
Ludwigfr 51:b863fad80574 973 //check if the point A(x,y) in the world space is within the range of the sonar (which has the coordinates xs, ys in the world space)
Ludwigfr 51:b863fad80574 974 //check that again
Ludwigfr 51:b863fad80574 975 //compute for front sonar
Ludwigfr 51:b863fad80574 976 i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i);
Ludwigfr 51:b863fad80574 977 j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j);
Ludwigfr 51:b863fad80574 978
Ludwigfr 51:b863fad80574 979 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 51:b863fad80574 980 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 51:b863fad80574 981 //compute for right sonar
Ludwigfr 51:b863fad80574 982 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 51:b863fad80574 983 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
Ludwigfr 51:b863fad80574 984 //compute for left sonar
Ludwigfr 51:b863fad80574 985 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 51:b863fad80574 986 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
Ludwigfr 51:b863fad80574 987 }
Ludwigfr 51:b863fad80574 988 }
geotsam 24:8f4b820d8de8 989 }