Georgios Tsamis
Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of
VirtualForces
by 
Georgios Tsamis
Diff: main.cpp
- Revision:
- 24:8f4b820d8de8
- Parent:
- 23:901fc468b8a7
- Child:
- 25:572c9e9a8809
--- a/main.cpp Wed Apr 19 10:45:09 2017 +0000
+++ b/main.cpp Wed Apr 19 22:20:56 2017 +0000
@@ -11,16 +11,11 @@
//go the the given position while updating the map
void go_to_point_with_angle(float target_x, float target_y, float target_angle);
//Updates sonar values
-void update_sonar_values();
+void update_sonar_values(float leftMm, float frontMm, float rightMm);
//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]
float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
//print the map
void print_final_map();
-//compute the angles and distance used for the velocities
-void compute_angles_and_distance(float target_x, float target_y, float target_angle);
-//compute the linear and 2 angular velocities
-void compute_linear_angular_velocities();
-
//MATHS heavy functions
float dist(float robot_x, float robot_y, float target_x, float target_y);
@@ -38,55 +33,54 @@
const float pi=3.14159;
//spec of the sonar
//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
-const float RANGE_SONAR=50;//RADIUS_WHEELS
-const float RANGE_SONAR_MIN=10;//Rmin
-const float INCERTITUDE_SONAR=10;
-const float ANGLE_SONAR=pi/3;//Omega
+const float RANGE_SONAR=50;//cm
+const float RANGE_SONAR_MIN=10;//Rmin cm
+const float INCERTITUDE_SONAR=10;//cm
+const float ANGLE_SONAR=pi/3;//Omega rad
+const float SECURITY_DISTANCE=150;//mm
//those distance and angle are approximation in need of measurements
-const float ANGLE_FRONT_TO_LEFT=pi/3;
-const float DISTANCE_SONAR_LEFT_X=5;
-const float DISTANCE_SONAR_LEFT_Y=5;
-float leftMm;
+const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees
+const float DISTANCE_SONAR_LEFT_X=4;
+const float DISTANCE_SONAR_LEFT_Y=4;
-const float ANGLE_FRONT_TO_RIGHT=-pi/3;
-const float DISTANCE_SONAR_RIGHT_X=-5;
-const float DISTANCE_SONAR_RIGHT_Y=5;
-float rightMm;
+const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees
+const float DISTANCE_SONAR_RIGHT_X=-4;
+const float DISTANCE_SONAR_RIGHT_Y=4;
const float ANGLE_FRONT_TO_FRONT=0;
const float DISTANCE_SONAR_FRONT_X=0;
const float DISTANCE_SONAR_FRONT_Y=5;
-float frontMm;
//TODO adjust the size of the map for computation time (25*25?)
-const int SIZE_MAP=25;
+const float WIDTH_ARENA=120;//cm
+const float HEIGHT_ARENA=90;//cm
+
+//const int SIZE_MAP=25;
+const int NB_CELL_WIDTH=24;
+const int NB_CELL_HEIGHT=18;
//position and orientation of the robot when put on the map (ODOMETRY doesn't know those)
-const float DEFAULT_X=0;
-const float DEFAULT_Y=0;
+const float DEFAULT_X=WIDTH_ARENA/2;
+const float DEFAULT_Y=HEIGHT_ARENA/2;
const float DEFAULT_THETA=pi/2;
-//used to create the map 250 represent the 250cm of the table where the robot is tested
-float sizeCell=250/(float)SIZE_MAP;
-float map[SIZE_MAP][SIZE_MAP];//contains the log values for each cell
-float initialLogValues[SIZE_MAP][SIZE_MAP];
+//used to create the map 250 represent the 250cm of the square where the robot is tested
+//float sizeCell=250/(float)SIZE_MAP;
+float sizeCellX=WIDTH_ARENA/(float)NB_CELL_WIDTH;
+float sizeCellY=HEIGHT_ARENA/(float)NB_CELL_HEIGHT;
+
+float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell
+float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT];
//Diameter of a wheel and distance between the 2
const float RADIUS_WHEELS=3.25;
const float DISTANCE_WHEELS=7.2;
-float alpha; //angle error
-float rho; //distance from target
-float beta;
-float kRho=12, ka=30, kb=-13; //Kappa values
-float linear, angular, angular_left, angular_right;
-float dt=0.5;
-float temp;
-float d2;
-Timer t;
+const int MAX_SPEED=500;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
-int speed=999; // Max speed at beggining of movement
+bool tooClose=false;
+bool turnFromObstacle=false;
int main(){
@@ -95,26 +89,28 @@
pc.baud(9600); // baud for the pc communication
measure_always_on();//TODO check if needed
+ wait(0.5);
fill_initial_log_values();
- for (int i = 0; i<25; i++) {
+ theta=DEFAULT_THETA;
+ X=DEFAULT_X;
+ Y=DEFAULT_Y;
+
+ for (int i = 0; i<20; i++) {
randomize_and_map();
+ wait(2);
+ print_final_map();
}
- //Stop at the end
- leftMotor(1,0);
- rightMotor(1,0);
- //pc.printf("\n\r %f -- arrived!", rho);
- print_final_map();
}
//fill initialLogValues with the values we already know (here the bordurs)
void fill_initial_log_values(){
//Fill map, we know the border are occupied
- for (int i = 0; i<SIZE_MAP; i++) {
- for (int j = 0; j<SIZE_MAP; j++) {
- if(j==0 || j==SIZE_MAP-1 || i==0 || i==SIZE_MAP-1)
+ for (int i = 0; i<NB_CELL_WIDTH; i++) {
+ for (int j = 0; j<NB_CELL_HEIGHT; j++) {
+ if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1)
initialLogValues[i][j] = proba_to_log(1);
else
initialLogValues[i][j] = proba_to_log(0.5);
@@ -124,23 +120,56 @@
//generate a position randomly and makes the robot go there while updating the map
void randomize_and_map() {
- float target_x = (rand()%2500)/10;//for decimal precision
- float target_y = (rand()%2500)/10;
+ //TODO check that it's aurelien's work
+ float target_x = (rand()%(int)(WIDTH_ARENA*10))/10;//for decimal precision
+ float target_y = (rand()%(int)(HEIGHT_ARENA*10))/10;
float target_angle = ((float)(rand()%31416)-15708)/10000.0;
- /*
- pc.printf("\n\r targ_X=%f", target_x);
- pc.printf("\n\r targ_Y=%f", target_y);
- pc.printf("\n\r targ_Angle=%f", target_angle);
- */
+
+ //TODO comment that
+ //pc.printf("\n\r targ_X=%f", target_x);
+ //pc.printf("\n\r targ_Y=%f", target_y);
+ //pc.printf("\n\r targ_Angle=%f", target_angle);
+
go_to_point_with_angle(target_x, target_y, target_angle);
}
//go the the given position while updating the map
//TODO clean this procedure it's ugly as hell and too long
void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
- bool tooClose=false; //binary variable to know if an obstacle is <10cm away
- do {
- pc.printf("\n\n\r entered while");
+ if(tooClose){
+ target_x=X;
+ target_y=Y;
+ target_angle=theta+pi/2;
+ target_angle = atan(sin(target_angle)/cos(target_angle));
+ pc.printf("\n\rShould just turn now, new target_angle=%f\n\r", target_angle);
+ }
+
+ //TODO ugly old stuff
+ float alpha; //angle error
+ float rho; //distance from target
+ float beta;
+ float kRho=12, ka=30, kb=-13; //Kappa values
+ float linear, angular, angular_left, angular_right;
+ float dt=0.5;
+ float temp;
+ float d2;
+ Timer t;
+
+ int speed=MAX_SPEED; // Max speed at beggining of movement
+
+ float leftMm;
+ float frontMm;
+ float rightMm;
+
+ alpha = atan2((target_y-Y),(target_x-X))-theta;
+ alpha = atan(sin(alpha)/cos(alpha));
+ rho = dist(X, Y, target_x, target_y);
+
+ beta = -alpha-theta+target_angle;
+ //beta = atan(sin(beta)/cos(beta));
+
+ do {
+ //pc.printf("\n\n\r entered while");
//Timer stuff
dt = t.read();
@@ -149,25 +178,76 @@
//Updating X,Y and theta with the odometry values
Odometria();
-
- //Updare the values from the sonar
- update_sonar_values();
+
+ leftMm = get_distance_left_sensor();
+ frontMm = get_distance_front_sensor();
+ rightMm = get_distance_right_sensor();
+
+ //pc.printf("\n\r leftMm=%f", leftMm);
+ //pc.printf("\n\r frontMm=%f", frontMm);
+ //pc.printf("\n\r rightMm=%f", rightMm);
- //Check if one of the sonars finds an obstacle that is too close
- if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
+ update_sonar_values(leftMm, frontMm, rightMm);
+
+ if ((leftMm < SECURITY_DISTANCE || frontMm < SECURITY_DISTANCE || rightMm < SECURITY_DISTANCE) && turnFromObstacle==false) {
tooClose = true;
+ turnFromObstacle = true;
+ pc.printf("\n\r TOO CLOSE \n\r");
+ leftMotor(1,0);
+ rightMotor(1,0);
+ Odometria();
+ go_to_point_with_angle(X, Y, rad_angle_check(theta+pi));
break;
}
- compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target
- compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular)
+ alpha = atan2((target_y-Y),(target_x-X))-theta;
+ alpha = atan(sin(alpha)/cos(alpha));
+ rho = dist(X, Y, target_x, target_y);
+ d2 = rho;
+ beta = -alpha-theta+target_angle;
+
+ //Computing angle error and distance towards the target value
+ rho += dt*(-kRho*cos(alpha)*rho);
+ temp = alpha;
+ alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
+ beta += dt*(-kRho*sin(temp));
+ //pc.printf("\n\r d2=%f", d2);
+ //pc.printf("\n\r dt=%f", dt);
- //Printing values for debugging purposes
- pc.printf("\n\r X=%f", X);
- pc.printf("\n\r Y=%f", Y);
- pc.printf("\n\r leftMm=%f", leftMm);
- pc.printf("\n\r frontMm=%f", frontMm);
- pc.printf("\n\r rightMm=%f", rightMm);
+ //Computing linear and angular velocities
+ if(alpha>=-1.5708 && alpha<=1.5708){
+ linear=kRho*rho;
+ angular=ka*alpha+kb*beta;
+ }
+ else{
+ linear=-kRho*rho;
+ angular=-ka*alpha-kb*beta;
+ }
+ angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+ angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+
+ pc.printf("\n\r a_r=%f", angular_right);
+ pc.printf("\n\r a_l=%f", angular_left);
+
+ //Slowing down at the end for more precision
+ //if (d2<25) {
+// speed = d2*30;
+// }
+
+ //Normalize speed for motors
+ if(angular_left>angular_right) {
+ angular_right=speed*angular_right/angular_left;
+ angular_left=speed;
+ } else {
+ angular_left=speed*angular_left/angular_right;
+ angular_right=speed;
+ }
+
+ //pc.printf("\n\r X=%f", X);
+ //pc.printf("\n\r Y=%f", Y);
+
+ pc.printf("\n\r a_r=%f", angular_right);
+ pc.printf("\n\r a_l=%f", angular_left);
//Updating motor velocities
leftMotor(1,angular_left);
@@ -176,29 +256,34 @@
wait(0.2);
//Timer stuff
t.stop();
- } while(d2>1 && tooClose==false);
+ } while(d2>1 && (abs(target_angle-theta)>0.01) && tooClose==false);
+
+ //Stop at the end
+ leftMotor(1,0);
+ rightMotor(1,0);
+
+ if(turnFromObstacle){
+ turnFromObstacle=false;
+ tooClose=false;
+ }
}
//Updates sonar values
-void update_sonar_values(){
-
- float leftMm = get_distance_left_sensor();
- float frontMm = get_distance_front_sensor();
- float rightMm = get_distance_right_sensor();
+void update_sonar_values(float leftMm, float frontMm, float rightMm){
float currProba;
- for(int i=0;i<SIZE_MAP;i++){
- for(int j=0;j<SIZE_MAP;j++){
- //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)
-
+ for(int i=0;i<NB_CELL_WIDTH;i++){
+ for(int j=0;j<NB_CELL_HEIGHT;j++){
+ //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)
+ //check that again
//compute for front sonar
- 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);
+ currProba=compute_probability_t(sizeCellX/2+i*sizeCellX,sizeCellY/2+j*sizeCellY,Y+DISTANCE_SONAR_FRONT_Y,NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_FRONT_X,ANGLE_FRONT_TO_FRONT,frontMm/10);
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
//compute for right sonar
- 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);
+ currProba=compute_probability_t(sizeCellX/2+i*sizeCellX,sizeCellY/2+j*sizeCellY,Y+DISTANCE_SONAR_RIGHT_Y,NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_RIGHT_X,ANGLE_FRONT_TO_RIGHT,rightMm/10);
map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
//compute for left sonar
- 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);
+ currProba=compute_probability_t(sizeCellX/2+i*sizeCellX,sizeCellY/2+j*sizeCellY,Y+DISTANCE_SONAR_LEFT_Y,NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_LEFT_X,ANGLE_FRONT_TO_LEFT,leftMm/10);
map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
}
}
@@ -208,7 +293,7 @@
float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){
float alpha=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
- float alphaBeforeAdjustment=alpha-theta-DEFAULT_THETA-angleFromSonarPosition;
+ float alphaBeforeAdjustment=alpha-theta-angleFromSonarPosition;
alpha=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
@@ -250,24 +335,48 @@
}
}
-void print_final_map() {
+void print_final_map_1() {
float currProba;
- for (int x = 0; x<SIZE_MAP; x++) {
- for (int y = 0; y<SIZE_MAP; y++) {
- currProba=log_to_proba(map[SIZE_MAP-1-x][y]);
- if ( currProba < 0.5) {//by default [0][0] would be top left, we want it bottom left
- pc.printf("0");
+ pc.printf("\n\r");
+ for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
+ for (int x= 0; x<NB_CELL_WIDTH; x++) {
+ currProba=log_to_proba(map[x][y]);
+ if ( currProba < 0.5) {
+ pc.printf(" 0 ");
} else {
if(currProba==0.5)
- pc.printf(".");
+ pc.printf(" . ");
else
- pc.printf("+");
+ pc.printf(" + ");
}
}
+ pc.printf("\n\r");
}
}
-
+void print_final_map_2() {
+ float currProba;
+ pc.printf("\n\r");
+ for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
+ for (int x= 0; x<NB_CELL_WIDTH; x++) {
+ currProba=log_to_proba(map[x][y]);
+ if(x => && x <= && y=> && y<= ){
+ pc.printf(" R ");
+ }else{
+ if ( currProba < 0.5) {
+ pc.printf(" 0 ");
+ } else {
+ if(currProba==0.5)
+ pc.printf(" . ");
+ else
+ pc.printf(" + ");
+ }
+ }
+
+ }
+ pc.printf("\n\r");
+ }
+}
//MATHS heavy functions
@@ -277,7 +386,7 @@
return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
}
-//returns the probability [0,1] that the cell is occupied from the log value lt
+//returns the probability [0,1] that the cell is occupied from the log valAue lt
float log_to_proba(float lt){
return 1-1/(1+exp(lt));
}
@@ -322,48 +431,4 @@
return -acos(cosAlpha);
else
return acos(cosAlpha);
-}
-
-void compute_angles_and_distance(float target_x, float target_y, float target_angle){
- alpha = atan2((target_y-Y),(target_x-X))-theta;
- alpha = atan(sin(alpha)/cos(alpha));
- rho = dist(X, Y, target_x, target_y);
- d2 = rho;
- beta = -alpha-theta+target_angle;
-
- //Computing angle error and distance towards the target value
- rho += dt*(-kRho*cos(alpha)*rho);
- temp = alpha;
- alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
- beta += dt*(-kRho*sin(temp));
- pc.printf("\n\r d2=%f", d2);
- pc.printf("\n\r dt=%f", dt);
-}
-
-void compute_linear_angular_velocities(){
- //Computing linear and angular velocities
- if(alpha>=-1.5708 && alpha<=1.5708){
- linear=kRho*rho;
- angular=ka*alpha+kb*beta;
- }
- else{
- linear=-kRho*rho;
- angular=-ka*alpha-kb*beta;
- }
- angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
- angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
-
- //Slowing down at the end for more precision
- if (d2<25) {
- speed = d2*30;
- }
-
- //Normalize speed for motors
- if(angular_left>angular_right) {
- angular_right=speed*angular_right/angular_left;
- angular_left=speed;
- } else {
- angular_left=speed*angular_left/angular_right;
- angular_right=speed;
- }
-}
+}
\ No newline at end of file