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@52:54b3fe68a4f2, 2017-06-08 (annotated)

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