Georgios Tsamis / Mbed 2 deprecated VirtualForces_debug

Navigate to a given point using the OGM and virtual forces

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

main.cpp@46:e57ebcf747dc, 2017-05-29 (annotated)

Committer:
geotsam
Date:
Mon May 29 17:13:42 2017 +0000
Revision:
46:e57ebcf747dc
Parent:
45:fb07065a64a9 
full of isnan() and other weird debugging stuff

Who changed what in which revision?

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