Diff: main.cpp

Revision:

34:128fc7aed957

Parent:

33:78139f82ea74

Child:

35:c8f224ab153f

--- a/main.cpp	Thu May 04 15:21:24 2017 +0000
+++ b/main.cpp	Thu May 04 16:43:04 2017 +0000
@@ -2,7 +2,7 @@
 #include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
 #include "math.h"
 
- using namespace std;
+using namespace std;
 
 //fill initialLogValues with the values we already know (here the bordurs)
 void fill_initial_log_values();
@@ -52,13 +52,13 @@
 //update foceX and forceY if necessary
 void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho );
 //compute the X and Y force
-void compute_forceX_and_forceY(float targetX, float targetY,float forceX, float forceY);
+void compute_forceX_and_forceY(float targetX, float targetY,float* forceX, float* forceY);
 
 
 const float pi=3.14159;
 
 //CONSTANT FORCE FIELD
-const float FORCE_CONSTANT_REPULSION=10;//TODO tweak it
+const float FORCE_CONSTANT_REPULSION=50;//TODO tweak it
 const float FORCE_CONSTANT_ATTRACTION=10;//TODO tweak it
 const float RANGE_FORCE=50;//TODO tweak it
 
@@ -108,7 +108,7 @@
 const float RADIUS_WHEELS=3.25;
 const float DISTANCE_WHEELS=7.2;
 
-const int MAX_SPEED=250;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
+const int MAX_SPEED=100;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
 
 float alpha; //angle error
 float rho; //distance from target
@@ -130,8 +130,8 @@
 float line_b;
 float line_c;
 
-float targetX;
-float targetY;
+float targetX=46.8;
+float targetY=78.6;
 
 bool reached=false;
 
@@ -147,8 +147,8 @@
     fill_initial_log_values();
 
     theta=DEFAULT_THETA;
-    X=DEFAULT_X;
-    Y=DEFAULT_Y;
+    X=5;//DEFAULT_X;
+    Y=5;//DEFAULT_Y;
         
     while (!reached) {
         vff(); 
@@ -552,20 +552,22 @@
     }
 }
 
-void compute_forceX_and_forceY(float targetX, float targetY,float forceX, float forceY){
+void compute_forceX_and_forceY(float targetX, float targetY,float* forceX, float* forceY){
      float xRobotOrtho=robot_center_x_in_orthonormal_x();
      float yRobotOrtho=robot_center_y_in_orthonormal_y();
      for(int i=0;i<NB_CELL_WIDTH;i++){
         for(int j=0;j<NB_CELL_HEIGHT;j++){
-            updateForce(i,j,RANGE_FORCE,&forceX,&forceY,xRobotOrtho,yRobotOrtho);
+            updateForce(i,j,RANGE_FORCE,forceX,forceY,xRobotOrtho,yRobotOrtho);
         }
     }
-    //update with attraction force  
-    forceX+=FORCE_CONSTANT_ATTRACTION*(targetX-xRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2));
-    forceY+=FORCE_CONSTANT_ATTRACTION*(targetY-yRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2));
-    float amplitude=sqrt(pow(forceX,2)+pow(forceY,2));
-    forceX=forceX/amplitude;
-    forceY=forceY/amplitude;
+    //update with attraction force
+    *forceX=-*forceX;
+    *forceY=-*forceY;
+    *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX-X)/sqrt(pow(targetX-X,2)+pow(targetY-Y,2));
+    *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY-Y)/sqrt(pow(targetX-X,2)+pow(targetY-Y,2));
+    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){
@@ -584,17 +586,26 @@
     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);
+    updateForce(WIDTH_ARENA, HEIGHT_ARENA, RANGE_FORCE, &forceX, &forceY, robot_center_x_in_orthonormal_x(), robot_center_y_in_orthonormal_y()); //TODO check range value, I randomly put 20
+    compute_forceX_and_forceY(targetX, targetY,&forceX, &forceY);
     calculate_line(forceX, forceY, X, Y);
+    go_to_line();
+    pc.printf("\r\n forceX=%f", forceX);
+    pc.printf("\r\n forceY=%f", forceY);
+    pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c);
 
     //Updating motor velocities
     leftMotor(1,angular_left);
     rightMotor(1,angular_right);
+    
+    pc.printf("\r\n angR=%f", angular_right);
+    pc.printf("\r\n angL=%f", angular_left);
+    pc.printf("\r\n dist=%f", dist(X,Y,targetX,targetY));
 
-    wait(0.2);
+
+    wait(0.1);
     Odometria();
-    if(dist(X,Y,targetX,targetY)<0.5)
+    if(dist(X,Y,targetX,targetY)<10)
         reached=true;
 }