Diff: odometry.h

Revision:

9:f14532fd7a02

Parent:

8:5ca76759a67e

--- a/odometry.h	Mon Dec 12 13:35:34 2016 +0000
+++ b/odometry.h	Thu Dec 15 13:50:26 2016 +0000
@@ -4,38 +4,37 @@
 #include "hack_motor.h"
 #include "math.h"   
 
-//  ODOMETRY VALUES
+/////////////////////////////////////////////////////////////////////////////
+//  ODOMETRY VALUES                                                        //
+/////////////////////////////////////////////////////////////////////////////
 #define N 20 // ticks on wheel
 #define R 33.5 // radius = 33.5 mm on wheels
 #define L 133 // 133 mm distance between wheels
 #define PI 3.141592
-#define DISTANCE 10000
-#define TURN 282  
 int tickL = 0; // tick on left wheel
 int tickR = 0; // tick on right wheel
 
-//  PID VALUES
-#define P_TERM 1.0
+/////////////////////////////////////////////////////////////////////////////
+//  PID VALUES                                                             //
+/////////////////////////////////////////////////////////////////////////////
+#define P_TERM 0.13    
 #define I_TERM 0
 #define D_TERM 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
+/////////////////////////////////////////////////////////////////////////////
+//  GLOBAL DEFINITIONS                                                     //
+/////////////////////////////////////////////////////////////////////////////
 double right,left;  
 double result;
 double speed=0.5;
-double speed_turn=1;   
-DigitalOut LED(LED1);
-
-//  ANALOG POWER
+double distance = 4500;
+/////////////////////////////////////////////////////////////////////////////
+//  ANALOG POWER                                                           //
+/////////////////////////////////////////////////////////////////////////////
 AnalogIn   ain(PC_4);
 DigitalOut dout(PB_13);
 DigitalOut greenout(PB_12);
-int stop=0;         //DigitalOut DEFINITION OF RED LED!!
+int stop=0;                 //DigitalOut DEFINITION OF RED LED!!
 DigitalOut LedFlash(PA_3);  //  RED LED WILL FLASH IF ROBOT STOPS 
 
 /////////////////////////////////////////////////////////////////////////////
@@ -44,9 +43,9 @@
 Timer t; // DEFINE A TIMER T
 Serial pc(USBTX, USBRX); // not used here because we only have one serial 
 // connection
-InterruptIn tacho_left(PC_3); // Set PC_2 to be interupt in for tacho left - Ledningsfarve: Hvid
-InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right - Ledningsfarve: Grå    
-Wheel robot(PB_2, PB_1, PB_15, PB_14);  //PB_15: grøn PB_14: blå PB_2: orange PB_1: gul
+InterruptIn tacho_left(PC_3); // Set PC_2 to be interupt in for tacho left
+InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right
+Wheel robot(PB_2, PB_1, PB_15, PB_14); 
 // create an object of robot H-bridge. ---(M1A, M1B, M2A, M2B)---
 
     //  GETTING INFO FROM SENSORS 
@@ -59,7 +58,6 @@
     } else {
         dout = not dout;
         stop=1; // flash red led
-        LED = 1;
     }
     if (ain==1.0f) greenout = 1; // full power
     else greenout = 0;
@@ -95,7 +93,7 @@
 {
     return (Dleft()+Dright())/2;
 }
-
+    
 /////////////////////////////////////////////////////////////////////
 //                      PID REGULATOR                              //
 /////////////////////////////////////////////////////////////////////
@@ -108,18 +106,12 @@
     double integral = 0;
     double e_old = 0;
 
-    while (Dcenter() <= DISTANCE) 
-    {    
-        e = tickR - tickL;  //error calculation
-        if(e < 0)
-        {
-            tickR = tickL;   
-        }
-        else if(e > 0)
-        {
-         tickR = tickL;   
-        }
-    
+    while (Dcenter() <= distance) 
+    {
+        right = speed;     
+        left = speed;
+        e = right;      
+        
         //PID calculation
         proportional = e; // GETTING THE ERROR VALUE
         integral += proportional;   // THE ERROR NEVER GETS TOO LOW
@@ -131,79 +123,15 @@
         (derivative * (D_TERM));
         
         // COMPUTING NEW SPEEDS ON WHEELS
-        right = speed + output;//if power is 0, no error->same speed on wheels
-        right = right - WheelFactor*right;
-        left = speed + output;
+        right = speed - output;
+        left = speed + output*0.15; 
         
-        //CHECK YOUR LIMITS
-        if(right < MIN)
-        {
-            right = MIN- WheelFactor*MIN;
-        }
-        else if(right > MAX)
-        {
-            right = MAX- WheelFactor*MAX;
-        }
-        if(left < MIN)
-        {
-            left = MIN;
-        }
-        else if(left > MAX)
-        {
-            left = MAX;
-        }
-        printf("\n\r RightSpeed: %.2lf  RightTicks: %d  LeftSpeed: %.2lf"  
-        "LeftTicks: %d  Output: %lf", right, tickR, left, tickL, output);    
-    
-        // set new positions
-        robot.FW(right,left); 
+        robot.FW(right,left);
+        printf("tickR: %d   rightSpeed: %.2lf   tickL: %d   leftSpeed: %.2lf",
+        tickR, right, tickL, left);
         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