Diff: main.cpp

Revision:

52:54b3fe68a4f2

Parent:

51:b863fad80574

--- a/main.cpp	Wed Jun 07 16:25:54 2017 +0000
+++ b/main.cpp	Thu Jun 08 16:13:59 2017 +0000
@@ -36,9 +36,14 @@
 //calculate virtual force field and move
 void vff(bool* reached);
 void test_got_to_line(bool* reached);
+//Go to a given X,Y position, regardless of the final angle
+void go_to_point(float target_x, float target_y);
+//go to line and follow it, from lab 1
+void go_to_line_lab_1(float line_a, float line_b, float line_c);
 
 //MATHS heavy functions
 float dist(float robot_x, float robot_y, float target_x, float target_y);
+float distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c);
 //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]
@@ -155,7 +160,21 @@
 
 int main(){
     initialise_parameters();   
-    procedure_Lab_3();
+    procedure_Lab_3();                      //uses force fields to reach target with set_terget
+    //procedure_Lab_2();                    //picks random targets and makes a map (SUCCESS - builds a more or less accurate map without colliding with obstacles)
+    
+    //theta=0;
+    //X=0;
+    //Y=0;
+    
+    //go_to_point(16.8, 78.6);              //(x,y) in the global coordinates x cm on the x direction, y cm in the y direction form the 0,0 of the table 
+                                            //(SUCCESS - goes to the specified point)
+    
+    //go_to_line_lab_1(10, -10, 20);        //a,b,c of a line: ax+by+c=0, again on the global coordinates of the table (SUCCESS - goes along the line)
+    
+    //go_to_point_with_angle(46.8, 78.6, 0);//(x,y,theta) in the global coordinates (SUCCESS - goes to the point, the angle is almost right as well)
+    
+    //test_procedure_Lab_2();               //starts from the middle of the arena, goes to a point with set_terget
 }
 
 void procedure_Lab_3(){
@@ -203,11 +222,15 @@
     
 }
 void procedure_Lab_2(){
-    for(int i=0;i<25;i++){
+    for(int i=0;i<15;i++){
         randomize_and_map();
         print_final_map_with_robot_position();
         wait(2);
     }
+    while(1){
+        print_final_map_with_robot_position();
+        wait(10);
+    }
 }
 
 
@@ -986,4 +1009,119 @@
             map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
         }
     }
+}
+
+
+//Go to a given X,Y position, regardless of the final angle
+void go_to_point(float target_x, float target_y){
+    float angle_error; //angle error
+    float d; //distance from target
+    float k_linear=10, k_angular=200;
+
+    do {        
+        //Updating X,Y and theta with the odometry values
+        Odometria();
+
+        //Computing angle error and distance towards the target value
+        angle_error = atan2((target_y-Y),(target_x-X))-theta;
+        angle_error = atan(sin(angle_error)/cos(angle_error));
+        d=dist(X, Y, target_x, target_y);
+
+        //Computing linear and angular velocities
+        linear=k_linear*d;
+        angular=k_angular*angle_error;
+        angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+        angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+
+        
+        //Normalize speed for motors
+        if(angular_left>angular_right) {
+            angular_right=MAX_SPEED*angular_right/angular_left;
+            angular_left=MAX_SPEED;
+        } else {
+            angular_left=MAX_SPEED*angular_left/angular_right;
+            angular_right=MAX_SPEED;
+        }
+
+        pc.printf("\n\r X=%f", X);
+        pc.printf("\n\r Y=%f", Y);
+
+        //Updating motor velocities
+        leftMotor(1,angular_left);
+        rightMotor(1,angular_right);
+
+        wait(0.5);
+    } while(d>1);
+
+    //Stop at the end
+    leftMotor(1,0);
+    rightMotor(1,0);
+}
+
+//go to line and follow it, from lab 1
+void go_to_line_lab_1(float line_a, float line_b, float line_c){
+
+    float angle_error; //angle error
+    float d; //distance from line
+    float kd=5, kh=200, kv=200;
+    float linear, angular, angular_left, angular_right;
+    float line_angle;
+     
+    //Check if b=0, if yes line_angle=90
+    if(line_b!=0){
+        line_angle=atan(-line_a/line_b);
+    }
+    else{
+        line_angle=1.5708;
+    }
+
+    do {
+        pc.printf("\n\n\r entered while");
+
+        //Updating X,Y and theta with the odometry values
+        Odometria();
+
+        //Computing angle error and distance from the target line
+        angle_error = line_angle-theta;
+        angle_error = atan(sin(angle_error)/cos(angle_error));
+        d=distFromLine(X, Y, line_a, line_b, line_c);
+        pc.printf("\n\r d=%f", d);
+
+        //Calculating velocities
+        linear=kv*(3.1416-angle_error);
+        angular=-kd*d+kh*angle_error;
+        angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+        angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+
+        //Normalize MAX_SPEED for motors
+        if(angular_left>angular_right) {
+            angular_right=MAX_SPEED*angular_right/angular_left;
+            angular_left=MAX_SPEED;
+        }
+        else {
+            angular_left=MAX_SPEED*angular_left/angular_right;
+            angular_right=MAX_SPEED;
+        }
+        
+        //Updating motor velocities
+        if(angular_left>0){
+            leftMotor(1,angular_left);
+        }
+        else{
+            leftMotor(0,-angular_left);
+        }
+        
+        if(angular_right>0){
+            rightMotor(1,angular_right);
+        }
+        else{
+            rightMotor(0,-angular_right);
+        }
+
+        wait(0.5);
+    } while(1);       
+}
+
+float 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));
 }
\ No newline at end of file