Aurélien Bernier Levalois / Mbed 2 deprecated Mapping

Mapping for TP2

Dependencies:   ISR_Mini-explorer mbed

Fork of GoToPointWithAngle by Georgios Tsamis

main.cpp@23:901fc468b8a7, 2017-04-19 (annotated)

Committer:
geotsam
Date:
Wed Apr 19 10:45:09 2017 +0000
Revision:
23:901fc468b8a7
Parent:
21:ebb37a249b5f
Child:
24:8f4b820d8de8
cleaned up and shortened the go_to_point_with_angle method

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 21:ebb37a249b5f 4
Ludwigfr 21:ebb37a249b5f 5 using namespace std;
AurelienBernier 4:8c56c3ba6e54 6
Ludwigfr 21:ebb37a249b5f 7 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 21:ebb37a249b5f 8 void fill_initial_log_values();
Ludwigfr 21:ebb37a249b5f 9 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 21:ebb37a249b5f 10 void randomize_and_map();
Ludwigfr 21:ebb37a249b5f 11 //go the the given position while updating the map
Ludwigfr 21:ebb37a249b5f 12 void go_to_point_with_angle(float target_x, float target_y, float target_angle);
Ludwigfr 21:ebb37a249b5f 13 //Updates sonar values
Ludwigfr 21:ebb37a249b5f 14 void update_sonar_values();
Ludwigfr 21:ebb37a249b5f 15 //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 21:ebb37a249b5f 16 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
Ludwigfr 21:ebb37a249b5f 17 //print the map
Ludwigfr 21:ebb37a249b5f 18 void print_final_map();
geotsam 23:901fc468b8a7 19 //compute the angles and distance used for the velocities
geotsam 23:901fc468b8a7 20 void compute_angles_and_distance(float target_x, float target_y, float target_angle);
geotsam 23:901fc468b8a7 21 //compute the linear and 2 angular velocities
geotsam 23:901fc468b8a7 22 void compute_linear_angular_velocities();
geotsam 23:901fc468b8a7 23
geotsam 0:8bffb51cc345 24
Ludwigfr 21:ebb37a249b5f 25 //MATHS heavy functions
Ludwigfr 21:ebb37a249b5f 26 float dist(float robot_x, float robot_y, float target_x, float target_y);
Ludwigfr 21:ebb37a249b5f 27 //returns the probability [0,1] that the cell is occupied from the log value lt
Ludwigfr 21:ebb37a249b5f 28 float log_to_proba(float lt);
Ludwigfr 21:ebb37a249b5f 29 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 21:ebb37a249b5f 30 float proba_to_log(float p);
Ludwigfr 21:ebb37a249b5f 31 //returns the new log value
Ludwigfr 21:ebb37a249b5f 32 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue );
Ludwigfr 21:ebb37a249b5f 33 //makes the angle inAngle between 0 and 2pi
Ludwigfr 21:ebb37a249b5f 34 float rad_angle_check(float inAngle);
Ludwigfr 21:ebb37a249b5f 35 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 21:ebb37a249b5f 36 float compute_angle_between_vectors(float x, float y,float xs,float ys);
AurelienBernier 8:109314be5b68 37
Ludwigfr 21:ebb37a249b5f 38 const float pi=3.14159;
Ludwigfr 21:ebb37a249b5f 39 //spec of the sonar
Ludwigfr 21:ebb37a249b5f 40 //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
Ludwigfr 21:ebb37a249b5f 41 const float RANGE_SONAR=50;//RADIUS_WHEELS
Ludwigfr 21:ebb37a249b5f 42 const float RANGE_SONAR_MIN=10;//Rmin
Ludwigfr 21:ebb37a249b5f 43 const float INCERTITUDE_SONAR=10;
Ludwigfr 21:ebb37a249b5f 44 const float ANGLE_SONAR=pi/3;//Omega
Ludwigfr 21:ebb37a249b5f 45
Ludwigfr 21:ebb37a249b5f 46 //those distance and angle are approximation in need of measurements
Ludwigfr 21:ebb37a249b5f 47 const float ANGLE_FRONT_TO_LEFT=pi/3;
Ludwigfr 21:ebb37a249b5f 48 const float DISTANCE_SONAR_LEFT_X=5;
Ludwigfr 21:ebb37a249b5f 49 const float DISTANCE_SONAR_LEFT_Y=5;
Ludwigfr 21:ebb37a249b5f 50 float leftMm;
Ludwigfr 21:ebb37a249b5f 51
Ludwigfr 21:ebb37a249b5f 52 const float ANGLE_FRONT_TO_RIGHT=-pi/3;
Ludwigfr 21:ebb37a249b5f 53 const float DISTANCE_SONAR_RIGHT_X=-5;
Ludwigfr 21:ebb37a249b5f 54 const float DISTANCE_SONAR_RIGHT_Y=5;
Ludwigfr 21:ebb37a249b5f 55 float rightMm;
AurelienBernier 11:e641aa08c92e 56
Ludwigfr 21:ebb37a249b5f 57 const float ANGLE_FRONT_TO_FRONT=0;
Ludwigfr 21:ebb37a249b5f 58 const float DISTANCE_SONAR_FRONT_X=0;
Ludwigfr 21:ebb37a249b5f 59 const float DISTANCE_SONAR_FRONT_Y=5;
Ludwigfr 21:ebb37a249b5f 60 float frontMm;
Ludwigfr 21:ebb37a249b5f 61
Ludwigfr 21:ebb37a249b5f 62 //TODO adjust the size of the map for computation time (25*25?)
Ludwigfr 21:ebb37a249b5f 63 const int SIZE_MAP=25;
Ludwigfr 21:ebb37a249b5f 64
Ludwigfr 21:ebb37a249b5f 65 //position and orientation of the robot when put on the map (ODOMETRY doesn't know those)
Ludwigfr 21:ebb37a249b5f 66 const float DEFAULT_X=0;
Ludwigfr 21:ebb37a249b5f 67 const float DEFAULT_Y=0;
Ludwigfr 21:ebb37a249b5f 68 const float DEFAULT_THETA=pi/2;
Ludwigfr 21:ebb37a249b5f 69
Ludwigfr 21:ebb37a249b5f 70 //used to create the map 250 represent the 250cm of the table where the robot is tested
Ludwigfr 21:ebb37a249b5f 71 float sizeCell=250/(float)SIZE_MAP;
Ludwigfr 21:ebb37a249b5f 72 float map[SIZE_MAP][SIZE_MAP];//contains the log values for each cell
Ludwigfr 21:ebb37a249b5f 73 float initialLogValues[SIZE_MAP][SIZE_MAP];
Ludwigfr 21:ebb37a249b5f 74
Ludwigfr 21:ebb37a249b5f 75 //Diameter of a wheel and distance between the 2
Ludwigfr 21:ebb37a249b5f 76 const float RADIUS_WHEELS=3.25;
Ludwigfr 21:ebb37a249b5f 77 const float DISTANCE_WHEELS=7.2;
Ludwigfr 21:ebb37a249b5f 78
AurelienBernier 8:109314be5b68 79 float alpha; //angle error
AurelienBernier 8:109314be5b68 80 float rho; //distance from target
AurelienBernier 8:109314be5b68 81 float beta;
AurelienBernier 11:e641aa08c92e 82 float kRho=12, ka=30, kb=-13; //Kappa values
AurelienBernier 8:109314be5b68 83 float linear, angular, angular_left, angular_right;
AurelienBernier 8:109314be5b68 84 float dt=0.5;
AurelienBernier 8:109314be5b68 85 float temp;
AurelienBernier 8:109314be5b68 86 float d2;
Ludwigfr 21:ebb37a249b5f 87 Timer t;
AurelienBernier 8:109314be5b68 88
AurelienBernier 8:109314be5b68 89 int speed=999; // Max speed at beggining of movement
AurelienBernier 8:109314be5b68 90
geotsam 0:8bffb51cc345 91 int main(){
geotsam 17:caf393b63e27 92
geotsam 13:41f75c132135 93 i2c1.frequency(100000);
AurelienBernier 2:ea61e801e81f 94 initRobot(); //Initializing the robot
geotsam 0:8bffb51cc345 95 pc.baud(9600); // baud for the pc communication
geotsam 0:8bffb51cc345 96
Ludwigfr 21:ebb37a249b5f 97 measure_always_on();//TODO check if needed
AurelienBernier 18:dbc5fbad4975 98
Ludwigfr 21:ebb37a249b5f 99 fill_initial_log_values();
geotsam 13:41f75c132135 100
AurelienBernier 20:62154d644531 101 for (int i = 0; i<25; i++) {
Ludwigfr 21:ebb37a249b5f 102 randomize_and_map();
geotsam 17:caf393b63e27 103 }
AurelienBernier 8:109314be5b68 104 //Stop at the end
AurelienBernier 8:109314be5b68 105 leftMotor(1,0);
AurelienBernier 8:109314be5b68 106 rightMotor(1,0);
AurelienBernier 8:109314be5b68 107
AurelienBernier 20:62154d644531 108 //pc.printf("\n\r %f -- arrived!", rho);
Ludwigfr 21:ebb37a249b5f 109 print_final_map();
AurelienBernier 8:109314be5b68 110 }
AurelienBernier 8:109314be5b68 111
Ludwigfr 21:ebb37a249b5f 112 //fill initialLogValues with the values we already know (here the bordurs)
Ludwigfr 21:ebb37a249b5f 113 void fill_initial_log_values(){
Ludwigfr 21:ebb37a249b5f 114 //Fill map, we know the border are occupied
Ludwigfr 21:ebb37a249b5f 115 for (int i = 0; i<SIZE_MAP; i++) {
Ludwigfr 21:ebb37a249b5f 116 for (int j = 0; j<SIZE_MAP; j++) {
Ludwigfr 21:ebb37a249b5f 117 if(j==0 || j==SIZE_MAP-1 || i==0 || i==SIZE_MAP-1)
Ludwigfr 21:ebb37a249b5f 118 initialLogValues[i][j] = proba_to_log(1);
Ludwigfr 21:ebb37a249b5f 119 else
Ludwigfr 21:ebb37a249b5f 120 initialLogValues[i][j] = proba_to_log(0.5);
AurelienBernier 20:62154d644531 121 }
Ludwigfr 21:ebb37a249b5f 122 }
AurelienBernier 8:109314be5b68 123 }
AurelienBernier 8:109314be5b68 124
Ludwigfr 21:ebb37a249b5f 125 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 21:ebb37a249b5f 126 void randomize_and_map() {
Ludwigfr 21:ebb37a249b5f 127 float target_x = (rand()%2500)/10;//for decimal precision
Ludwigfr 21:ebb37a249b5f 128 float target_y = (rand()%2500)/10;
Ludwigfr 21:ebb37a249b5f 129 float target_angle = ((float)(rand()%31416)-15708)/10000.0;
Ludwigfr 21:ebb37a249b5f 130 /*
AurelienBernier 18:dbc5fbad4975 131 pc.printf("\n\r targ_X=%f", target_x);
AurelienBernier 18:dbc5fbad4975 132 pc.printf("\n\r targ_Y=%f", target_y);
AurelienBernier 18:dbc5fbad4975 133 pc.printf("\n\r targ_Angle=%f", target_angle);
Ludwigfr 21:ebb37a249b5f 134 */
Ludwigfr 21:ebb37a249b5f 135 go_to_point_with_angle(target_x, target_y, target_angle);
AurelienBernier 18:dbc5fbad4975 136 }
AurelienBernier 18:dbc5fbad4975 137
Ludwigfr 21:ebb37a249b5f 138 //go the the given position while updating the map
Ludwigfr 21:ebb37a249b5f 139 //TODO clean this procedure it's ugly as hell and too long
Ludwigfr 21:ebb37a249b5f 140 void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
geotsam 23:901fc468b8a7 141 bool tooClose=false; //binary variable to know if an obstacle is <10cm away
AurelienBernier 8:109314be5b68 142 do {
geotsam 0:8bffb51cc345 143 pc.printf("\n\n\r entered while");
AurelienBernier 2:ea61e801e81f 144
AurelienBernier 6:afde4b08166b 145 //Timer stuff
AurelienBernier 6:afde4b08166b 146 dt = t.read();
AurelienBernier 6:afde4b08166b 147 t.reset();
AurelienBernier 6:afde4b08166b 148 t.start();
AurelienBernier 6:afde4b08166b 149
geotsam 15:d58f2bdbf42e 150 //Updating X,Y and theta with the odometry values
geotsam 15:d58f2bdbf42e 151 Odometria();
geotsam 15:d58f2bdbf42e 152
geotsam 23:901fc468b8a7 153 //Updare the values from the sonar
Ludwigfr 21:ebb37a249b5f 154 update_sonar_values();
Ludwigfr 21:ebb37a249b5f 155
geotsam 23:901fc468b8a7 156 //Check if one of the sonars finds an obstacle that is too close
AurelienBernier 11:e641aa08c92e 157 if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
AurelienBernier 20:62154d644531 158 tooClose = true;
geotsam 15:d58f2bdbf42e 159 break;
AurelienBernier 11:e641aa08c92e 160 }
AurelienBernier 11:e641aa08c92e 161
geotsam 23:901fc468b8a7 162 compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target
geotsam 23:901fc468b8a7 163 compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular)
geotsam 0:8bffb51cc345 164
geotsam 23:901fc468b8a7 165 //Printing values for debugging purposes
geotsam 0:8bffb51cc345 166 pc.printf("\n\r X=%f", X);
geotsam 0:8bffb51cc345 167 pc.printf("\n\r Y=%f", Y);
AurelienBernier 14:44ab4626f1ad 168 pc.printf("\n\r leftMm=%f", leftMm);
AurelienBernier 14:44ab4626f1ad 169 pc.printf("\n\r frontMm=%f", frontMm);
AurelienBernier 14:44ab4626f1ad 170 pc.printf("\n\r rightMm=%f", rightMm);
geotsam 0:8bffb51cc345 171
AurelienBernier 2:ea61e801e81f 172 //Updating motor velocities
AurelienBernier 1:f0807d5c5a4b 173 leftMotor(1,angular_left);
AurelienBernier 1:f0807d5c5a4b 174 rightMotor(1,angular_right);
geotsam 0:8bffb51cc345 175
AurelienBernier 7:c94070f9af78 176 wait(0.2);
AurelienBernier 6:afde4b08166b 177 //Timer stuff
AurelienBernier 6:afde4b08166b 178 t.stop();
Ludwigfr 21:ebb37a249b5f 179 } while(d2>1 && tooClose==false);
Ludwigfr 21:ebb37a249b5f 180 }
Ludwigfr 21:ebb37a249b5f 181
Ludwigfr 21:ebb37a249b5f 182 //Updates sonar values
Ludwigfr 21:ebb37a249b5f 183 void update_sonar_values(){
Ludwigfr 21:ebb37a249b5f 184
Ludwigfr 21:ebb37a249b5f 185 float leftMm = get_distance_left_sensor();
Ludwigfr 21:ebb37a249b5f 186 float frontMm = get_distance_front_sensor();
Ludwigfr 21:ebb37a249b5f 187 float rightMm = get_distance_right_sensor();
Ludwigfr 21:ebb37a249b5f 188
Ludwigfr 21:ebb37a249b5f 189 float currProba;
Ludwigfr 21:ebb37a249b5f 190 for(int i=0;i<SIZE_MAP;i++){
Ludwigfr 21:ebb37a249b5f 191 for(int j=0;j<SIZE_MAP;j++){
Ludwigfr 21:ebb37a249b5f 192 //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)
Ludwigfr 21:ebb37a249b5f 193
Ludwigfr 21:ebb37a249b5f 194 //compute for front sonar
Ludwigfr 21:ebb37a249b5f 195 currProba=compute_probability_t(sizeCell/2+i*sizeCell,sizeCell/2+j*sizeCell,SIZE_MAP*sizeCell-X+DEFAULT_X+DISTANCE_SONAR_FRONT_X,Y+DEFAULT_Y+DISTANCE_SONAR_FRONT_Y,ANGLE_FRONT_TO_FRONT,frontMm/10);
Ludwigfr 21:ebb37a249b5f 196 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 21:ebb37a249b5f 197 //compute for right sonar
Ludwigfr 21:ebb37a249b5f 198 currProba=compute_probability_t(sizeCell/2+i*sizeCell,sizeCell/2+j*sizeCell,SIZE_MAP*sizeCell-X+DEFAULT_X+DISTANCE_SONAR_RIGHT_X,Y+DEFAULT_Y+DISTANCE_SONAR_RIGHT_Y,ANGLE_FRONT_TO_RIGHT,rightMm/10);
Ludwigfr 21:ebb37a249b5f 199 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
Ludwigfr 21:ebb37a249b5f 200 //compute for left sonar
Ludwigfr 21:ebb37a249b5f 201 currProba=compute_probability_t(sizeCell/2+i*sizeCell,sizeCell/2+j*sizeCell,SIZE_MAP*sizeCell-X+DEFAULT_X+DISTANCE_SONAR_LEFT_X,Y+DEFAULT_Y+DISTANCE_SONAR_LEFT_Y,ANGLE_FRONT_TO_LEFT,leftMm/10);
Ludwigfr 21:ebb37a249b5f 202 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
Ludwigfr 21:ebb37a249b5f 203 }
Ludwigfr 21:ebb37a249b5f 204 }
Ludwigfr 21:ebb37a249b5f 205 }
Ludwigfr 21:ebb37a249b5f 206
Ludwigfr 21:ebb37a249b5f 207 //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 21:ebb37a249b5f 208 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){
Ludwigfr 21:ebb37a249b5f 209
Ludwigfr 21:ebb37a249b5f 210 float alpha=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
Ludwigfr 21:ebb37a249b5f 211 float alphaBeforeAdjustment=alpha-theta-DEFAULT_THETA-angleFromSonarPosition;
Ludwigfr 21:ebb37a249b5f 212 alpha=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
Ludwigfr 21:ebb37a249b5f 213 float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
Ludwigfr 21:ebb37a249b5f 214
Ludwigfr 21:ebb37a249b5f 215 //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
Ludwigfr 21:ebb37a249b5f 216 //check if absolute difference between the angles is no more than Omega/2
Ludwigfr 21:ebb37a249b5f 217 if( distancePointToSonar < (RANGE_SONAR)&& (alpha <= ANGLE_SONAR/2 || alpha >= rad_angle_check(-ANGLE_SONAR/2))){
Ludwigfr 21:ebb37a249b5f 218 if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
Ludwigfr 21:ebb37a249b5f 219 //point before obstacle, probably empty
Ludwigfr 21:ebb37a249b5f 220 /*****************************************************************************/
Ludwigfr 21:ebb37a249b5f 221 float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 21:ebb37a249b5f 222 float Er;
Ludwigfr 21:ebb37a249b5f 223 if(distancePointToSonar < RANGE_SONAR_MIN){
Ludwigfr 21:ebb37a249b5f 224 //point before minimum sonar range
Ludwigfr 21:ebb37a249b5f 225 Er=0.f;
Ludwigfr 21:ebb37a249b5f 226 }else{
Ludwigfr 21:ebb37a249b5f 227 //point after minimum sonar range
Ludwigfr 21:ebb37a249b5f 228 Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
Ludwigfr 21:ebb37a249b5f 229 }
Ludwigfr 21:ebb37a249b5f 230 /*****************************************************************************/
Ludwigfr 21:ebb37a249b5f 231 return (1.f-Er*Ea)/2.f;
Ludwigfr 21:ebb37a249b5f 232 }else{
Ludwigfr 21:ebb37a249b5f 233 //probably occupied
Ludwigfr 21:ebb37a249b5f 234 /*****************************************************************************/
Ludwigfr 21:ebb37a249b5f 235 float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
Ludwigfr 21:ebb37a249b5f 236 float Or;
Ludwigfr 21:ebb37a249b5f 237 if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
Ludwigfr 21:ebb37a249b5f 238 //point between distanceObstacleDetected +- INCERTITUDE_SONAR
Ludwigfr 21:ebb37a249b5f 239 Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
Ludwigfr 21:ebb37a249b5f 240 }else{
Ludwigfr 21:ebb37a249b5f 241 //point after in range of the sonar but after the zone detected
Ludwigfr 21:ebb37a249b5f 242 Or=0;
Ludwigfr 21:ebb37a249b5f 243 }
Ludwigfr 21:ebb37a249b5f 244 /*****************************************************************************/
Ludwigfr 21:ebb37a249b5f 245 return (1+Or*Oa)/2;
Ludwigfr 21:ebb37a249b5f 246 }
Ludwigfr 21:ebb37a249b5f 247 }else{
Ludwigfr 21:ebb37a249b5f 248 //not in range of the sonar
Ludwigfr 21:ebb37a249b5f 249 return 0.5;
AurelienBernier 18:dbc5fbad4975 250 }
Ludwigfr 21:ebb37a249b5f 251 }
Ludwigfr 21:ebb37a249b5f 252
Ludwigfr 21:ebb37a249b5f 253 void print_final_map() {
Ludwigfr 21:ebb37a249b5f 254 float currProba;
Ludwigfr 21:ebb37a249b5f 255 for (int x = 0; x<SIZE_MAP; x++) {
Ludwigfr 21:ebb37a249b5f 256 for (int y = 0; y<SIZE_MAP; y++) {
Ludwigfr 21:ebb37a249b5f 257 currProba=log_to_proba(map[SIZE_MAP-1-x][y]);
Ludwigfr 21:ebb37a249b5f 258 if ( currProba < 0.5) {//by default [0][0] would be top left, we want it bottom left
Ludwigfr 21:ebb37a249b5f 259 pc.printf("0");
Ludwigfr 21:ebb37a249b5f 260 } else {
Ludwigfr 21:ebb37a249b5f 261 if(currProba==0.5)
Ludwigfr 21:ebb37a249b5f 262 pc.printf(".");
Ludwigfr 21:ebb37a249b5f 263 else
Ludwigfr 21:ebb37a249b5f 264 pc.printf("+");
Ludwigfr 21:ebb37a249b5f 265 }
Ludwigfr 21:ebb37a249b5f 266 }
Ludwigfr 21:ebb37a249b5f 267 }
Ludwigfr 21:ebb37a249b5f 268 }
Ludwigfr 21:ebb37a249b5f 269
Ludwigfr 21:ebb37a249b5f 270
Ludwigfr 21:ebb37a249b5f 271
Ludwigfr 21:ebb37a249b5f 272
Ludwigfr 21:ebb37a249b5f 273 //MATHS heavy functions
Ludwigfr 21:ebb37a249b5f 274 /**********************************************************************/
Ludwigfr 21:ebb37a249b5f 275 //Distance computation function
Ludwigfr 21:ebb37a249b5f 276 float dist(float robot_x, float robot_y, float target_x, float target_y){
Ludwigfr 21:ebb37a249b5f 277 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
Ludwigfr 21:ebb37a249b5f 278 }
Ludwigfr 21:ebb37a249b5f 279
Ludwigfr 21:ebb37a249b5f 280 //returns the probability [0,1] that the cell is occupied from the log value lt
Ludwigfr 21:ebb37a249b5f 281 float log_to_proba(float lt){
Ludwigfr 21:ebb37a249b5f 282 return 1-1/(1+exp(lt));
Ludwigfr 21:ebb37a249b5f 283 }
Ludwigfr 21:ebb37a249b5f 284
Ludwigfr 21:ebb37a249b5f 285 //returns the log value that the cell is occupied from the probability value [0,1]
Ludwigfr 21:ebb37a249b5f 286 float proba_to_log(float p){
Ludwigfr 21:ebb37a249b5f 287 return log(p/(1-p));
Ludwigfr 21:ebb37a249b5f 288 }
Ludwigfr 21:ebb37a249b5f 289
Ludwigfr 21:ebb37a249b5f 290 //returns the new log value
Ludwigfr 21:ebb37a249b5f 291 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){
Ludwigfr 21:ebb37a249b5f 292 return previousLogValue+proba_to_log(currentProbability)-originalLogvalue;
Ludwigfr 21:ebb37a249b5f 293 }
Ludwigfr 21:ebb37a249b5f 294
Ludwigfr 21:ebb37a249b5f 295 //makes the angle inAngle between 0 and 2pi
Ludwigfr 21:ebb37a249b5f 296 float rad_angle_check(float inAngle){
Ludwigfr 21:ebb37a249b5f 297 //cout<<"before :"<<inAngle;
Ludwigfr 21:ebb37a249b5f 298 if(inAngle > 0){
Ludwigfr 21:ebb37a249b5f 299 while(inAngle > (2*pi))
Ludwigfr 21:ebb37a249b5f 300 inAngle-=2*pi;
Ludwigfr 21:ebb37a249b5f 301 }else{
Ludwigfr 21:ebb37a249b5f 302 while(inAngle < 0)
Ludwigfr 21:ebb37a249b5f 303 inAngle+=2*pi;
Ludwigfr 21:ebb37a249b5f 304 }
Ludwigfr 21:ebb37a249b5f 305 //cout<<" after :"<<inAngle<<endl;
Ludwigfr 21:ebb37a249b5f 306 return inAngle;
Ludwigfr 21:ebb37a249b5f 307 }
Ludwigfr 21:ebb37a249b5f 308
Ludwigfr 21:ebb37a249b5f 309 //returns the angle between the vectors (x,y) and (xs,ys)
Ludwigfr 21:ebb37a249b5f 310 float compute_angle_between_vectors(float x, float y,float xs,float ys){
Ludwigfr 21:ebb37a249b5f 311 //alpha angle between ->x and ->SA
Ludwigfr 21:ebb37a249b5f 312 //vector S to A ->SA
Ludwigfr 21:ebb37a249b5f 313 float vSAx=x-xs;
Ludwigfr 21:ebb37a249b5f 314 float vSAy=y-ys;
Ludwigfr 21:ebb37a249b5f 315 //norme SA
Ludwigfr 21:ebb37a249b5f 316 float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
Ludwigfr 21:ebb37a249b5f 317 //vector ->x (1,0)
Ludwigfr 21:ebb37a249b5f 318 float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
Ludwigfr 21:ebb37a249b5f 319 //vector ->y (0,1)
Ludwigfr 21:ebb37a249b5f 320 float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
Ludwigfr 21:ebb37a249b5f 321 if (sinAlpha < 0)
Ludwigfr 21:ebb37a249b5f 322 return -acos(cosAlpha);
Ludwigfr 21:ebb37a249b5f 323 else
Ludwigfr 21:ebb37a249b5f 324 return acos(cosAlpha);
Ludwigfr 21:ebb37a249b5f 325 }
Ludwigfr 21:ebb37a249b5f 326
geotsam 23:901fc468b8a7 327 void compute_angles_and_distance(float target_x, float target_y, float target_angle){
geotsam 23:901fc468b8a7 328 alpha = atan2((target_y-Y),(target_x-X))-theta;
geotsam 23:901fc468b8a7 329 alpha = atan(sin(alpha)/cos(alpha));
geotsam 23:901fc468b8a7 330 rho = dist(X, Y, target_x, target_y);
geotsam 23:901fc468b8a7 331 d2 = rho;
geotsam 23:901fc468b8a7 332 beta = -alpha-theta+target_angle;
geotsam 23:901fc468b8a7 333
geotsam 23:901fc468b8a7 334 //Computing angle error and distance towards the target value
geotsam 23:901fc468b8a7 335 rho += dt*(-kRho*cos(alpha)*rho);
geotsam 23:901fc468b8a7 336 temp = alpha;
geotsam 23:901fc468b8a7 337 alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
geotsam 23:901fc468b8a7 338 beta += dt*(-kRho*sin(temp));
geotsam 23:901fc468b8a7 339 pc.printf("\n\r d2=%f", d2);
geotsam 23:901fc468b8a7 340 pc.printf("\n\r dt=%f", dt);
geotsam 23:901fc468b8a7 341 }
geotsam 23:901fc468b8a7 342
geotsam 23:901fc468b8a7 343 void compute_linear_angular_velocities(){
geotsam 23:901fc468b8a7 344 //Computing linear and angular velocities
geotsam 23:901fc468b8a7 345 if(alpha>=-1.5708 && alpha<=1.5708){
geotsam 23:901fc468b8a7 346 linear=kRho*rho;
geotsam 23:901fc468b8a7 347 angular=ka*alpha+kb*beta;
geotsam 23:901fc468b8a7 348 }
geotsam 23:901fc468b8a7 349 else{
geotsam 23:901fc468b8a7 350 linear=-kRho*rho;
geotsam 23:901fc468b8a7 351 angular=-ka*alpha-kb*beta;
geotsam 23:901fc468b8a7 352 }
geotsam 23:901fc468b8a7 353 angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
geotsam 23:901fc468b8a7 354 angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
geotsam 23:901fc468b8a7 355
geotsam 23:901fc468b8a7 356 //Slowing down at the end for more precision
geotsam 23:901fc468b8a7 357 if (d2<25) {
geotsam 23:901fc468b8a7 358 speed = d2*30;
geotsam 23:901fc468b8a7 359 }
geotsam 23:901fc468b8a7 360
geotsam 23:901fc468b8a7 361 //Normalize speed for motors
geotsam 23:901fc468b8a7 362 if(angular_left>angular_right) {
geotsam 23:901fc468b8a7 363 angular_right=speed*angular_right/angular_left;
geotsam 23:901fc468b8a7 364 angular_left=speed;
geotsam 23:901fc468b8a7 365 } else {
geotsam 23:901fc468b8a7 366 angular_left=speed*angular_left/angular_right;
geotsam 23:901fc468b8a7 367 angular_right=speed;
geotsam 23:901fc468b8a7 368 }
geotsam 23:901fc468b8a7 369 }