Diff: odometry.h

Revision:

5:cf033e9d4468

Parent:

4:62a6681510d6

Child:

7:a852e63cac3d

--- a/odometry.h	Wed Nov 23 21:40:26 2016 +0000
+++ b/odometry.h	Thu Nov 24 09:14:48 2016 +0000
@@ -101,7 +101,7 @@
     [11] = sqrt(pow((xPosition[12] - xPosition[11]), 2) + pow((yPosition[12] - yPosition[11]), 2)),
     [12] = sqrt(pow((xPosition[13] - xPosition[12]), 2) + pow((yPosition[13] - yPosition[12]), 2)),
     [13] = sqrt(pow((xPosition[14] - xPosition[13]), 2) + pow((yPosition[14] - yPosition[13]), 2))
-};  //REMEMBER TO INCLUDE DISTANCE FOR TRAVELLING BACK AGAIN
+};  //REMEMBER TO INCLUDE DISTANCE FOR TRAVELLING BACK AGAIN!
 
 /*
 =============================================================================
@@ -187,10 +187,11 @@
 }
 
     //  PID REGULATOR
-void get_to_goal ( ) // FUNCTION TO REACH GOAL WITH ERROR CONSIDERATION
+void get_to_goal ( ) // FUNCTION TO REACH GOAL WITH ERROR CALCULATION   
 {   
     double e = 0; // angle error
-    double THETA_D1 = atan((yPosition[1]-yPosition[0])/(xPosition[1]-xPosition[0]));    //  THETA DESIRED CALCULATED
+    double THETA_D1 = atan(DeltaY_1() / DeltaX_1());
+    
     double output = 0;
     double derivative = 0;
     double proportional = 0;
@@ -248,7 +249,7 @@
                
         printf("\n\r Error: %.2lf", e); //  HUSK DETTE!
 
-        wait_ms(20); // should be adjusted to your context. Give the motor time
+        wait_ms(5000); // should be adjusted to your context. Give the motor time
         // to do something before you react
     }
     t.stop(); // stop timer