Mapping for TP2
Dependencies: ISR_Mini-explorer mbed
Fork of GoToPointWithAngle by
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?
User | Revision | Line number | New 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 | } |