Yo Fleyt
with class
Dependencies: ISR_Mini-explorer mbed
Fork of
VirtualForces
by 
Georgios Tsamis
Diff: main.cpp
- Revision:
- 33:814bcd7d3cfe
- Parent:
- 32:d51928b58645
- Child:
- 34:c208497dd079
--- a/main.cpp Wed May 03 16:46:43 2017 +0000
+++ b/main.cpp Fri Jun 09 00:28:32 2017 +0000
@@ -1,555 +1,8 @@
-#include "mbed.h"
-#include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
-#include "math.h"
-
- using namespace std;
-
-//fill initialLogValues with the values we already know (here the bordurs)
-void fill_initial_log_values();
-//generate a position randomly and makes the robot go there while updating the map
-void randomize_and_map();
-//make the robot do a pi/2 flip
-void do_half_flip();
-//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(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();
-//print the map with the robot marked on it
-void print_final_map_with_robot_position();
-
-//MATHS heavy functions
-float dist(float robot_x, float robot_y, float target_x, float target_y);
-//returns the probability [0,1] that the cell is occupied from the log value lt
-float log_to_proba(float lt);
-//returns the log value that the cell is occupied from the probability value [0,1]
-float proba_to_log(float p);
-//returns the new log value
-float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue );
-//makes the angle inAngle between 0 and 2pi
-float rad_angle_check(float inAngle);
-//returns the angle between the vectors (x,y) and (xs,ys)
-float compute_angle_between_vectors(float x, float y,float xs,float ys);
-float robot_center_x_in_orthonormal_x();
-float robot_center_y_in_orthonormal_y();
-float robot_sonar_front_x_in_orthonormal_x();
-float robot_sonar_front_y_in_orthonormal_y();
-float robot_sonar_right_x_in_orthonormal_x();
-float robot_sonar_right_y_in_orthonormal_y();
-float robot_sonar_left_x_in_orthonormal_x();
-float robot_sonar_left_y_in_orthonormal_y();
-float estimated_width_indice_in_orthonormal_x(int i);
-float estimated_height_indice_in_orthonormal_y(int j);
-void compute_angles_and_distance(float target_x, float target_y, float target_angle);
-void compute_linear_angular_velocities();
-//update foceX and forceY if necessary
-void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho );
-//compute the X and Y force
-void compute_forceX_and_forceY(float targetX, float targetY,float forceX, float forceY);
-
-
-const float pi=3.14159;
-
-//CONSTANT FORCE FIELD
-const float FORCE_CONSTANT_REPULSION=10;//TODO tweak it
-const float FORCE_CONSTANT_ATTRACTION=10;//TODO tweak it
-const float RANGE_FORCE=50;//TODO tweak it
+#include "MiniExplorerCoimbra.hpp"
-//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;//cm
-const float RANGE_SONAR_MIN=10;//Rmin cm
-const float INCERTITUDE_SONAR=10;//cm
-const float ANGLE_SONAR=pi/3;//Omega rad
-
-//those distance and angle are approximation in need of measurements, in the orthonormal frame
-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=-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;
-
-//TODO adjust the size of the map for computation time (25*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) it's in the robot frame
-//this configuration suppose that the robot is in the middle of the arena facing up (to be sure you can use print_final_map_with_robot_position
-const float DEFAULT_X=HEIGHT_ARENA/2;
-const float DEFAULT_Y=WIDTH_ARENA/2;
-const float DEFAULT_THETA=0;
-
-//used to create the map 250 represent the 250cm of the square where the robot is tested
-//float sizeCell=250/(float)SIZE_MAP;
-float sizeCellWidth=WIDTH_ARENA/(float)NB_CELL_WIDTH;
-float sizeCellHeight=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;
-
-const int MAX_SPEED=250;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
-
-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;
+using namespace std;
int main(){
-
- i2c1.frequency(100000);
- initRobot(); //Initializing the robot
- pc.baud(9600); // baud for the pc communication
-
- measure_always_on();//TODO check if needed
- wait(0.5);
-
- fill_initial_log_values();
-
- theta=DEFAULT_THETA;
- X=DEFAULT_X;
- Y=DEFAULT_Y;
-
-
- for (int i = 0; i<10; i++) {
- randomize_and_map();
- //print_final_map();
- print_final_map_with_robot_position();
- }
- while(1){
- print_final_map();
- print_final_map_with_robot_position();
- }
-
-}
-
-//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<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);
- }
- }
-}
-
-//generate a position randomly and makes the robot go there while updating the map
-void randomize_and_map() {
- //TODO check that it's aurelien's work
- float target_x = (rand()%(int)(HEIGHT_ARENA*10))/10;//for decimal precision
- float target_y = (rand()%(int)(WIDTH_ARENA*10))/10;
- float target_angle = 2*((float)(rand()%31416)-15708)/10000.0;
-
- //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);
-}
-
-
-void do_half_flip(){
- Odometria();
- float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
- if(theta_plus_h_pi > pi)
- theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
- leftMotor(0,100);
- rightMotor(1,100);
- while(abs(theta_plus_h_pi-theta)>0.05){
- Odometria();
- // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
- }
- leftMotor(1,0);
- rightMotor(1,0);
-}
-
-//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) {
- Odometria();
- 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));
- bool keep_going=true;
- do {
- //Timer stuff
- dt = t.read();
- t.reset();
- t.start();
-
- //Updating X,Y and theta with the odometry values
- Odometria();
- 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);
-
- //if in dangerzone
- if(frontMm < 120 || leftMm <120 || rightMm <120){
- leftMotor(1,0);
- rightMotor(1,0);
- update_sonar_values(leftMm, frontMm, rightMm);
- //TODO Giorgos maybe you can also test the do_half_flip() function
- Odometria();
- //do a flip TODO
- keep_going=false;
- do_half_flip();
- }else{
- //if not in danger zone continue as usual
- update_sonar_values(leftMm, frontMm, rightMm);
- 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)
-
- //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);
- rightMotor(1,angular_right);
-
- wait(0.2);
- //Timer stuff
- t.stop();
- }
- } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going);
-
- //Stop at the end
- leftMotor(1,0);
- rightMotor(1,0);
-}
-
-//Updates sonar values
-void update_sonar_values(float leftMm, float frontMm, float rightMm){
- float currProba;
- float i_in_orthonormal;
- float j_in_orthonormal;
- 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
- i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i);
- j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j);
-
- 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);
- 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(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);
- map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
- //compute for left sonar
- 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);
- map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
- }
- }
-}
-
-//ODOMETRIA MUST HAVE BEEN CALLED
-//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){
-
- float alpha=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
- 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));
-
- //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
- //check if absolute difference between the angles is no more than Omega/2
- if( distancePointToSonar < (RANGE_SONAR)&& (alpha <= ANGLE_SONAR/2 || alpha >= rad_angle_check(-ANGLE_SONAR/2))){
- if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
- //point before obstacle, probably empty
- /*****************************************************************************/
- float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
- float Er;
- if(distancePointToSonar < RANGE_SONAR_MIN){
- //point before minimum sonar range
- Er=0.f;
- }else{
- //point after minimum sonar range
- Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
- }
- /*****************************************************************************/
- return (1.f-Er*Ea)/2.f;
- }else{
- //probably occupied
- /*****************************************************************************/
- float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
- float Or;
- if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
- //point between distanceObstacleDetected +- INCERTITUDE_SONAR
- Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
- }else{
- //point after in range of the sonar but after the zone detected
- Or=0;
- }
- /*****************************************************************************/
- return (1+Or*Oa)/2;
- }
- }else{
- //not checked by the sonar
- return 0.5;
- }
+ MiniExplorerCoimbra myRobot(0,0,0);
+ return 0;
}
-
-void print_final_map() {
- 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 ( currProba < 0.5) {
- pc.printf(" ");
- } else {
- if(currProba==0.5)
- pc.printf(" . ");
- else
- pc.printf(" X ");
- }
- }
- pc.printf("\n\r");
- }
-}
-
-void print_final_map_with_robot_position() {
- float currProba;
- Odometria();
- float Xrobot=robot_center_x_in_orthonormal_x();
- float Yrobot=robot_center_y_in_orthonormal_y();
-
- float heightIndiceInOrthonormal;
- float widthIndiceInOrthonormal;
-
- float widthMalus=-(3*sizeCellWidth/2);
- float widthBonus=sizeCellWidth/2;
-
- float heightMalus=-(3*sizeCellHeight/2);
- float heightBonus=sizeCellHeight/2;
-
- pc.printf("\n\r");
- for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
- for (int x= 0; x<NB_CELL_WIDTH; x++) {
- heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
- widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
- if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
- pc.printf(" R ");
- else{
- currProba=log_to_proba(map[x][y]);
- if ( currProba < 0.5)
- pc.printf(" ");
- else{
- if(currProba==0.5)
- pc.printf(" . ");
- else
- pc.printf(" X ");
- }
- }
- }
- pc.printf("\n\r");
- }
-}
-
-//MATHS heavy functions
-/**********************************************************************/
-//Distance computation function
-float dist(float robot_x, float robot_y, float target_x, float target_y){
- 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 valAue lt
-float log_to_proba(float lt){
- return 1-1/(1+exp(lt));
-}
-
-//returns the log value that the cell is occupied from the probability value [0,1]
-float proba_to_log(float p){
- return log(p/(1-p));
-}
-
-//returns the new log value
-float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){
- return previousLogValue+proba_to_log(currentProbability)-originalLogvalue;
-}
-
-//makes the angle inAngle between 0 and 2pi
-float rad_angle_check(float inAngle){
- //cout<<"before :"<<inAngle;
- if(inAngle > 0){
- while(inAngle > (2*pi))
- inAngle-=2*pi;
- }else{
- while(inAngle < 0)
- inAngle+=2*pi;
- }
- //cout<<" after :"<<inAngle<<endl;
- return inAngle;
-}
-
-//returns the angle between the vectors (x,y) and (xs,ys)
-float compute_angle_between_vectors(float x, float y,float xs,float ys){
- //alpha angle between ->x and ->SA
- //vector S to A ->SA
- float vSAx=x-xs;
- float vSAy=y-ys;
- //norme SA
- float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
- //vector ->x (1,0)
- float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
- //vector ->y (0,1)
- float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
- if (sinAlpha < 0)
- return -acos(cosAlpha);
- else
- return acos(cosAlpha);
-}
-
-float robot_center_x_in_orthonormal_x(){
- return NB_CELL_WIDTH*sizeCellWidth-Y;
-}
-
-float robot_center_y_in_orthonormal_y(){
- return X;
-}
-
-float robot_sonar_front_x_in_orthonormal_x(){
- return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_FRONT_X;
-}
-float robot_sonar_front_y_in_orthonormal_y(){
- return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_FRONT_Y;
-}
-
-float robot_sonar_right_x_in_orthonormal_x(){
- return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_RIGHT_X;
-}
-float robot_sonar_right_y_in_orthonormal_y(){
- return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_RIGHT_Y;
-}
-
-float robot_sonar_left_x_in_orthonormal_x(){
- return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_LEFT_X;
-}
-float robot_sonar_left_y_in_orthonormal_y(){
- return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_LEFT_Y;
-}
-
-float estimated_width_indice_in_orthonormal_x(int i){
- return sizeCellWidth/2+i*sizeCellWidth;
-}
-float estimated_height_indice_in_orthonormal_y(int j){
- return sizeCellHeight/2+j*sizeCellHeight;
-}
-
-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;
- }
-}
-
-
-void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){
-
- //get the coordonate of the map and the robot in the ortonormal frame
- float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice);
- float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice);
- //compute the distance beetween the cell and the robot
- float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2));
- //check if the cell is in range
- if(distanceCellToRobot <= (range)) {
- float probaCell=log_to_proba(map[widthIndice][heightIndice]);
- float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3);
- float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3);
- *forceX+=xForceComputed;
- *forceY+=yForceComputed;
- }
-}
-
-void compute_forceX_and_forceY(float targetX, float targetY,float forceX, float forceY){
- float xRobotOrtho=robot_center_x_in_orthonormal_x();
- float yRobotOrtho=robot_center_y_in_orthonormal_y();
- for(int i=0;i<NB_CELL_WIDTH;i++){
- for(int j=0;j<NB_CELL_HEIGHT;j++){
- updateForce(i,j,RANGE_FORCE,&forceX,&forceY,xRobotOrtho,yRobotOrtho);
- }
- }
- //update with attraction force
- forceX+=FORCE_CONSTANT_ATTRACTION*(targetX-xRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2));
- forceY+=FORCE_CONSTANT_ATTRACTION*(targetY-yRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2));
- float amplitude=sqrt(pow(forceX,2)+pow(forceY,2));
- forceX=forceX/amplitude;
- forceY=forceY/amplitude;
-}
\ No newline at end of file