Diff: main.cpp

Revision:

0:1e2ff5ae5204

Child:

1:f4dd7bc26785

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 16 10:32:00 2013 +0000
@@ -0,0 +1,261 @@
+#include "mbed.h"
+#include "m3pi_ng.h"
+
+
+// Minimum and maximum motor speeds
+#define MAX 0.1
+#define MIN 0
+
+// PID terms
+#define P_TERM 1
+#define I_TERM 0
+#define D_TERM 20
+
+
+
+
+
+// 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++) 
+        {
+        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()
+    {
+        char complete=pi.sensor_auto_calibrate();   //wozu denn?
+        pi.locate(0,0);
+        pi.printf("%c",complete);
+        wait(2);
+        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  
+    //        
+    int raw_sens_values[5];
+    pi.raw_sensor(raw_sens_values);
+    
+    // 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    
+    ///////////////////////////////////////
+    
+    pi.cls(); // ohne clear display zeigt er Werte über 2000 an
+    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;
+        {
+        //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;
+    }
+
+// 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 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;
+    }
+
+
+    
+    
+/////////////////////////////////////////////////////////////////////////////////// 
+// 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 
+{
+
+
+    // 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 derivative,proportional,integral = 0;
+    float power;
+    float speed = MAX;
+    
+
+        // 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);
+
+    } 
+
+/////////////////////////////////////////////////////////////////////////////////// 
+// 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        
+///////////////////////////////////////////////////////////////////////////////////
+
+
+
+main() 
+{
+    
+    float cross_speed = MAX*0.9;
+    
+    
+    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
+                        
+            /////////////////////////////////////////////////////////////////////////
+            // t-cross?
+            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();
+            }
+        }              
+    }
+}