All the lab works are here!

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

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