Diff: main.cpp

Revision:

8:109314be5b68

Parent:

7:c94070f9af78

Child:

9:b7138acdf4ac

Child:

11:e641aa08c92e

--- a/main.cpp	Fri Mar 24 18:02:57 2017 +0000
+++ b/main.cpp	Mon Mar 27 16:20:29 2017 +0000
@@ -6,30 +6,32 @@
 
 float dist(float robot_x, float robot_y, float target_x, float target_y);
 
+int goToPointWithAngle(float target_x, float target_y, int theta);
+
+float alpha; //angle error
+float rho; //distance from target
+float beta;
+//float k_linear=10, k_angular=200;
+//kb = -15  and ka = 30 tabom
+float kRho=12, ka=30, kb=-13;
+float linear, angular, angular_left, angular_right;
+float dt=0.5;
+float temp;
+float d2;
+
+//Diameter of a wheel and distance between the 2
+float r=3.25, b=7.2;
+
+int speed=999; // Max speed at beggining of movement
+
+//Target example x,y values
+float target_x=46.8, target_y=78.6, target_angle=1.7;
+
 //Timeout time;
 int main(){
     initRobot(); //Initializing the robot
     pc.baud(9600); // baud for the pc communication
 
-    //Target example x,y values
-    float target_x=46.8, target_y=78.6, target_angle=1.7;
-
-    float alpha; //angle error
-    float rho; //distance from target
-    float beta;
-    //float k_linear=10, k_angular=200;
-    //kb = -15  and ka = 30 tabom
-    float kRho=12, ka=30, kb=-13;
-    float linear, angular, angular_left, angular_right;
-    float dt=0.5;
-    float temp;
-    float d2;
-
-    //Diameter of a wheel and distance between the 2
-    float r=3.25, b=7.2;
-
-    int speed=999; // Max speed at beggining of movement
-
     //Resetting coordinates before moving
     theta=0; 
     X=0;
@@ -41,8 +43,23 @@
     
     beta = -alpha-theta+target_angle;
     //beta = atan(sin(beta)/cos(beta));
-    
-    do {
+
+    goToPointWithAngle(target_x, target_y, target_angle);
+
+    //Stop at the end
+    leftMotor(1,0);
+    rightMotor(1,0);
+
+    pc.printf("\n\r %f -- arrived!", rho);
+}
+
+//Distance computation function
+float dist(float robot_x, float robot_y, float target_x, float target_y){
+    return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
+}
+
+int goToPointWithAngle(float target_x, float target_y, int theta) {
+     do {
         pc.printf("\n\n\r entered while");
         
         //Timer stuff
@@ -57,7 +74,6 @@
         alpha = atan(sin(alpha)/cos(alpha));
         rho = dist(X, Y, target_x, target_y);
         d2 = rho;
-        //beta = -alpha-theta;
         beta = -alpha-theta+target_angle;
         //beta = atan(sin(beta)/cos(beta));
         
@@ -107,15 +123,6 @@
         //Timer stuff
         t.stop();
     } while(d2>1);
-
-    //Stop at the end
-    leftMotor(1,0);
-    rightMotor(1,0);
-
-    pc.printf("\n\r %f -- arrived!", rho);
-}
-
-//Distance computation function
-float dist(float robot_x, float robot_y, float target_x, float target_y){
-    return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
+    
+    return 0;
 }
\ No newline at end of file