Diff: odometry.h

Revision:

8:5ca76759a67e

Parent:

7:a852e63cac3d

--- a/odometry.h	Fri Dec 09 12:32:21 2016 +0000
+++ b/odometry.h	Mon Dec 12 13:35:34 2016 +0000
@@ -6,10 +6,11 @@
 
 //  ODOMETRY VALUES
 #define N 20 // ticks on wheel
-#define R 32.5 // radius = 32.5 mm on wheels
+#define R 33.5 // radius = 33.5 mm on wheels
 #define L 133 // 133 mm distance between wheels
 #define PI 3.141592
-#define DISTANCE 3000
+#define DISTANCE 10000
+#define TURN 282  
 int tickL = 0; // tick on left wheel
 int tickR = 0; // tick on right wheel
 
@@ -17,13 +18,17 @@
 #define P_TERM 1.0
 #define I_TERM 0
 #define D_TERM 0
-#define MIN 0
+#define MIN 0.9
 #define MAX 1.0
+#define WheelFactor 0.10
+#define T_MIN 0.00
+#define T_MAX 1.00
 
 //  GLOBAL DEFINITIONS
 double right,left;  
 double result;
-double speed=0.5;  
+double speed=0.5;
+double speed_turn=1;   
 DigitalOut LED(LED1);
 
 //  ANALOG POWER
@@ -60,7 +65,7 @@
     else greenout = 0;
 }
 //  INIT YOUR PARAMETERS
-void init()
+void init()                          
 {
     tickL=0;
     tickR=0;
@@ -78,22 +83,18 @@
     
 float Dleft() // DISTANCE FOR LEFT WHEEL
 {
-    return 2*PI*R*tickL/N;
+    return (2*PI*R*tickL)/N;
 }
 
 float Dright() // DISTANCE FOR RIGHT WHEEL
 {
-    return 2*PI*R*tickR/N;
+    return (2*PI*R*tickR)/N;
 }
 
 float Dcenter() // DISTANCE FOR CENTER
 {
     return (Dleft()+Dright())/2;
 }
-void turn( )
-{
-    
-}
 
 /////////////////////////////////////////////////////////////////////
 //                      PID REGULATOR                              //
@@ -112,11 +113,11 @@
         e = tickR - tickL;  //error calculation
         if(e < 0)
         {
-            e = 0;   
+            tickR = tickL;   
         }
         else if(e > 0)
         {
-         e = 0;   
+         tickR = tickL;   
         }
     
         //PID calculation
@@ -130,17 +131,18 @@
         (derivative * (D_TERM));
         
         // COMPUTING NEW SPEEDS ON WHEELS
-        right = speed - output;//if power is 0, no error->same speed on wheels
+        right = speed + output;//if power is 0, no error->same speed on wheels
+        right = right - WheelFactor*right;
         left = speed + output;
         
         //CHECK YOUR LIMITS
         if(right < MIN)
         {
-            right = MIN;
+            right = MIN- WheelFactor*MIN;
         }
         else if(right > MAX)
         {
-            right = MAX;
+            right = MAX- WheelFactor*MAX;
         }
         if(left < MIN)
         {
@@ -151,13 +153,57 @@
             left = MAX;
         }
         printf("\n\r RightSpeed: %.2lf  RightTicks: %d  LeftSpeed: %.2lf"  
-        "LeftTicks: %d", right, tickR, left, tickL);    
+        "LeftTicks: %d  Output: %lf", right, tickR, left, tickL, output);    
     
         // set new positions
         robot.FW(right,left); 
-        wait_ms(25); // should be adjusted to your context. Give the motor time
+        wait_ms(10); // should be adjusted to your context. Give the motor time
         // to do something before you react
     }
     t.stop(); // stop timer
+} 
+
+void turn()
+{
+    double e = 0; 
+    double output = 0;
+    double derivative = 0;
+    double proportional = 0;
+    double integral = 0;
+    double e_old = 0;
+    
+    while(Dright() <= TURN) 
+   {
+       e = tickR - tickL;
+        if(e < 0)
+        {
+            tickR = tickL;   
+        }
+        else if(e > 0)
+        {
+         tickR = tickL;   
+        }
+        proportional = e; 
+        integral += proportional;
+        derivative = e - e_old;
+        e_old = e;
+        
+        output = (proportional * (P_TERM)) + (integral * (I_TERM))+ 
+        (derivative * (D_TERM));
+        right = speed + output;
+        left = 0;
+       
+       if(right <= T_MIN)
+       {
+        right = T_MAX;
+        }
+        
+        else if(right >= T_MAX)
+        {
+            right = T_MAX;
+        }
+       robot.FW(right, left); 
+       wait_ms(10);
+   }   
 }
-#endif
\ No newline at end of file
+#endif 
\ No newline at end of file