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@43:ffd5a6d4dd48, 2017-05-22 (annotated)

Committer:
Ludwigfr
Date:
Mon May 22 18:20:08 2017 +0000
Revision:
43:ffd5a6d4dd48
Parent:
42:ab25bffdc32b
Child:
44:e2bd06f94dc0
carefull the + and minus are inverted, the khapas must be adjusted

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