Diff: MiniExplorerCoimbra.cpp

Revision:

1:20f48907c726

Parent:

0:9f7ee7ed13e4

Child:

2:11cd5173aa36

--- a/MiniExplorerCoimbra.cpp	Mon Jun 26 12:05:20 2017 +0000
+++ b/MiniExplorerCoimbra.cpp	Mon Jul 03 17:06:16 2017 +0000
@@ -14,11 +14,15 @@
     this->radiusWheels=3.25;
     this->distanceWheels=7.2; 
     
+    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;
+    this->speed=300;
 
     this->rangeForce=30;
     this->attractionConstantForce=10000;
@@ -47,6 +51,70 @@
 		this->thetaWorld=theta+PI/2;
 }
 
+void MiniExplorerCoimbra::go_to_point(float targetXWorld, float targetYWorld) {
+    
+    float angleError; //angle error
+    float d; //distance from target
+    float k_linear=10, k_angular=200;
+    float angularLeft, angularRight, linear, angular;
+    int speed=300;
+
+    do {        
+        //Updating X,Y and theta with the odometry values
+        this->myOdometria();
+
+        //Computing angle error and distance towards the target value
+        angleError = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
+        if(angleError>PI) angleError=-(angleError-PI);
+        else if(angleError<-PI) angleError=-(angleError+PI);
+        pc.printf("\n\r error=%f",angleError);
+        
+        d=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld);
+		pc.printf("\n\r dist=%f/n", d);
+
+        //Computing linear and angular velocities
+        linear=k_linear*d;
+        angular=k_angular*angleError;
+        angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
+        angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
+
+        
+        //Normalize speed for motors
+        if(angularLeft>angularRight) {
+            angularRight=speed*angularRight/angularLeft;
+            angularLeft=speed;
+        } else {
+            angularLeft=speed*angularLeft/angularRight;
+            angularRight=speed;
+        }
+
+        pc.printf("\n\r X=%f", this->xWorld);
+        pc.printf("\n\r Y=%f", this->yWorld);
+        pc.printf("\n\r theta=%f", this->thetaWorld);
+
+        //Updating motor velocities
+        if(angularLeft>0){
+            leftMotor(1,angularLeft);
+        }
+        else{
+            leftMotor(0,-angularLeft);
+        }
+        
+        if(angularRight>0){
+            rightMotor(1,angularRight);
+        }
+        else{
+            rightMotor(0,-angularRight);
+        }
+
+        wait(0.5);
+    } while(d>1);
+
+    //Stop at the end
+    leftMotor(1,0);
+    rightMotor(1,0);
+}
+
 //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
@@ -116,12 +184,46 @@
     rightMotor(1,0);
 }
 
+
+//move of targetXWorld and targetYWorld ending in a targetAngleWorld
+void MiniExplorerCoimbra::go_to_point_with_angle_first_lab(float targetXWorld, float targetYWorld, float targetAngleWorld) {
+    float dt;
+    Timer t;
+    float distanceToTarget;
+    do {
+        //Timer stuff
+        dt = t.read();
+        t.reset();
+        t.start();
+        
+        //Updating X,Y and theta with the odometry values
+        this->myOdometria();
+       	
+        
+        //Updating motor velocities
+        distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
+    
+        wait(0.2);
+        //Timer stuff
+        t.stop();
+        pc.printf("\n\rdist to target= %f",distanceToTarget);
+        
+    } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));
+
+    //Stop at the end
+    leftMotor(1,0);
+    rightMotor(1,0);
+    pc.printf("\r\nReached Target!");
+}
+
 float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt){
     //compute_angles_and_distance
     //atan2 take the deplacement on x and the deplacement on y as parameters
     float angleToPoint = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
-    angleToPoint = atan(sin(angleToPoint)/cos(angleToPoint));
-    //rho is the distance to the point of arrival
+	
+	if(angleToPoint>PI) angleToPoint=-(angleToPoint-PI);
+    else if(angleToPoint<-PI) angleToPoint=-(angleToPoint+PI);    //rho is the distance to the point of arrival
+    
     float rho = dist(targetXWorld,targetYWorld,this->xWorld,this->yWorld);
     float distanceToTarget = rho;
     //TODO check that
@@ -144,21 +246,27 @@
         linear=-this->khro*rho;
         angular=-this->ka*angleToPoint-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;
+        
+    float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
+    float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
+    
+    //Slowing down at the end for more precision
+	if (distanceToTarget<30) {
+    	this->speed = distanceToTarget*10;
+	}
     
     //Normalize speed for motors
-    if(angular_left>angular_right) {
-        angular_right=this->speed*angular_right/angular_left;
-        angular_left=this->speed;
+    if(angularLeft>angularRight) {
+        angularRight=this->speed*angularRight/angularLeft;
+        angularLeft=this->speed;
     } else {
-        angular_left=this->speed*angular_left/angular_right;
-        angular_right=this->speed;
+        angularLeft=this->speed*angularLeft/angularRight;
+        angularRight=this->speed;
     }
 
     //compute_linear_angular_velocities 
-    leftMotor(1,angular_left);
-    rightMotor(1,angular_right);
+    leftMotor(1,angularLeft);
+    rightMotor(1,angularRight);
     
     return distanceToTarget;
 }
@@ -428,6 +536,57 @@
     rightMotor(this->sign2(angularRight),abs(angularRight));
 }
 
+void MiniExplorerCoimbra::go_to_line_first_lab(float line_a, float line_b, float line_c){
+    float lineAngle;
+    float angleError;
+    float linear;
+    float angular;
+    float d;
+    
+    //line angle is beetween pi/2 and -pi/2
+    	
+    if(line_b!=0){
+        lineAngle=atan(line_a/-line_b);
+    }
+    else{
+        lineAngle=1.5708;
+    }
+    
+    do{
+    	this->myOdometria();
+	    //Computing angle error
+	    angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
+	    
+	    if(angleError>PI) angleError=-(angleError-PI);
+	    else if(angleError<-PI) angleError=-(angleError+PI);
+	    
+	    d=this->distFromLine(xWorld, yWorld, line_a, line_b, line_c);
+		
+	    //Calculating velocities
+	    linear= this->kv*(3.14);
+	    angular=-this->kd*d + 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);
+	    }
+	    
+	    pc.printf("\r\nd = %f", d);
+	    pc.printf("\r\nerror = %f\n", angleError);
+	    
+	    leftMotor(this->sign2(angularLeft),abs(angularLeft));
+	    rightMotor(this->sign2(angularRight),abs(angularRight));
+    }while(1);
+}
+
 void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ){
     //get the coordonate of the map and the robot in the ortonormal space
     float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
@@ -458,6 +617,10 @@
         return 0;
 }
 
+float MiniExplorerCoimbra::distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c){
+    return abs((line_a*robot_x+line_b*robot_y+line_c)/sqrt(line_a*line_a+line_b*line_b));
+}
+
 /*
 void MiniExplorerCoimbra::test_procedure_lab_4(){
     this->map.fill_map_with_initial();