Diff: main.cpp

Revision:

1:f4dd7bc26785

Parent:

0:1e2ff5ae5204

--- a/main.cpp	Thu May 16 10:32:00 2013 +0000
+++ b/main.cpp	Fri May 17 11:14:34 2013 +0000
@@ -1,261 +1,305 @@
 #include "mbed.h"
 #include "m3pi_ng.h"
-
+#include "btbee.h"
 
 // Minimum and maximum motor speeds
-#define MAX 0.1
+#define MAX 0.2
 #define MIN 0
 
 // PID terms
 #define P_TERM 1
 #define I_TERM 0
-#define D_TERM 20
+#define D_TERM 2   // with one call printf 
 
-
-
+// Declaration 
+m3pi    pi;  //pi is of the classtyp m3pi
+btbee   bee; //bee is of the classtyp mtbee
 
 
-// Declaration pi is of the classtyp m3pi
-m3pi pi;
-
-//  shows the battery voltage in first line for a little? time , how long takes one for
-    void battery_voltage()
-    {
-       float voltage;
-       // 
-       for(int i=0;i<100;i++) 
-        {
+//  shows the battery voltage in first line for a little? time , 
+// how long takes one for
+void battery_voltage()
+{
+    float voltage;
+    //
+    for(int i=0; i<100; i++) {
         voltage=pi.battery();
         pi.locate(0,0);
         pi.printf("%f",voltage);
         pi.locate(0,1);
         pi.printf("%i",i);
-        }
-        pi.cls();
-     } 
- 
-// calibration with c for complete on screen
-    void calib()
+    }
+    pi.cls();
+}
+
+// function calibration //////
+void calib()
+{
+    char complete=pi.sensor_auto_calibrate();
+    // sensor_auto_calibrate, wie geht das intern?Atmel
+    if(complete=='c')
     {
-        char complete=pi.sensor_auto_calibrate();   //wozu denn?
-        pi.locate(0,0);
-        pi.printf("%c",complete);
-        wait(2);
-        pi.cls();
+    pi.locate(0,0);
+    pi.printf("calibrat");
+    pi.locate(0,1);
+    pi.printf("complete");
+    wait(2);
+    }else
+    {
+    pi.locate(0,0);
+    pi.printf("calibrat");
+    pi.locate(0,1);
+    pi.printf("failed");
+    wait(2);
     }
-    
-    
-    
-    
-    
-    
-// function gives back 1 if outer sensors on black surface //////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+    pi.cls();
+}
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////////
+// function gives back 1 if outer sensors on black surface ////////////////////////
+///////////////////////////////////////////////////////////////////////////////////
 //parameter "black_or_white" is a pointer, argument has to be a address
 
 int outer_sens()//int* outer_b_or_w)
-    {
+{
     int outer_b_or_w;
-    // read raw sensor values  
-    //        
+    // read raw sensor values
+    //
     int raw_sens_values[5];
     pi.raw_sensor(raw_sens_values);
-    
-    // give the raw sensorvalue in 1. line                                                       
-    /*    
+
+    // give the raw sensorvalue in 1. line
+    /*
     pi.locate(0,0);
-    ///////////////////////////////////////
-    wait(0.2);//value meassurement with delay (not good but good for representation of the values on scream    
-    ///////////////////////////////////////
-    
+    ///////////////////////////////////////////////////////////////////////////////////
+    wait(0.2);//value meassurement with delay 
+    (not good but good for representation of the values on scream
+    ///////////////////////////////////////////////////////////////////////////////////
+
     pi.cls(); // ohne clear display zeigt er Werte über 2000 an
-    pi.printf("%d",raw_sens_values[0]);    
+    pi.printf("%d",raw_sens_values[0]);
     // array entspricht pointer
     */
-    
+
     if (raw_sens_values[0]> 1000 & raw_sens_values[4]> 1000)// solange
-    //auf saal:white = ca. 400; black = ca. 2000;
-        {
+        //auf saal:white = ca. 400; black = ca. 2000;
+    {
         //pi.locate(0,1);
         //pi.printf("%i",1);
-         outer_b_or_w=1;          //1 is put to the address where the
-         } //black              // pointer is pointing at    
-        else 
-        {
-         //  pi.locate(0,1);
-         //  pi.printf("%i",0);
-          outer_b_or_w=0;  
-                               
-        } //white
-        
-      // return by ??? 
-      // return by value is slow far structures and arrays !!!!!!!!!!!!!!!!!!???????????????????
-      return outer_b_or_w;
-    }
+        outer_b_or_w=1;          //1 is put to the address where the
+    } //black              // pointer is pointing at
+    else {
+        //  pi.locate(0,1);
+        //  pi.printf("%i",0);
+        outer_b_or_w=0;
 
-// function gives back 1 if inner sensors on black surface //////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+    } //white
+
+    // return by ???
+    // return by value is slow far structures and arrays !!!!!!!!!!!!!!!!!!????????
+    return outer_b_or_w;
+}
+
+// function gives back 1 if inner sensors on black surface ////////////////////////
+///////////////////////////////////////////////////////////////////////////////////
 //parameter "black_or_white" is a pointer, argument has to be a address
 
 int inner_sens()
-    {
-    int inner_b_or_w;        
+{
+    int inner_b_or_w;
     int raw_sens_values[5];
     pi.raw_sensor(raw_sens_values);
-    
-    
-    if (raw_sens_values[2]> 1000)// solange
-    //white = ca. 400; black = ca. 2000;
-        {
-         inner_b_or_w=1;          //1 is put to the address where the
-         } //black              // pointer is pointing at    
-        else 
-        {
-          inner_b_or_w=0;  
-                               
-        } 
-      return inner_b_or_w;
-    }
 
 
-    
-    
-/////////////////////////////////////////////////////////////////////////////////// 
+    if (raw_sens_values[2]> 1000)// solange
+        //white = ca. 400; black = ca. 2000;
+    {
+        inner_b_or_w=1;          //1 is put to the address where the
+    } //black              // pointer is pointing at
+    else {
+        inner_b_or_w=0;
+
+    }
+    return inner_b_or_w;
+}
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////////
 // function line_follow ///////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////
 
-//void line_follow(const int* ON)// runs only if ON points to a integer 1
-  void line_follow()
-// argument passed by address, ON is a pointer that points to an address 
+void line_follow(float * integrator_ptr, float * prev_line_ptr)
+// argument passed by address, 
+// integrator_ptr is a pointer that points to an address
+// in main 
+
 {
 
 
     // Defines on top
-
-
-
-    
     pi.locate(0,1);
     pi.printf("Line PID");
-    
+
     //wait(2.0);
-    
+
 
     float right;
     float left;
     float current_pos_of_line = 0.0;
-    float previous_pos_of_line = 0.0;
+    //float previous_pos_of_line = 0.0;
     float derivative,proportional,integral = 0;
-    float power;
+    float diff_speed;
     float speed = MAX;
+
+    //integral = *integrator_ptr;
+    //previous_pos_of_line = *prev_line_ptr;
+
+
+    // Get the position of the line.
+    current_pos_of_line = pi.line_position();
+    proportional = current_pos_of_line;
+
+    // Compute the derivative
+    derivative = current_pos_of_line - *prev_line_ptr;//previous_pos_of_line;
+                        // pointer points to the address where the
+                        // value of the previous position x(t_{i-1}) is stored
+                        
+    // Compute the integral
+    integral += proportional;
+    *integrator_ptr = integral;
     
 
-        // Get the position of the line.
-        current_pos_of_line = pi.line_position();        
-        proportional = current_pos_of_line;
-        
-        // Compute the derivative
-        derivative = current_pos_of_line - previous_pos_of_line;
-        
-        // Compute the integral
-        integral += proportional;
-        
-        // Remember the last position.
-        previous_pos_of_line = current_pos_of_line;
-        
-        // Compute the power
-        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
-        
-        // Compute new speeds   
-        right = speed+power;
-        left  = speed-power;
-        
-        // limit checks
-        if (right < MIN)
-            right = MIN;
-        else if (right > MAX)
-            right = MAX;
-            
-        if (left < MIN)
-            left = MIN;
-        else if (left > MAX)
-            left = MAX;
-            
-       // set speed 
-        pi.left_motor(left);
-        pi.right_motor(right);
+    // Remember the last position.
+    //previous_pos_of_line = current_pos_of_line;
+    *prev_line_ptr = current_pos_of_line; // x(t_i) is put to the address 
+                                            // where 
+
+    // Compute the diff_speed
+    
+    diff_speed = (proportional * (P_TERM) ) + (*integrator_ptr*(I_TERM)) + (derivative*(D_TERM)) ;
+    // Compute new speeds
+    right = speed+diff_speed;
+    left  = speed-diff_speed;
 
-    } 
+    // limit checks ////////////////////////
+    if (right < MIN)
+        right = MIN;
+    else if (right > MAX)
+        right = MAX;
 
-/////////////////////////////////////////////////////////////////////////////////// 
-// function turn right ///////////////////////////////////////////////////////////
+    if (left < MIN)
+        left = MIN;
+    else if (left > MAX)
+        left = MAX;
+
+    // set speed
+    pi.left_motor(left);
+    pi.right_motor(right);
+
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// function turn right ////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////
 void turn_right()
-    {
+{
     float turn_speed = MAX;
-     pi.left_motor(-turn_speed);// look on the wheel, pos speed => wheel turns clockwise
-     pi.right_motor(turn_speed);
-     wait (0.9);
-    }  
-///////////////////////////////////////////////////////////////////////////////////        
-// main        
+    pi.left_motor(-turn_speed);// look on the wheel, pos speed => wheel turns clockwise
+    pi.right_motor(turn_speed);
+    wait (0.9);
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////////
+// main
 ///////////////////////////////////////////////////////////////////////////////////
 
 
-
-main() 
+main()
 {
+// declaration //////
+    
+    float cross_time;
+    float cross_speed; 
+    float integrator;
+    float prev_line_pos;
+    
+// initialation ////
     
-    float cross_speed = MAX*0.9;
+    //how long does it take to move over the cross?
+            // 9.5/2cm=4.25cm // half diameter of the m3pi
+            // 0 < speed < 1
+            // measure: speed1=0.1 s1=42cm t1=5.5s voltage1=4.4 V
+            // measure: speed1=0.2 s1=42cm t1=2.1s 
+            
+            // real speed1=42/5.5 cm/s=7.6 cm/s
+            // dt1=4.25/7.6s=0.6s
+            // real speed2=42/2.1 cm/s=20 cm/s
+            // dt2=4.24/20s=0.212s
     
-    
+    cross_time=0.22;
+     
+    integrator=0.0;
+    prev_line_pos=0.0;
+    cross_speed = MAX*0.6;//  MAX=0.2 60%
+
+
+    pi.cls();
     battery_voltage();
     calib();
-      
-    while(true)
-    {         
-        
-      if(outer_sens()==0 && inner_sens()==1)//outer white, inner black
-      {
-        pi.locate(0,0);
-        pi.printf("while %i",outer_sens());
-        pi.cls();
-        line_follow();
-       }
-       else if(outer_sens()==1 && inner_sens()==1 )// outer black, inner black 
-       {   
-        pi.cls();
-        pi.stop();   
-        pi.locate(0,0);
-        pi.printf("stop");
-        wait(0.2);         
-        // move just little further
-        pi.left_motor(cross_speed);
-        pi.right_motor(cross_speed);
-        wait(0.5); 
-            //die zeit die erbraucht für den halben durch messer
-            // 9.5/2cm=4.25cm
-            // 0 < speed < 1
-            // measure: speed=0.1 s=42cm t=5.5s voltage=4.4 V
-            // real speed=42/5.5 cm/s=7.6 cm/s
-            // dt=4.25/7.6=0.6
-                        
+
+
+
+    while(true) 
+    {
+
+        if(outer_sens()==0 && inner_sens()==1)//outer white, inner black
+        {    
+            //pi.locate(0,0);
+            //pi.printf("while %i",outer_sens());
+            //pi.cls();
+            line_follow(&integrator, &prev_line_pos);// zurücksetzen von integrator und pos x(t_i)=0
+        } 
+        else if(outer_sens()==1 && inner_sens()==1 ) // outer black, inner black
+        { 
+            pi.cls();
+            pi.stop();
+            pi.locate(0,0);
+            pi.printf("cross");
+            pi.locate(0,1);
+            pi.printf("stop");
+            wait(0.2);
+            // move just little further
+            pi.left_motor(cross_speed);
+            pi.right_motor(cross_speed);
+            wait(cross_time);
+            
+
             /////////////////////////////////////////////////////////////////////////
             // t-cross?
-            if(outer_sens()==0 && inner_sens()==0 )// outer white, inner white 
             //////////////////////////////////////////////////////////////////////////
+            if(outer_sens()==0 && inner_sens()==0 )// outer white, inner white
+                
             {
-            pi.cls();
-            pi.stop();   
-            pi.locate(0,0);
-            pi.printf("T end");
-            }       
-            else 
-            /////////////////////////////////////////////////////////////////////////
-            // cross-cross
-            if(outer_sens()==0 && inner_sens()==1 )// outer white, inner black 
-            {//turn_right();
-            }
-        }              
-    }
-}   
+                pi.stop();
+                pi.cls();
+                pi.locate(0,0);
+                pi.printf("T end");
+            } else
+                /////////////////////////////////////////////////////////////////////////
+                // cross-cross
+                if(outer_sens()==0 && inner_sens()==1 ) { // outer white, inner black
+                    //turn_right();
+                
+                }//second else
+         }//first else
+    }//while true
+ 
+}