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@35:c8f224ab153f, 2017-05-15 (annotated)

Committer:
Ludwigfr
Date:
Mon May 15 15:49:26 2017 +0000
Revision:
35:c8f224ab153f
Parent:
34:128fc7aed957
Child:
36:c806c568720a
just tweaked staring position and target;

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