with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 33:814bcd7d3cfe
- Child:
- 34:c208497dd079
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MiniExplorerCoimbra.cpp Fri Jun 09 00:28:32 2017 +0000 @@ -0,0 +1,458 @@ +#include "MiniExplorerCoimbra.hpp" + +#define PI 3.14159 + +MiniExplorerCoimbra:: MiniExplorerCoimbra(float defaultX, float defaultY, float defaultTheta):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){ + i2c1.frequency(100000); + initRobot(); //Initializing the robot + pc.baud(9600); // baud for the pc communication + + measure_always_on();//TODO check if needed + + this->setXYTheta(defaultX,defaultY,defaultTheta); + this->radiusWheels=3.25; + this->distanceWheels=7.2; + + this->khro=12; + this->ka=30; + this->kb=-13; + this->kv=200; + this->kh=200; + + this->rangeForce=30; + this->attractionConstantForce=10000; + this->repulsionConstantForce=1; +} + +void MiniExplorerCoimbra::setXYTheta(float x, float y, float theta){ + X=x; + Y=y; + theta=theta; +} + +//generate a position randomly and makes the robot go there while updating the map +void MiniExplorerCoimbra::randomize_and_map() { + //TODO check that it's aurelien's work + float movementOnX=rand()%(int)(this->map.widthRealMap/2); + float movementOnY=rand()%(int)(this->map.heightRealMap/2); + + float signOfX=rand()%2; + if(signOfX < 1) + signOfX=-1; + float signOfY=rand()%2; + if(signOfY < 1) + signOfY=-1; + + float targetX = movementOnX*signOfX; + float targetY = movementOnY*signOfY; + float targetAngle = 2*((float)(rand()%31416)-15708)/10000.0; + this->go_to_point_with_angle(targetX, targetY, targetAngle); +} + +//go the the given position while updating the map +void MiniExplorerCoimbra::go_to_point_with_angle(float targetX, float targetY, float targetAngle) { + bool keep_going=true; + float leftMm; + float frontMm; + float rightMm; + float dt; + Timer t; + float d2; + 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(); + + //if in dangerzone + if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){ + leftMotor(1,0); + rightMotor(1,0); + this->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; + this->do_half_flip(); + }else{ + //if not in danger zone continue as usual + this->update_sonar_values(leftMm, frontMm, rightMm); + + //Updating motor velocities + d2=this->update_angular_speed_wheels_go_to_point_with_angle(targetX,targetY,targetAngle,dt); + + wait(0.2); + //Timer stuff + t.stop(); + pc.printf("\n\r dist to target= %f",d2); + } + } while(d2>1 && (abs(targetAngle-theta)>0.01) && keep_going); + + //Stop at the end + leftMotor(1,0); + rightMotor(1,0); +} + +void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){ + float xWorldCell; + float yWorldCell; + for(int i=0;i<this->map.nbCellWidth;i++){ + for(int j=0;j<this->map.nbCellHeight;j++){ + xWorldCell=this->map.cell_width_coordinate_to_world(i); + yWorldCell=this->map.cell_height_coordinate_to_world(j); + //compute_probability_t(float distanceObstacleDetected, float x, float y, float[2] robotCoordinatesInWorld) + this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm,xWorldCell,yWorldCell,this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world())); + this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world())); + this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world())); + + } + } +} + +float MiniExplorerCoimbra::get_converted_robot_X_into_world(){ + //x world coordinate + return this->map.nbCellWidth*this->map.sizeCellWidth-Y; +} + +float MiniExplorerCoimbra::get_converted_robot_Y_into_world(){ + //y worldcoordinate + return X; +} + +void MiniExplorerCoimbra::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); +} + +//Distance computation function +float MiniExplorerCoimbra::dist(float robotX, float robotY, float targetX, float targetY){ + //pc.printf("%f, %f, %f, %f",robotX,robotY,targetX,targetY); + return sqrt(pow(targetY-robotY,2) + pow(targetX-robotX,2)); +} + +float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetX, float targetY, float targetAngle, float dt){ + //compute_angles_and_distance + float alpha = atan2((targetY-Y),(targetX-X))-theta; + alpha = atan(sin(alpha)/cos(alpha)); + float rho = this->dist(X, Y, targetX, targetY); + float d2 = rho; + float beta = -alpha-theta+targetAngle; + + //Computing angle error and distance towards the target value + rho += dt*(-this->khro*cos(alpha)*rho); + float temp = alpha; + alpha += dt*(this->khro*sin(alpha)-this->ka*alpha-this->kb*beta); + beta += dt*(-this->khro*sin(temp)); + + //Computing linear and angular velocities + float linear; + float angular; + if(alpha>=-1.5708 && alpha<=1.5708){ + linear=this->khro*rho; + angular=this->ka*alpha+this->kb*beta; + } + else{ + linear=-this->khro*rho; + angular=-this->ka*alpha-this->kb*beta; + } + float angular_left=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels; + float angular_right=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels; + + //Normalize speed for motors + if(angular_left>angular_right) { + angular_right=this->speed*angular_right/angular_left; + angular_left=this->speed; + } else { + angular_left=this->speed*angular_left/angular_right; + angular_right=this->speed; + } + + //compute_linear_angular_velocities + leftMotor(1,angular_left); + rightMotor(1,angular_right); + + return d2; +} + +void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){ + //atan2 gives the angle beetween PI and -PI + float angleToTarget=atan2(targetXWorld,targetYWorld); + turn_to_target(angleToTarget); + bool reached=false; + int print=0; + while (!reached) { + vff(&reached,targetXWorld,targetYWorld); + //test_got_to_line(&reached); + if(print==10){ + leftMotor(1,0); + rightMotor(1,0); + /* + pc.printf("\r\n theta=%f", theta); + pc.printf("\r\n IN ORTHO:"); + pc.printf("\r\n X Robot=%f", this->get_converted_robot_X_into_world()); + pc.printf("\r\n Y Robot=%f", this->get_converted_robot_Y_into_world()); + pc.printf("\r\n X Target=%f", targetXWorld); + pc.printf("\r\n Y Target=%f", targetYWorld); + */ + print_final_map_with_robot_position_and_target(); + print=0; + }else + print+=1; + } + //Stop at the end + leftMotor(1,0); + rightMotor(1,0); + pc.printf("\r\n target reached"); + wait(3);// +} + +void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){ + float line_a; + float line_b; + float line_c; + //Updating X,Y and theta with the odometry values + float forceX=0; + float forceY=0; + //we update the odometrie + Odometria(); + //we check the sensors + float leftMm = get_distance_left_sensor(); + float frontMm = get_distance_front_sensor(); + float rightMm = get_distance_right_sensor(); + //update the probabilities values + this->update_sonar_values(leftMm, frontMm, rightMm); + //we compute the force on X and Y + this->compute_forceX_and_forceY(&forceX, &forceY,targetXWorld,targetYWorld); + //we compute a new ine + this->calculate_line(forceX, forceY, &line_a,&line_b,&line_c); + //Updating motor velocities + this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld); + + //wait(0.1); + Odometria(); + if(dist(this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world(),targetXWorld,targetYWorld)<10) + *reached=true; +} + +//compute the force on X and Y +void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceX, float* forceY, float targetXWorld, float targetYWorld){ + float xRobotWorld=this->get_converted_robot_X_into_world(); + float yRobotWorld=this->get_converted_robot_Y_into_world(); + + float forceRepulsionComputedX=0; + float forceRepulsionComputedY=0; + //for each cell of the map we compute a force of repulsion + for(int i=0;i<this->map.nbCellWidth;i++){ + for(int j=0;j<this->map.nbCellHeight;j++){ + this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotWorld,yRobotWorld); + } + } + //update with attraction force + *forceX=+forceRepulsionComputedX; + *forceY=+forceRepulsionComputedY; + float distanceTargetRobot=sqrt(pow(targetXWorld-xRobotWorld,2)+pow(targetYWorld-yRobotWorld,2)); + if(distanceTargetRobot != 0){ + *forceX-=this->attractionConstantForce*(targetXWorld-xRobotWorld)/distanceTargetRobot; + *forceY-=this->attractionConstantForce*(targetYWorld-yRobotWorld)/distanceTargetRobot; + } + float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2)); + if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0 + *forceX=*forceX/amplitude; + *forceY=*forceY/amplitude; + } +} + +void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceX, float* forceY, float xRobotWorld, float yRobotWorld ){ + //get the coordonate of the map and the robot in the ortonormal space + float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice); + float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice); + + //compute the distance beetween the cell and the robot + float distanceCellToRobot=sqrt(pow(xWorldCell-xRobotWorld,2)+pow(yWorldCell-yRobotWorld,2)); + //check if the cell is in range + if(distanceCellToRobot <= this->rangeForce) { + float probaCell=this->map.get_proba_cell(widthIndice,heightIndice); + float xForceComputed=this->repulsionConstantForce*probaCell*(xWorldCell-xRobotWorld)/pow(distanceCellToRobot,3); + float yForceComputed=this->repulsionConstantForce*probaCell*(yWorldCell-yRobotWorld)/pow(distanceCellToRobot,3); + *forceX+=xForceComputed; + *forceY+=yForceComputed; + } +} + +//robotX and robotY are from Odometria, calculate line_a, line_b and line_c +void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){ + /* + in the teachers maths it is + + *line_a=forceY; + *line_b=-forceX; + + because a*x+b*y+c=0 + a impact the vertical and b the horizontal + and he has to put them like this because + Robot space: orthonormal space: + ^ ^ + |x |y + <- R O -> + y x + but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to + */ + *line_a=forceX; + *line_b=forceY; + //because the line computed always pass by the robot center we dont need lince_c + //*line_c=forceX*yRobotWorld+forceY*xRobotWorld; + *line_c=0; +} + +//currently line_c is not used +void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){ + float lineAngle; + float angleError; + float linear; + float angular; + + bool direction=true; + + //take as parameter how much the robot is supposed to move on x and y (world) + float angleToTarget=atan2(targetXWorld-this->get_converted_robot_X_into_world(),targetYWorld-this->get_converted_robot_Y_into_world()); + bool inFront=true; + if(angleToTarget < 0)//the target is in front + inFront=false; + + if(theta > 0){ + //the robot is oriented to the left + if(theta > PI/2){ + //the robot is oriented lower left + if(inFront) + direction=false;//robot and target not oriented the same way + }else{ + //the robot is oriented upper left + if(!inFront) + direction=false;//robot and target not oriented the same way + } + }else{ + //the robot is oriented to the right + if(theta < -PI/2){ + //the robot is oriented lower right + if(inFront) + direction=false;//robot and target not oriented the same way + }else{ + //the robot is oriented upper right + if(!inFront) + direction=false;//robot and target not oriented the same way + } + } + //pc.printf("\r\n Target is in front"); + + if(line_b!=0){ + if(!direction) + lineAngle=atan(-line_a/line_b); + else + lineAngle=atan(line_a/-line_b); + } + else{ + lineAngle=1.5708; + } + + //Computing angle error + angleError = lineAngle-theta; + angleError = atan(sin(angleError)/cos(angleError)); + + //Calculating velocities + linear=this->kv*(3.1416); + angular=this->kh*angleError; + + float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels; + float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels; + + //Normalize speed for motors + if(abs(angularLeft)>abs(angularRight)) { + angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight); + angularLeft=this->speed*this->sign1(angularLeft); + } + else { + angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft); + angularRight=this->speed*this->sign1(angularRight); + } + leftMotor(this->sign2(angularLeft),abs(angularLeft)); + rightMotor(this->sign2(angularRight),abs(angularRight)); +} + +//return 1 if positiv, -1 if negativ +float MiniExplorerCoimbra::sign1(float value){ + if(value>=0) + return 1; + else + return -1; +} + +//return 1 if positiv, 0 if negativ +int MiniExplorerCoimbra::sign2(float value){ + if(value>=0) + return 1; + else + return 0; +} + +/*angleToTarget is obtained through atan2 so it s: +< 0 if the angle is bettween PI and 2pi on a trigo circle +> 0 if it is between 0 and PI +*/ +void MiniExplorerCoimbra::turn_to_target(float angleToTarget){ + 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); + if(angleToTarget>0){ + leftMotor(0,1); + rightMotor(1,1); + }else{ + leftMotor(1,1); + rightMotor(0,1); + } + while(abs(angleToTarget-theta_plus_h_pi)>0.05){ + Odometria(); + 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); + //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi)); + } + leftMotor(1,0); + rightMotor(1,0); +} + + +/* +//x and y passed are TargetNotMap +float get_error_angles(float x, float y,float theta){ + float angleBeetweenRobotAndTarget=atan2(y,x); + if(y > 0){ + if(angleBeetweenRobotAndTarget < PI/2)//up right + angleBeetweenRobotAndTarget=-PI/2+angleBeetweenRobotAndTarget; + else//up right + angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2; + }else{ + if(angleBeetweenRobotAndTarget > -PI/2)//lower right + angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2; + else//lower left + angleBeetweenRobotAndTarget=2*PI+angleBeetweenRobotAndTarget-PI/2; + } + return angleBeetweenRobotAndTarget-theta; +} +*/