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@50:a970cf7889d3, 2017-05-30 (annotated)

Committer:
Ludwigfr
Date:
Tue May 30 17:18:55 2017 +0000
Revision:
50:a970cf7889d3
Parent:
49:d61da2bc8882
Child:
51:b863fad80574
some tweaks

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