Yo Fleyt
test morning
Dependencies: ISR_Mini-explorer mbed
Fork of
roboticLab_withclass_3_July
by 
Georgios Tsamis
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 8:072a76960e27
- Parent:
- 6:0e8db3a23486
- Child:
- 9:1cc27f33d3e1
--- a/MiniExplorerCoimbra.cpp Fri Jul 07 11:50:28 2017 +0000
+++ b/MiniExplorerCoimbra.cpp Mon Jul 10 12:49:07 2017 +0000
@@ -3,7 +3,7 @@
#define PI 3.14159
-MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap):map(widthRealMap,heightRealMap,18,12),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
+MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap):map(widthRealMap,heightRealMap,12,8),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
@@ -14,14 +14,20 @@
this->radiusWheels=3.25;
this->distanceWheels=7.2;
+ //go to point
this->k_linear=10;
this->k_angular=200;
- this->khro=12;
- this->ka=30;
- this->kb=-13;
- this->kv=200;
- this->kh=200;
- this->kd=5;//previous 5
+
+ //go to point with angle
+ this->khro=0.3;
+ this->ka=0.8;
+ this->kb=-0.36;
+ this->kv=200;//velocity
+
+ //go to line kh > 0, kd > 0 (200,5), dont turn to line enough, (10,10) turn on itself,
+ this->kh=650;
+ this->kd=10;//previous 5
+
this->speed=300;
this->rangeForce=50;
@@ -29,8 +35,8 @@
//not too bad values 200 and -20000
//not too bad values 500 and -20000
//not too bad values 500 and -25000 rangeForce 50
- this->attractionConstantForce=600;
- this->repulsionConstantForce=0;
+ this->attractionConstantForce=1;
+ this->repulsionConstantForce=-90;//-90 ok //idea make repulsion less divaded by distance
this->covariancePositionEstimationK[0][0]=0;
this->covariancePositionEstimationK[0][1]=0;
@@ -72,7 +78,6 @@
float angleError; //angle error
float d; //distance from target
- float k_linear=10, k_angular=200;
float angularLeft, angularRight, linear, angular;
int speed=300;
@@ -425,11 +430,14 @@
float rightMm = get_distance_right_sensor();
//update the probabilities values
this->update_sonar_values(leftMm, frontMm, rightMm);
+ this->myOdometria();
//we compute the force on X and Y
this->compute_forceX_and_forceY(&forceXWorld, &forceYWorld,targetXWorld,targetYWorld);
//we compute a new ine
this->calculate_line(forceXWorld, forceYWorld, &line_a,&line_b,&line_c);
//Updating motor velocities
+ //pc.printf("\r\nX=%f;Y=%f",xWorld,yWorld);
+ //pc.printf("\r\n%f*x+%f*y+%f=0",line_a,line_b,line_c);
this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
//wait(0.1);
@@ -567,14 +575,15 @@
*line_a=forceY;
*line_b=-forceX;
//because the line computed always pass by the robot center we dont need lince_c
- //*line_c=forceX*this->yWorld+forceY*this->xWorld;
- *line_c=0;
+ *line_c=forceX*this->yWorld+forceY*this->xWorld;
+ //*line_c=0;
}
//compute the force on X and Y
void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld){
float forceRepulsionComputedX=0;
float forceRepulsionComputedY=0;
- for(int i=0;i<this->map.nbCellWidth;i++){ //for each cell of the map we compute a force of repulsion
+ this->print_map_with_robot_position();
+ for(int i=0;i<this->map.nbCellWidth;i++){ //for each cell of the map we compute a force of repulsion
for(int j=0;j<this->map.nbCellHeight;j++){
this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY);
}
@@ -582,27 +591,27 @@
//update with attraction force
*forceXWorld=forceRepulsionComputedX;
*forceYWorld=forceRepulsionComputedY;
- this->print_map_with_robot_position();
- pc.printf("\r\nForce X repul:%f",*forceXWorld);
- pc.printf("\r\nForce Y repul:%f",*forceYWorld);
+ //this->print_map_with_robot_position();
+ //pc.printf("\r\nForce X repul:%f",*forceXWorld);
+ //pc.printf("\r\nForce Y repul:%f",*forceYWorld);
float distanceTargetRobot=sqrt(pow(targetXWorld-this->xWorld,2)+pow(targetYWorld-this->yWorld,2));
if(distanceTargetRobot != 0){
- *forceXWorld+=-this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot;
- *forceYWorld+=-this->attractionConstantForce*(targetYWorld-this->yWorld)/distanceTargetRobot;
+ *forceXWorld+=this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot;
+ *forceYWorld+=this->attractionConstantForce*(targetYWorld-this->yWorld)/distanceTargetRobot;
}else{
- *forceXWorld+=-this->attractionConstantForce*(targetXWorld-this->xWorld)/0.01;
- *forceYWorld+=-this->attractionConstantForce*(targetYWorld-this->yWorld)/0.01;
+ *forceXWorld+=this->attractionConstantForce*(targetXWorld-this->xWorld)/0.001;
+ *forceYWorld+=this->attractionConstantForce*(targetYWorld-this->yWorld)/0.001;
}
- pc.printf("\r\nForce X after attract:%f",*forceXWorld);
- pc.printf("\r\nForce Y after attract:%f",*forceYWorld);
+ //pc.printf("\r\nForce X after attract:%f",*forceXWorld);
+ //pc.printf("\r\nForce Y after attract:%f",*forceYWorld);
float amplitude=sqrt(pow(*forceXWorld,2)+pow(*forceYWorld,2));
if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
*forceXWorld=*forceXWorld/amplitude;
*forceYWorld=*forceYWorld/amplitude;
}else{
- *forceXWorld=*forceXWorld/0.01;
- *forceYWorld=*forceYWorld/0.01;
+ *forceXWorld=*forceXWorld/0.001;
+ *forceYWorld=*forceYWorld/0.001;
}
}
@@ -634,6 +643,7 @@
//d=this->distFromLine(this->xWorld, this->yWorld, line_a, line_b, line_c);//this could be 0
d=0;
+ //pc.printf("\r\ndistance from line:%f",d);
//Calculating velocities
linear= this->kv*(3.14);
angular=-this->kd*d + this->kh*angleError;
@@ -721,12 +731,12 @@
float distanceCellToRobot=sqrt(pow(xWorldCell-this->xWorld,2)+pow(yWorldCell-this->yWorld,2));
float probaCell;
//check if the cell is in range
- float anglePointToRobot=atan2(yWorldCell-this->yWorld,xWorldCell-this->xWorld);//like world system
+ //float anglePointToRobot=atan2(yWorldCell-this->yWorld,xWorldCell-this->xWorld);//like world system
float temp1;
float temp2;
if(distanceCellToRobot <= this->rangeForce) {
probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
- pc.printf("\r\nupdate force proba:%f",probaCell);
+ //pc.printf("\r\nupdate force proba:%f",probaCell);
temp1=this->repulsionConstantForce*probaCell/pow(distanceCellToRobot,2);
temp2=(xWorldCell-this->xWorld)/distanceCellToRobot;
*forceRepulsionComputedX+=temp1*temp2;
@@ -1075,7 +1085,7 @@
//S for each sonar (concatenated covariance matrix of innovation)
float innovationCovarianceFront=R_front+ jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]);
- float innovationCovarianceLeft=R_left+ jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]);
+ float innovationCovarianceLeft=R_left + jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]);
float innovationCovarianceRight=R_right+ jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]);
//check if it pass the validation gate
@@ -1101,7 +1111,7 @@
this->print_map_with_robot_position();
- pc.printf("\r\n E_LCm=%f, E_FCm=%f, E_RCm=%f",frontCmEstimated,leftCmEstimated,rightCmEstimated);
+ pc.printf("\r\n E_LCm=%f, E_FCm=%f, E_RCm=%f",leftCmEstimated,frontCmEstimated,rightCmEstimated);
pc.printf("\r\n Lcm=%f, FCm=%f, RCm=%f",leftCm,frontCm,rightCm);
pc.printf("\r\n IL=%f, IF=%f, IR=%f",innovationLeft,innovationFront,innovationRight);
pc.printf("\r\n ICL=%f, ICF=%f, ICR=%f",innovationCovarianceLeft,innovationCovarianceFront,innovationCovarianceRight);