Georgios Tsamis / Mbed 2 deprecated OneFileToRuleThemAll

All the lab works are here!

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

main.cpp@39:dd8326ec75ce, 2017-05-18 (annotated)

Committer:
Ludwigfr
Date:
Thu May 18 15:06:25 2017 +0000
Revision:
39:dd8326ec75ce
Parent:
38:3c9f8cbf5250
Child:
40:f5e212d9f900
fixed a bunch of issues concerning the target, the robot is still going on the right for some reason, maybe check the line computation

Who changed what in which revision?

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