Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
main.cpp@25:572c9e9a8809, 2017-04-20 (annotated)
- Committer:
- Ludwigfr
- Date:
- Thu Apr 20 15:02:28 2017 +0000
- Revision:
- 25:572c9e9a8809
- Parent:
- 24:8f4b820d8de8
- Child:
- 26:b020cf253059
fixed a few things, and I did print_final_map_with_robot if you want to check where odometria thinks the robot is in the map
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 | 22:ebb37a249b5f | 4 | |
Ludwigfr | 22:ebb37a249b5f | 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(); |
Ludwigfr | 22:ebb37a249b5f | 11 | //go the the given position while updating the map |
Ludwigfr | 22:ebb37a249b5f | 12 | void go_to_point_with_angle(float target_x, float target_y, float target_angle); |
Ludwigfr | 22:ebb37a249b5f | 13 | //Updates sonar values |
geotsam | 24:8f4b820d8de8 | 14 | void update_sonar_values(float leftMm, float frontMm, float rightMm); |
Ludwigfr | 22: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 | 22:ebb37a249b5f | 16 | float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected); |
Ludwigfr | 22:ebb37a249b5f | 17 | //print the map |
Ludwigfr | 22:ebb37a249b5f | 18 | void print_final_map(); |
Ludwigfr | 25:572c9e9a8809 | 19 | //print the map with the robot marked on it |
Ludwigfr | 25:572c9e9a8809 | 20 | void print_final_map_with_robot_position(); |
geotsam | 0:8bffb51cc345 | 21 | |
Ludwigfr | 22:ebb37a249b5f | 22 | //MATHS heavy functions |
Ludwigfr | 22:ebb37a249b5f | 23 | float dist(float robot_x, float robot_y, float target_x, float target_y); |
Ludwigfr | 22:ebb37a249b5f | 24 | //returns the probability [0,1] that the cell is occupied from the log value lt |
Ludwigfr | 22:ebb37a249b5f | 25 | float log_to_proba(float lt); |
Ludwigfr | 22:ebb37a249b5f | 26 | //returns the log value that the cell is occupied from the probability value [0,1] |
Ludwigfr | 22:ebb37a249b5f | 27 | float proba_to_log(float p); |
Ludwigfr | 22:ebb37a249b5f | 28 | //returns the new log value |
Ludwigfr | 22:ebb37a249b5f | 29 | float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ); |
Ludwigfr | 22:ebb37a249b5f | 30 | //makes the angle inAngle between 0 and 2pi |
Ludwigfr | 22:ebb37a249b5f | 31 | float rad_angle_check(float inAngle); |
Ludwigfr | 22:ebb37a249b5f | 32 | //returns the angle between the vectors (x,y) and (xs,ys) |
Ludwigfr | 22:ebb37a249b5f | 33 | float compute_angle_between_vectors(float x, float y,float xs,float ys); |
Ludwigfr | 25:572c9e9a8809 | 34 | float robot_center_x_in_orthonormal_x(); |
Ludwigfr | 25:572c9e9a8809 | 35 | float robot_center_y_in_orthonormal_y(); |
Ludwigfr | 25:572c9e9a8809 | 36 | float robot_sonar_front_x_in_orthonormal_x(); |
Ludwigfr | 25:572c9e9a8809 | 37 | float robot_sonar_front_y_in_orthonormal_y(); |
Ludwigfr | 25:572c9e9a8809 | 38 | float robot_sonar_right_x_in_orthonormal_x(); |
Ludwigfr | 25:572c9e9a8809 | 39 | float robot_sonar_right_y_in_orthonormal_y(); |
Ludwigfr | 25:572c9e9a8809 | 40 | float robot_sonar_left_x_in_orthonormal_x(); |
Ludwigfr | 25:572c9e9a8809 | 41 | float robot_sonar_left_y_in_orthonormal_y(); |
Ludwigfr | 25:572c9e9a8809 | 42 | float estimated_width_indice_in_orthonormal_x(int i); |
Ludwigfr | 25:572c9e9a8809 | 43 | float estimated_height_indice_in_orthonormal_y(int j); |
AurelienBernier | 8:109314be5b68 | 44 | |
Ludwigfr | 22:ebb37a249b5f | 45 | const float pi=3.14159; |
Ludwigfr | 22:ebb37a249b5f | 46 | //spec of the sonar |
Ludwigfr | 22:ebb37a249b5f | 47 | //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 | 48 | const float RANGE_SONAR=50;//cm |
geotsam | 24:8f4b820d8de8 | 49 | const float RANGE_SONAR_MIN=10;//Rmin cm |
geotsam | 24:8f4b820d8de8 | 50 | const float INCERTITUDE_SONAR=10;//cm |
geotsam | 24:8f4b820d8de8 | 51 | const float ANGLE_SONAR=pi/3;//Omega rad |
geotsam | 24:8f4b820d8de8 | 52 | const float SECURITY_DISTANCE=150;//mm |
Ludwigfr | 22:ebb37a249b5f | 53 | |
Ludwigfr | 22:ebb37a249b5f | 54 | //those distance and angle are approximation in need of measurements |
geotsam | 24:8f4b820d8de8 | 55 | const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees |
geotsam | 24:8f4b820d8de8 | 56 | const float DISTANCE_SONAR_LEFT_X=4; |
geotsam | 24:8f4b820d8de8 | 57 | const float DISTANCE_SONAR_LEFT_Y=4; |
Ludwigfr | 22:ebb37a249b5f | 58 | |
geotsam | 24:8f4b820d8de8 | 59 | const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees |
geotsam | 24:8f4b820d8de8 | 60 | const float DISTANCE_SONAR_RIGHT_X=-4; |
geotsam | 24:8f4b820d8de8 | 61 | const float DISTANCE_SONAR_RIGHT_Y=4; |
AurelienBernier | 11:e641aa08c92e | 62 | |
Ludwigfr | 22:ebb37a249b5f | 63 | const float ANGLE_FRONT_TO_FRONT=0; |
Ludwigfr | 22:ebb37a249b5f | 64 | const float DISTANCE_SONAR_FRONT_X=0; |
Ludwigfr | 22:ebb37a249b5f | 65 | const float DISTANCE_SONAR_FRONT_Y=5; |
Ludwigfr | 22:ebb37a249b5f | 66 | |
Ludwigfr | 22:ebb37a249b5f | 67 | //TODO adjust the size of the map for computation time (25*25?) |
geotsam | 24:8f4b820d8de8 | 68 | const float WIDTH_ARENA=120;//cm |
geotsam | 24:8f4b820d8de8 | 69 | const float HEIGHT_ARENA=90;//cm |
geotsam | 24:8f4b820d8de8 | 70 | |
geotsam | 24:8f4b820d8de8 | 71 | //const int SIZE_MAP=25; |
geotsam | 24:8f4b820d8de8 | 72 | const int NB_CELL_WIDTH=24; |
geotsam | 24:8f4b820d8de8 | 73 | const int NB_CELL_HEIGHT=18; |
Ludwigfr | 22:ebb37a249b5f | 74 | |
Ludwigfr | 22:ebb37a249b5f | 75 | //position and orientation of the robot when put on the map (ODOMETRY doesn't know those) |
geotsam | 24:8f4b820d8de8 | 76 | const float DEFAULT_X=WIDTH_ARENA/2; |
geotsam | 24:8f4b820d8de8 | 77 | const float DEFAULT_Y=HEIGHT_ARENA/2; |
Ludwigfr | 22:ebb37a249b5f | 78 | const float DEFAULT_THETA=pi/2; |
Ludwigfr | 22:ebb37a249b5f | 79 | |
geotsam | 24:8f4b820d8de8 | 80 | //used to create the map 250 represent the 250cm of the square where the robot is tested |
geotsam | 24:8f4b820d8de8 | 81 | //float sizeCell=250/(float)SIZE_MAP; |
geotsam | 24:8f4b820d8de8 | 82 | float sizeCellX=WIDTH_ARENA/(float)NB_CELL_WIDTH; |
geotsam | 24:8f4b820d8de8 | 83 | float sizeCellY=HEIGHT_ARENA/(float)NB_CELL_HEIGHT; |
geotsam | 24:8f4b820d8de8 | 84 | |
geotsam | 24:8f4b820d8de8 | 85 | float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell |
geotsam | 24:8f4b820d8de8 | 86 | float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT]; |
Ludwigfr | 22:ebb37a249b5f | 87 | |
Ludwigfr | 22:ebb37a249b5f | 88 | //Diameter of a wheel and distance between the 2 |
Ludwigfr | 22:ebb37a249b5f | 89 | const float RADIUS_WHEELS=3.25; |
Ludwigfr | 22:ebb37a249b5f | 90 | const float DISTANCE_WHEELS=7.2; |
Ludwigfr | 22:ebb37a249b5f | 91 | |
geotsam | 24:8f4b820d8de8 | 92 | const int MAX_SPEED=500;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP |
AurelienBernier | 8:109314be5b68 | 93 | |
geotsam | 24:8f4b820d8de8 | 94 | bool tooClose=false; |
geotsam | 24:8f4b820d8de8 | 95 | bool turnFromObstacle=false; |
AurelienBernier | 8:109314be5b68 | 96 | |
geotsam | 0:8bffb51cc345 | 97 | int main(){ |
geotsam | 17:caf393b63e27 | 98 | |
geotsam | 13:41f75c132135 | 99 | i2c1.frequency(100000); |
AurelienBernier | 2:ea61e801e81f | 100 | initRobot(); //Initializing the robot |
geotsam | 0:8bffb51cc345 | 101 | pc.baud(9600); // baud for the pc communication |
geotsam | 0:8bffb51cc345 | 102 | |
Ludwigfr | 22:ebb37a249b5f | 103 | measure_always_on();//TODO check if needed |
geotsam | 24:8f4b820d8de8 | 104 | wait(0.5); |
AurelienBernier | 19:dbc5fbad4975 | 105 | |
Ludwigfr | 22:ebb37a249b5f | 106 | fill_initial_log_values(); |
geotsam | 13:41f75c132135 | 107 | |
geotsam | 24:8f4b820d8de8 | 108 | theta=DEFAULT_THETA; |
geotsam | 24:8f4b820d8de8 | 109 | X=DEFAULT_X; |
geotsam | 24:8f4b820d8de8 | 110 | Y=DEFAULT_Y; |
geotsam | 24:8f4b820d8de8 | 111 | |
geotsam | 24:8f4b820d8de8 | 112 | for (int i = 0; i<20; i++) { |
Ludwigfr | 22:ebb37a249b5f | 113 | randomize_and_map(); |
geotsam | 24:8f4b820d8de8 | 114 | wait(2); |
geotsam | 24:8f4b820d8de8 | 115 | print_final_map(); |
geotsam | 17:caf393b63e27 | 116 | } |
AurelienBernier | 8:109314be5b68 | 117 | |
AurelienBernier | 8:109314be5b68 | 118 | } |
AurelienBernier | 8:109314be5b68 | 119 | |
Ludwigfr | 22:ebb37a249b5f | 120 | //fill initialLogValues with the values we already know (here the bordurs) |
Ludwigfr | 22:ebb37a249b5f | 121 | void fill_initial_log_values(){ |
Ludwigfr | 22:ebb37a249b5f | 122 | //Fill map, we know the border are occupied |
geotsam | 24:8f4b820d8de8 | 123 | for (int i = 0; i<NB_CELL_WIDTH; i++) { |
geotsam | 24:8f4b820d8de8 | 124 | for (int j = 0; j<NB_CELL_HEIGHT; j++) { |
geotsam | 24:8f4b820d8de8 | 125 | if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1) |
Ludwigfr | 22:ebb37a249b5f | 126 | initialLogValues[i][j] = proba_to_log(1); |
Ludwigfr | 22:ebb37a249b5f | 127 | else |
Ludwigfr | 22:ebb37a249b5f | 128 | initialLogValues[i][j] = proba_to_log(0.5); |
AurelienBernier | 21:62154d644531 | 129 | } |
Ludwigfr | 22:ebb37a249b5f | 130 | } |
AurelienBernier | 8:109314be5b68 | 131 | } |
AurelienBernier | 8:109314be5b68 | 132 | |
Ludwigfr | 22:ebb37a249b5f | 133 | //generate a position randomly and makes the robot go there while updating the map |
Ludwigfr | 22:ebb37a249b5f | 134 | void randomize_and_map() { |
geotsam | 24:8f4b820d8de8 | 135 | //TODO check that it's aurelien's work |
geotsam | 24:8f4b820d8de8 | 136 | float target_x = (rand()%(int)(WIDTH_ARENA*10))/10;//for decimal precision |
geotsam | 24:8f4b820d8de8 | 137 | float target_y = (rand()%(int)(HEIGHT_ARENA*10))/10; |
Ludwigfr | 22:ebb37a249b5f | 138 | float target_angle = ((float)(rand()%31416)-15708)/10000.0; |
geotsam | 24:8f4b820d8de8 | 139 | |
geotsam | 24:8f4b820d8de8 | 140 | //TODO comment that |
geotsam | 24:8f4b820d8de8 | 141 | //pc.printf("\n\r targ_X=%f", target_x); |
geotsam | 24:8f4b820d8de8 | 142 | //pc.printf("\n\r targ_Y=%f", target_y); |
geotsam | 24:8f4b820d8de8 | 143 | //pc.printf("\n\r targ_Angle=%f", target_angle); |
geotsam | 24:8f4b820d8de8 | 144 | |
Ludwigfr | 22:ebb37a249b5f | 145 | go_to_point_with_angle(target_x, target_y, target_angle); |
AurelienBernier | 19:dbc5fbad4975 | 146 | } |
AurelienBernier | 19:dbc5fbad4975 | 147 | |
Ludwigfr | 22:ebb37a249b5f | 148 | //go the the given position while updating the map |
Ludwigfr | 22:ebb37a249b5f | 149 | //TODO clean this procedure it's ugly as hell and too long |
Ludwigfr | 22:ebb37a249b5f | 150 | void go_to_point_with_angle(float target_x, float target_y, float target_angle) { |
geotsam | 24:8f4b820d8de8 | 151 | if(tooClose){ |
geotsam | 24:8f4b820d8de8 | 152 | target_x=X; |
geotsam | 24:8f4b820d8de8 | 153 | target_y=Y; |
geotsam | 24:8f4b820d8de8 | 154 | target_angle=theta+pi/2; |
geotsam | 24:8f4b820d8de8 | 155 | target_angle = atan(sin(target_angle)/cos(target_angle)); |
geotsam | 24:8f4b820d8de8 | 156 | pc.printf("\n\rShould just turn now, new target_angle=%f\n\r", target_angle); |
geotsam | 24:8f4b820d8de8 | 157 | } |
geotsam | 24:8f4b820d8de8 | 158 | |
geotsam | 24:8f4b820d8de8 | 159 | //TODO ugly old stuff |
geotsam | 24:8f4b820d8de8 | 160 | float alpha; //angle error |
geotsam | 24:8f4b820d8de8 | 161 | float rho; //distance from target |
geotsam | 24:8f4b820d8de8 | 162 | float beta; |
geotsam | 24:8f4b820d8de8 | 163 | float kRho=12, ka=30, kb=-13; //Kappa values |
geotsam | 24:8f4b820d8de8 | 164 | float linear, angular, angular_left, angular_right; |
geotsam | 24:8f4b820d8de8 | 165 | float dt=0.5; |
geotsam | 24:8f4b820d8de8 | 166 | float temp; |
geotsam | 24:8f4b820d8de8 | 167 | float d2; |
geotsam | 24:8f4b820d8de8 | 168 | Timer t; |
geotsam | 24:8f4b820d8de8 | 169 | |
geotsam | 24:8f4b820d8de8 | 170 | int speed=MAX_SPEED; // Max speed at beggining of movement |
geotsam | 24:8f4b820d8de8 | 171 | |
geotsam | 24:8f4b820d8de8 | 172 | float leftMm; |
geotsam | 24:8f4b820d8de8 | 173 | float frontMm; |
geotsam | 24:8f4b820d8de8 | 174 | float rightMm; |
geotsam | 24:8f4b820d8de8 | 175 | |
geotsam | 24:8f4b820d8de8 | 176 | alpha = atan2((target_y-Y),(target_x-X))-theta; |
geotsam | 24:8f4b820d8de8 | 177 | alpha = atan(sin(alpha)/cos(alpha)); |
geotsam | 24:8f4b820d8de8 | 178 | rho = dist(X, Y, target_x, target_y); |
geotsam | 24:8f4b820d8de8 | 179 | |
geotsam | 24:8f4b820d8de8 | 180 | beta = -alpha-theta+target_angle; |
geotsam | 24:8f4b820d8de8 | 181 | //beta = atan(sin(beta)/cos(beta)); |
geotsam | 24:8f4b820d8de8 | 182 | |
geotsam | 24:8f4b820d8de8 | 183 | do { |
geotsam | 24:8f4b820d8de8 | 184 | //pc.printf("\n\n\r entered while"); |
AurelienBernier | 2:ea61e801e81f | 185 | |
AurelienBernier | 6:afde4b08166b | 186 | //Timer stuff |
AurelienBernier | 6:afde4b08166b | 187 | dt = t.read(); |
AurelienBernier | 6:afde4b08166b | 188 | t.reset(); |
AurelienBernier | 6:afde4b08166b | 189 | t.start(); |
AurelienBernier | 6:afde4b08166b | 190 | |
geotsam | 14:d58f2bdbf42e | 191 | //Updating X,Y and theta with the odometry values |
geotsam | 14:d58f2bdbf42e | 192 | Odometria(); |
geotsam | 24:8f4b820d8de8 | 193 | |
geotsam | 24:8f4b820d8de8 | 194 | leftMm = get_distance_left_sensor(); |
geotsam | 24:8f4b820d8de8 | 195 | frontMm = get_distance_front_sensor(); |
geotsam | 24:8f4b820d8de8 | 196 | rightMm = get_distance_right_sensor(); |
geotsam | 24:8f4b820d8de8 | 197 | |
geotsam | 24:8f4b820d8de8 | 198 | //pc.printf("\n\r leftMm=%f", leftMm); |
geotsam | 24:8f4b820d8de8 | 199 | //pc.printf("\n\r frontMm=%f", frontMm); |
geotsam | 24:8f4b820d8de8 | 200 | //pc.printf("\n\r rightMm=%f", rightMm); |
Ludwigfr | 22:ebb37a249b5f | 201 | |
geotsam | 24:8f4b820d8de8 | 202 | update_sonar_values(leftMm, frontMm, rightMm); |
geotsam | 24:8f4b820d8de8 | 203 | |
geotsam | 24:8f4b820d8de8 | 204 | if ((leftMm < SECURITY_DISTANCE || frontMm < SECURITY_DISTANCE || rightMm < SECURITY_DISTANCE) && turnFromObstacle==false) { |
AurelienBernier | 21:62154d644531 | 205 | tooClose = true; |
geotsam | 24:8f4b820d8de8 | 206 | turnFromObstacle = true; |
geotsam | 24:8f4b820d8de8 | 207 | pc.printf("\n\r TOO CLOSE \n\r"); |
geotsam | 24:8f4b820d8de8 | 208 | leftMotor(1,0); |
geotsam | 24:8f4b820d8de8 | 209 | rightMotor(1,0); |
geotsam | 24:8f4b820d8de8 | 210 | Odometria(); |
geotsam | 24:8f4b820d8de8 | 211 | go_to_point_with_angle(X, Y, rad_angle_check(theta+pi)); |
geotsam | 14:d58f2bdbf42e | 212 | break; |
AurelienBernier | 11:e641aa08c92e | 213 | } |
AurelienBernier | 11:e641aa08c92e | 214 | |
geotsam | 24:8f4b820d8de8 | 215 | alpha = atan2((target_y-Y),(target_x-X))-theta; |
geotsam | 24:8f4b820d8de8 | 216 | alpha = atan(sin(alpha)/cos(alpha)); |
geotsam | 24:8f4b820d8de8 | 217 | rho = dist(X, Y, target_x, target_y); |
geotsam | 24:8f4b820d8de8 | 218 | d2 = rho; |
geotsam | 24:8f4b820d8de8 | 219 | beta = -alpha-theta+target_angle; |
geotsam | 24:8f4b820d8de8 | 220 | |
geotsam | 24:8f4b820d8de8 | 221 | //Computing angle error and distance towards the target value |
geotsam | 24:8f4b820d8de8 | 222 | rho += dt*(-kRho*cos(alpha)*rho); |
geotsam | 24:8f4b820d8de8 | 223 | temp = alpha; |
geotsam | 24:8f4b820d8de8 | 224 | alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta); |
geotsam | 24:8f4b820d8de8 | 225 | beta += dt*(-kRho*sin(temp)); |
geotsam | 24:8f4b820d8de8 | 226 | //pc.printf("\n\r d2=%f", d2); |
geotsam | 24:8f4b820d8de8 | 227 | //pc.printf("\n\r dt=%f", dt); |
geotsam | 0:8bffb51cc345 | 228 | |
geotsam | 24:8f4b820d8de8 | 229 | //Computing linear and angular velocities |
geotsam | 24:8f4b820d8de8 | 230 | if(alpha>=-1.5708 && alpha<=1.5708){ |
geotsam | 24:8f4b820d8de8 | 231 | linear=kRho*rho; |
geotsam | 24:8f4b820d8de8 | 232 | angular=ka*alpha+kb*beta; |
geotsam | 24:8f4b820d8de8 | 233 | } |
geotsam | 24:8f4b820d8de8 | 234 | else{ |
geotsam | 24:8f4b820d8de8 | 235 | linear=-kRho*rho; |
geotsam | 24:8f4b820d8de8 | 236 | angular=-ka*alpha-kb*beta; |
geotsam | 24:8f4b820d8de8 | 237 | } |
geotsam | 24:8f4b820d8de8 | 238 | angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; |
geotsam | 24:8f4b820d8de8 | 239 | angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; |
geotsam | 24:8f4b820d8de8 | 240 | |
geotsam | 24:8f4b820d8de8 | 241 | pc.printf("\n\r a_r=%f", angular_right); |
geotsam | 24:8f4b820d8de8 | 242 | pc.printf("\n\r a_l=%f", angular_left); |
geotsam | 24:8f4b820d8de8 | 243 | |
geotsam | 24:8f4b820d8de8 | 244 | //Slowing down at the end for more precision |
geotsam | 24:8f4b820d8de8 | 245 | //if (d2<25) { |
geotsam | 24:8f4b820d8de8 | 246 | // speed = d2*30; |
geotsam | 24:8f4b820d8de8 | 247 | // } |
geotsam | 24:8f4b820d8de8 | 248 | |
geotsam | 24:8f4b820d8de8 | 249 | //Normalize speed for motors |
geotsam | 24:8f4b820d8de8 | 250 | if(angular_left>angular_right) { |
geotsam | 24:8f4b820d8de8 | 251 | angular_right=speed*angular_right/angular_left; |
geotsam | 24:8f4b820d8de8 | 252 | angular_left=speed; |
geotsam | 24:8f4b820d8de8 | 253 | } else { |
geotsam | 24:8f4b820d8de8 | 254 | angular_left=speed*angular_left/angular_right; |
geotsam | 24:8f4b820d8de8 | 255 | angular_right=speed; |
geotsam | 24:8f4b820d8de8 | 256 | } |
geotsam | 24:8f4b820d8de8 | 257 | |
geotsam | 24:8f4b820d8de8 | 258 | //pc.printf("\n\r X=%f", X); |
geotsam | 24:8f4b820d8de8 | 259 | //pc.printf("\n\r Y=%f", Y); |
geotsam | 24:8f4b820d8de8 | 260 | |
geotsam | 24:8f4b820d8de8 | 261 | pc.printf("\n\r a_r=%f", angular_right); |
geotsam | 24:8f4b820d8de8 | 262 | pc.printf("\n\r a_l=%f", angular_left); |
geotsam | 0:8bffb51cc345 | 263 | |
AurelienBernier | 2:ea61e801e81f | 264 | //Updating motor velocities |
AurelienBernier | 1:f0807d5c5a4b | 265 | leftMotor(1,angular_left); |
AurelienBernier | 1:f0807d5c5a4b | 266 | rightMotor(1,angular_right); |
geotsam | 0:8bffb51cc345 | 267 | |
AurelienBernier | 7:c94070f9af78 | 268 | wait(0.2); |
AurelienBernier | 6:afde4b08166b | 269 | //Timer stuff |
AurelienBernier | 6:afde4b08166b | 270 | t.stop(); |
geotsam | 24:8f4b820d8de8 | 271 | } while(d2>1 && (abs(target_angle-theta)>0.01) && tooClose==false); |
geotsam | 24:8f4b820d8de8 | 272 | |
geotsam | 24:8f4b820d8de8 | 273 | //Stop at the end |
geotsam | 24:8f4b820d8de8 | 274 | leftMotor(1,0); |
geotsam | 24:8f4b820d8de8 | 275 | rightMotor(1,0); |
geotsam | 24:8f4b820d8de8 | 276 | |
geotsam | 24:8f4b820d8de8 | 277 | if(turnFromObstacle){ |
geotsam | 24:8f4b820d8de8 | 278 | turnFromObstacle=false; |
geotsam | 24:8f4b820d8de8 | 279 | tooClose=false; |
geotsam | 24:8f4b820d8de8 | 280 | } |
Ludwigfr | 22:ebb37a249b5f | 281 | } |
Ludwigfr | 22:ebb37a249b5f | 282 | |
Ludwigfr | 22:ebb37a249b5f | 283 | //Updates sonar values |
geotsam | 24:8f4b820d8de8 | 284 | void update_sonar_values(float leftMm, float frontMm, float rightMm){ |
Ludwigfr | 22:ebb37a249b5f | 285 | |
Ludwigfr | 22:ebb37a249b5f | 286 | float currProba; |
Ludwigfr | 25:572c9e9a8809 | 287 | float i_in_orthonormal; |
Ludwigfr | 25:572c9e9a8809 | 288 | float j_in_orthonormal; |
geotsam | 24:8f4b820d8de8 | 289 | for(int i=0;i<NB_CELL_WIDTH;i++){ |
geotsam | 24:8f4b820d8de8 | 290 | for(int j=0;j<NB_CELL_HEIGHT;j++){ |
geotsam | 24:8f4b820d8de8 | 291 | //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 | 292 | //check that again |
Ludwigfr | 22:ebb37a249b5f | 293 | //compute for front sonar |
Ludwigfr | 25:572c9e9a8809 | 294 | i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i); |
Ludwigfr | 25:572c9e9a8809 | 295 | j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j); |
Ludwigfr | 25:572c9e9a8809 | 296 | |
Ludwigfr | 25:572c9e9a8809 | 297 | 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 | 298 | 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 | 299 | //compute for right sonar |
Ludwigfr | 25:572c9e9a8809 | 300 | 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 | 301 | map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; |
Ludwigfr | 22:ebb37a249b5f | 302 | //compute for left sonar |
Ludwigfr | 25:572c9e9a8809 | 303 | 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 | 304 | map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; |
Ludwigfr | 22:ebb37a249b5f | 305 | } |
Ludwigfr | 22:ebb37a249b5f | 306 | } |
Ludwigfr | 22:ebb37a249b5f | 307 | } |
Ludwigfr | 22:ebb37a249b5f | 308 | |
Ludwigfr | 25:572c9e9a8809 | 309 | //ODOMETRIA MUST HAVE BEEN CALLED |
Ludwigfr | 22:ebb37a249b5f | 310 | //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 | 311 | float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){ |
Ludwigfr | 22:ebb37a249b5f | 312 | |
Ludwigfr | 22:ebb37a249b5f | 313 | float alpha=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam |
geotsam | 24:8f4b820d8de8 | 314 | float alphaBeforeAdjustment=alpha-theta-angleFromSonarPosition; |
Ludwigfr | 22:ebb37a249b5f | 315 | alpha=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure |
Ludwigfr | 22:ebb37a249b5f | 316 | float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2)); |
Ludwigfr | 22:ebb37a249b5f | 317 | |
Ludwigfr | 22:ebb37a249b5f | 318 | //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS |
Ludwigfr | 22:ebb37a249b5f | 319 | //check if absolute difference between the angles is no more than Omega/2 |
Ludwigfr | 22:ebb37a249b5f | 320 | if( distancePointToSonar < (RANGE_SONAR)&& (alpha <= ANGLE_SONAR/2 || alpha >= rad_angle_check(-ANGLE_SONAR/2))){ |
Ludwigfr | 22:ebb37a249b5f | 321 | if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){ |
Ludwigfr | 22:ebb37a249b5f | 322 | //point before obstacle, probably empty |
Ludwigfr | 22:ebb37a249b5f | 323 | /*****************************************************************************/ |
Ludwigfr | 22:ebb37a249b5f | 324 | float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2); |
Ludwigfr | 22:ebb37a249b5f | 325 | float Er; |
Ludwigfr | 22:ebb37a249b5f | 326 | if(distancePointToSonar < RANGE_SONAR_MIN){ |
Ludwigfr | 22:ebb37a249b5f | 327 | //point before minimum sonar range |
Ludwigfr | 22:ebb37a249b5f | 328 | Er=0.f; |
Ludwigfr | 22:ebb37a249b5f | 329 | }else{ |
Ludwigfr | 22:ebb37a249b5f | 330 | //point after minimum sonar range |
Ludwigfr | 22:ebb37a249b5f | 331 | Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2); |
Ludwigfr | 22:ebb37a249b5f | 332 | } |
Ludwigfr | 22:ebb37a249b5f | 333 | /*****************************************************************************/ |
Ludwigfr | 22:ebb37a249b5f | 334 | return (1.f-Er*Ea)/2.f; |
Ludwigfr | 22:ebb37a249b5f | 335 | }else{ |
Ludwigfr | 22:ebb37a249b5f | 336 | //probably occupied |
Ludwigfr | 22:ebb37a249b5f | 337 | /*****************************************************************************/ |
Ludwigfr | 22:ebb37a249b5f | 338 | float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2); |
Ludwigfr | 22:ebb37a249b5f | 339 | float Or; |
Ludwigfr | 22:ebb37a249b5f | 340 | if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){ |
Ludwigfr | 22:ebb37a249b5f | 341 | //point between distanceObstacleDetected +- INCERTITUDE_SONAR |
Ludwigfr | 22:ebb37a249b5f | 342 | Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2); |
Ludwigfr | 22:ebb37a249b5f | 343 | }else{ |
Ludwigfr | 22:ebb37a249b5f | 344 | //point after in range of the sonar but after the zone detected |
Ludwigfr | 22:ebb37a249b5f | 345 | Or=0; |
Ludwigfr | 22:ebb37a249b5f | 346 | } |
Ludwigfr | 22:ebb37a249b5f | 347 | /*****************************************************************************/ |
Ludwigfr | 22:ebb37a249b5f | 348 | return (1+Or*Oa)/2; |
Ludwigfr | 22:ebb37a249b5f | 349 | } |
Ludwigfr | 22:ebb37a249b5f | 350 | }else{ |
Ludwigfr | 25:572c9e9a8809 | 351 | //not checked by the sonar |
Ludwigfr | 22:ebb37a249b5f | 352 | return 0.5; |
AurelienBernier | 19:dbc5fbad4975 | 353 | } |
Ludwigfr | 22:ebb37a249b5f | 354 | } |
Ludwigfr | 22:ebb37a249b5f | 355 | |
Ludwigfr | 25:572c9e9a8809 | 356 | void print_final_map() { |
Ludwigfr | 22:ebb37a249b5f | 357 | float currProba; |
geotsam | 24:8f4b820d8de8 | 358 | pc.printf("\n\r"); |
geotsam | 24:8f4b820d8de8 | 359 | for (int y = NB_CELL_HEIGHT -1; y>-1; y--) { |
geotsam | 24:8f4b820d8de8 | 360 | for (int x= 0; x<NB_CELL_WIDTH; x++) { |
geotsam | 24:8f4b820d8de8 | 361 | currProba=log_to_proba(map[x][y]); |
geotsam | 24:8f4b820d8de8 | 362 | if ( currProba < 0.5) { |
geotsam | 24:8f4b820d8de8 | 363 | pc.printf(" 0 "); |
Ludwigfr | 22:ebb37a249b5f | 364 | } else { |
Ludwigfr | 22:ebb37a249b5f | 365 | if(currProba==0.5) |
geotsam | 24:8f4b820d8de8 | 366 | pc.printf(" . "); |
Ludwigfr | 22:ebb37a249b5f | 367 | else |
geotsam | 24:8f4b820d8de8 | 368 | pc.printf(" + "); |
Ludwigfr | 22:ebb37a249b5f | 369 | } |
Ludwigfr | 22:ebb37a249b5f | 370 | } |
geotsam | 24:8f4b820d8de8 | 371 | pc.printf("\n\r"); |
Ludwigfr | 22:ebb37a249b5f | 372 | } |
Ludwigfr | 22:ebb37a249b5f | 373 | } |
Ludwigfr | 22:ebb37a249b5f | 374 | |
Ludwigfr | 25:572c9e9a8809 | 375 | void print_final_map_with_robot_position() { |
geotsam | 24:8f4b820d8de8 | 376 | float currProba; |
Ludwigfr | 25:572c9e9a8809 | 377 | Odometria(); |
Ludwigfr | 25:572c9e9a8809 | 378 | float Xrobot=robot_center_x_in_orthonormal_x(); |
Ludwigfr | 25:572c9e9a8809 | 379 | float Yrobot=robot_center_y_in_orthonormal_y(); |
Ludwigfr | 25:572c9e9a8809 | 380 | |
Ludwigfr | 25:572c9e9a8809 | 381 | float heightIndiceInOrthonormal; |
Ludwigfr | 25:572c9e9a8809 | 382 | float widthIndiceInOrthonormal; |
Ludwigfr | 25:572c9e9a8809 | 383 | |
Ludwigfr | 25:572c9e9a8809 | 384 | float heightMalus=-(3*sizeCellX/2); |
Ludwigfr | 25:572c9e9a8809 | 385 | float heightBonus=sizeCellX/2; |
Ludwigfr | 25:572c9e9a8809 | 386 | |
Ludwigfr | 25:572c9e9a8809 | 387 | float widthMalus=-(3*sizeCellY/2); |
Ludwigfr | 25:572c9e9a8809 | 388 | float widthBonus=sizeCellY/2; |
Ludwigfr | 25:572c9e9a8809 | 389 | |
geotsam | 24:8f4b820d8de8 | 390 | pc.printf("\n\r"); |
geotsam | 24:8f4b820d8de8 | 391 | for (int y = NB_CELL_HEIGHT -1; y>-1; y--) { |
geotsam | 24:8f4b820d8de8 | 392 | for (int x= 0; x<NB_CELL_WIDTH; x++) { |
Ludwigfr | 25:572c9e9a8809 | 393 | heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y); |
Ludwigfr | 25:572c9e9a8809 | 394 | widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x); |
Ludwigfr | 25:572c9e9a8809 | 395 | if(Xrobot >= (heightIndiceInOrthonormal+heightMalus) && Xrobot <= (heightIndiceInOrthonormal+heightBonus) && Yrobot >= (widthIndiceInOrthonormal+widthMalus) && Yrobot <= (widthIndiceInOrthonormal+widthBonus)) |
geotsam | 24:8f4b820d8de8 | 396 | pc.printf(" R "); |
Ludwigfr | 25:572c9e9a8809 | 397 | else{ |
Ludwigfr | 25:572c9e9a8809 | 398 | currProba=log_to_proba(map[x][y]); |
Ludwigfr | 25:572c9e9a8809 | 399 | if ( currProba < 0.5) |
Ludwigfr | 25:572c9e9a8809 | 400 | pc.printf(" 0 "); |
Ludwigfr | 25:572c9e9a8809 | 401 | else{ |
Ludwigfr | 25:572c9e9a8809 | 402 | if(currProba==0.5) |
Ludwigfr | 25:572c9e9a8809 | 403 | pc.printf(" . "); |
Ludwigfr | 25:572c9e9a8809 | 404 | else |
Ludwigfr | 25:572c9e9a8809 | 405 | pc.printf(" + "); |
Ludwigfr | 25:572c9e9a8809 | 406 | } |
geotsam | 24:8f4b820d8de8 | 407 | } |
geotsam | 24:8f4b820d8de8 | 408 | } |
geotsam | 24:8f4b820d8de8 | 409 | pc.printf("\n\r"); |
geotsam | 24:8f4b820d8de8 | 410 | } |
geotsam | 24:8f4b820d8de8 | 411 | } |
Ludwigfr | 22:ebb37a249b5f | 412 | |
Ludwigfr | 22:ebb37a249b5f | 413 | |
Ludwigfr | 22:ebb37a249b5f | 414 | //MATHS heavy functions |
Ludwigfr | 22:ebb37a249b5f | 415 | /**********************************************************************/ |
Ludwigfr | 22:ebb37a249b5f | 416 | //Distance computation function |
Ludwigfr | 22:ebb37a249b5f | 417 | float dist(float robot_x, float robot_y, float target_x, float target_y){ |
Ludwigfr | 22:ebb37a249b5f | 418 | return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); |
Ludwigfr | 22:ebb37a249b5f | 419 | } |
Ludwigfr | 22:ebb37a249b5f | 420 | |
geotsam | 24:8f4b820d8de8 | 421 | //returns the probability [0,1] that the cell is occupied from the log valAue lt |
Ludwigfr | 22:ebb37a249b5f | 422 | float log_to_proba(float lt){ |
Ludwigfr | 22:ebb37a249b5f | 423 | return 1-1/(1+exp(lt)); |
Ludwigfr | 22:ebb37a249b5f | 424 | } |
Ludwigfr | 22:ebb37a249b5f | 425 | |
Ludwigfr | 22:ebb37a249b5f | 426 | //returns the log value that the cell is occupied from the probability value [0,1] |
Ludwigfr | 22:ebb37a249b5f | 427 | float proba_to_log(float p){ |
Ludwigfr | 22:ebb37a249b5f | 428 | return log(p/(1-p)); |
Ludwigfr | 22:ebb37a249b5f | 429 | } |
Ludwigfr | 22:ebb37a249b5f | 430 | |
Ludwigfr | 22:ebb37a249b5f | 431 | //returns the new log value |
Ludwigfr | 22:ebb37a249b5f | 432 | float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){ |
Ludwigfr | 22:ebb37a249b5f | 433 | return previousLogValue+proba_to_log(currentProbability)-originalLogvalue; |
Ludwigfr | 22:ebb37a249b5f | 434 | } |
Ludwigfr | 22:ebb37a249b5f | 435 | |
Ludwigfr | 22:ebb37a249b5f | 436 | //makes the angle inAngle between 0 and 2pi |
Ludwigfr | 22:ebb37a249b5f | 437 | float rad_angle_check(float inAngle){ |
Ludwigfr | 22:ebb37a249b5f | 438 | //cout<<"before :"<<inAngle; |
Ludwigfr | 22:ebb37a249b5f | 439 | if(inAngle > 0){ |
Ludwigfr | 22:ebb37a249b5f | 440 | while(inAngle > (2*pi)) |
Ludwigfr | 22:ebb37a249b5f | 441 | inAngle-=2*pi; |
Ludwigfr | 22:ebb37a249b5f | 442 | }else{ |
Ludwigfr | 22:ebb37a249b5f | 443 | while(inAngle < 0) |
Ludwigfr | 22:ebb37a249b5f | 444 | inAngle+=2*pi; |
Ludwigfr | 22:ebb37a249b5f | 445 | } |
Ludwigfr | 22:ebb37a249b5f | 446 | //cout<<" after :"<<inAngle<<endl; |
Ludwigfr | 22:ebb37a249b5f | 447 | return inAngle; |
Ludwigfr | 22:ebb37a249b5f | 448 | } |
Ludwigfr | 22:ebb37a249b5f | 449 | |
Ludwigfr | 22:ebb37a249b5f | 450 | //returns the angle between the vectors (x,y) and (xs,ys) |
Ludwigfr | 22:ebb37a249b5f | 451 | float compute_angle_between_vectors(float x, float y,float xs,float ys){ |
Ludwigfr | 22:ebb37a249b5f | 452 | //alpha angle between ->x and ->SA |
Ludwigfr | 22:ebb37a249b5f | 453 | //vector S to A ->SA |
Ludwigfr | 22:ebb37a249b5f | 454 | float vSAx=x-xs; |
Ludwigfr | 22:ebb37a249b5f | 455 | float vSAy=y-ys; |
Ludwigfr | 22:ebb37a249b5f | 456 | //norme SA |
Ludwigfr | 22:ebb37a249b5f | 457 | float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2)); |
Ludwigfr | 22:ebb37a249b5f | 458 | //vector ->x (1,0) |
Ludwigfr | 22:ebb37a249b5f | 459 | float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;; |
Ludwigfr | 22:ebb37a249b5f | 460 | //vector ->y (0,1) |
Ludwigfr | 22:ebb37a249b5f | 461 | float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx; |
Ludwigfr | 22:ebb37a249b5f | 462 | if (sinAlpha < 0) |
Ludwigfr | 22:ebb37a249b5f | 463 | return -acos(cosAlpha); |
Ludwigfr | 22:ebb37a249b5f | 464 | else |
Ludwigfr | 22:ebb37a249b5f | 465 | return acos(cosAlpha); |
Ludwigfr | 25:572c9e9a8809 | 466 | } |
Ludwigfr | 25:572c9e9a8809 | 467 | |
Ludwigfr | 25:572c9e9a8809 | 468 | float robot_center_x_in_orthonormal_x(){ |
Ludwigfr | 25:572c9e9a8809 | 469 | return Y; |
Ludwigfr | 25:572c9e9a8809 | 470 | } |
Ludwigfr | 25:572c9e9a8809 | 471 | |
Ludwigfr | 25:572c9e9a8809 | 472 | float robot_center_y_in_orthonormal_y(){ |
Ludwigfr | 25:572c9e9a8809 | 473 | return NB_CELL_WIDTH*sizeCellX-X; |
Ludwigfr | 25:572c9e9a8809 | 474 | } |
Ludwigfr | 25:572c9e9a8809 | 475 | |
Ludwigfr | 25:572c9e9a8809 | 476 | float robot_sonar_front_x_in_orthonormal_x(){ |
Ludwigfr | 25:572c9e9a8809 | 477 | return Y+DISTANCE_SONAR_FRONT_Y; |
Ludwigfr | 25:572c9e9a8809 | 478 | } |
Ludwigfr | 25:572c9e9a8809 | 479 | float robot_sonar_front_y_in_orthonormal_y(){ |
Ludwigfr | 25:572c9e9a8809 | 480 | return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_FRONT_X; |
Ludwigfr | 25:572c9e9a8809 | 481 | } |
Ludwigfr | 25:572c9e9a8809 | 482 | |
Ludwigfr | 25:572c9e9a8809 | 483 | float robot_sonar_right_x_in_orthonormal_x(){ |
Ludwigfr | 25:572c9e9a8809 | 484 | return Y+DISTANCE_SONAR_RIGHT_Y; |
Ludwigfr | 25:572c9e9a8809 | 485 | } |
Ludwigfr | 25:572c9e9a8809 | 486 | float robot_sonar_right_y_in_orthonormal_y(){ |
Ludwigfr | 25:572c9e9a8809 | 487 | return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_RIGHT_X; |
Ludwigfr | 25:572c9e9a8809 | 488 | } |
Ludwigfr | 25:572c9e9a8809 | 489 | |
Ludwigfr | 25:572c9e9a8809 | 490 | float robot_sonar_left_x_in_orthonormal_x(){ |
Ludwigfr | 25:572c9e9a8809 | 491 | return Y+DISTANCE_SONAR_LEFT_Y; |
Ludwigfr | 25:572c9e9a8809 | 492 | } |
Ludwigfr | 25:572c9e9a8809 | 493 | float robot_sonar_left_y_in_orthonormal_y(){ |
Ludwigfr | 25:572c9e9a8809 | 494 | return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_LEFT_X; |
Ludwigfr | 25:572c9e9a8809 | 495 | } |
Ludwigfr | 25:572c9e9a8809 | 496 | |
Ludwigfr | 25:572c9e9a8809 | 497 | float estimated_width_indice_in_orthonormal_x(int i){ |
Ludwigfr | 25:572c9e9a8809 | 498 | return sizeCellX/2+i*sizeCellX; |
Ludwigfr | 25:572c9e9a8809 | 499 | } |
Ludwigfr | 25:572c9e9a8809 | 500 | float estimated_height_indice_in_orthonormal_y(int j){ |
Ludwigfr | 25:572c9e9a8809 | 501 | return sizeCellY/2+j*sizeCellY; |
geotsam | 24:8f4b820d8de8 | 502 | } |