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