Revision 9:f14532fd7a02, committed 2016-12-15

Comitter:

kimnielsen

Date:

Thu Dec 15 13:50:26 2016 +0000

Parent:

8:5ca76759a67e

Commit message:

Endelig kildekode med rettelser.

Changed in this revision

--- 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

--- a/robot.cpp	Mon Dec 12 13:35:34 2016 +0000
+++ b/robot.cpp	Thu Dec 15 13:50:26 2016 +0000
@@ -18,32 +18,40 @@
 
 int main()
 { 
-/////////////////////////////////////////////////////////////////////////////
-//                          Driving setup                                  // 
-/////////////////////////////////////////////////////////////////////////////
-   
-   while(1)
+  startfunction();
+  distance = 2000;
+  get_to_goal();;
+  robot.stop();
+  distance = 4500;
+  wait_ms(1000);
+  
+   init();
+   while(tickR <= 16)
    {
+    robot.FW(0.5, 0);
+    wait_ms(10);
+    }
+    robot.stop();
+  
+  while(1)   
+  {
+   init();
    startfunction();
    get_to_goal();
-   robot.stop();
-   printf("\n\r DistanceBysensor: %.2f \n", Dcenter());  
-   wait_ms(2000);
+   robot.stop(); 
+   wait_ms(1000);
    
-   init(); 
-   startfunction();
-   turn();
-   robot.stop();
-   }
+   init();
+   while(tickR <= 16)
+   {
+    robot.FW(0.5, 0);
+    wait_ms(10);
+    }
+    robot.stop();
 } 
-/*  
-----------------------------------------------------------------------------
-                            END OF MAIN FUNCTION
-----------------------------------------------------------------------------*/  
+}     
 void startfunction()
 {
-    T1.attach(&read_analog, 1.0); // attach the address of the read_analog
-    //function to read analog in every second
     tacho_left.rise(&tickLeft);  // attach the address of the count function
     //to the falling edge
     tacho_right.rise(&tickRight);  // attach the address of the count function