Diff: main.cpp

Revision:

33:78139f82ea74

Parent:

32:d51928b58645

Child:

34:128fc7aed957

--- a/main.cpp	Wed May 03 16:46:43 2017 +0000
+++ b/main.cpp	Thu May 04 15:21:24 2017 +0000
@@ -20,6 +20,10 @@
 void print_final_map();
 //print the map with the robot marked on it
 void print_final_map_with_robot_position();
+//go to a given line
+void go_to_line();
+//calculate virtual force field and move
+void vff();
 
 //MATHS heavy functions
 float dist(float robot_x, float robot_y, float target_x, float target_y);
@@ -109,7 +113,7 @@
 float alpha; //angle error
 float rho; //distance from target
 float beta;
-float kRho=12, ka=30, kb=-13; //Kappa values
+float kRho=12, ka=30, kb=-13, kv=200, kh=200; //Kappa values //TODO check kv, kh for go_to_line
 float linear, angular, angular_left, angular_right;
 float dt=0.5;
 float temp;
@@ -120,7 +124,16 @@
 
 float leftMm;
 float frontMm;
-float rightMm; 
+float rightMm;
+
+float line_a;
+float line_b;
+float line_c;
+
+float targetX;
+float targetY;
+
+bool reached=false;
 
 int main(){
     
@@ -136,18 +149,19 @@
     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();
+        
+    while (!reached) {
+        vff(); 
         print_final_map_with_robot_position();
     }
-
+    
+    //Stop at the end
+    leftMotor(1,0);
+    rightMotor(1,0);
+    
+    while(1){
+         print_final_map_with_robot_position();
+    }
 }
 
 //fill initialLogValues with the values we already know (here the bordurs)
@@ -552,4 +566,64 @@
     float amplitude=sqrt(pow(forceX,2)+pow(forceY,2));
     forceX=forceX/amplitude;
     forceY=forceY/amplitude;
+}
+
+void calculate_line(float forceX, float forceY, float robotX, float robotY){
+    line_a=forceY;
+    line_b=-forceX;
+    line_c=forceX*robotY-forceY*robotX;    
+}
+
+void vff(){
+    //Updating X,Y and theta with the odometry values
+    float forceX, forceY;
+    Odometria();
+    
+    leftMm = get_distance_left_sensor();
+    frontMm = get_distance_front_sensor();
+    rightMm = get_distance_right_sensor();
+    update_sonar_values(leftMm, frontMm, rightMm);
+    
+    updateForce(WIDTH_ARENA, HEIGHT_ARENA, 20, &forceX, &forceY, X, Y); //TODO check range value, I randomly put 20
+    compute_forceX_and_forceY(targetX, targetY,forceX, forceY);
+    calculate_line(forceX, forceY, X, Y);
+
+    //Updating motor velocities
+    leftMotor(1,angular_left);
+    rightMotor(1,angular_right);
+
+    wait(0.2);
+    Odometria();
+    if(dist(X,Y,targetX,targetY)<0.5)
+        reached=true;
+}
+
+void go_to_line(){
+    float line_angle, angle_error;
+    if(line_b!=0){
+        line_angle=atan(-line_a/line_b);
+    }
+    else{
+        line_angle=1.5708;
+    }
+
+    //Computing angle error
+    angle_error = line_angle-theta;
+    angle_error = atan(sin(angle_error)/cos(angle_error));
+
+    //Calculating velocities
+    linear=kv*(3.1416);
+    angular=kh*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=speed*angular_right/angular_left;
+        angular_left=speed;
+    }
+    else {
+        angular_left=speed*angular_left/angular_right;
+        angular_right=speed;
+    }
 }
\ No newline at end of file