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