Diff: main.cpp

Revision:

0:d1ca55d6ef68

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 14 09:35:14 2019 +0000
@@ -0,0 +1,161 @@
+#include "mbed.h"                                                                   // Mbed Library
+#include "m3pi_ng.h"                                                                // Nico's Library
+#include "btbee.h"                                                                  // Bluetooth Library
+                                                     
+m3pi Rob;                                                                           // create m3pi Object
+DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};                            // LEDs of 3PI
+DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};   // LEDs of M3PI
+
+DigitalIn m3pi_pb(p21,PullUp);                                                      // DigitalIn: User Button P21
+DigitalIn m3pi_fir(p12);                                                            // DigitalIn: Front IR-Senspor Pin: 12
+
+Timer timer;  
+
+//////////////////////              Variables and Parameters          ///////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////// 
+int sen_raw[5], sen_cal[5], sen_norm[5]; 
+int n_black = 0;
+int lap = 0;
+
+float e, u, vleft, vright;
+float dt=0.005;
+float t=0, t0=0, t_lap0=0, t_lap=0, t0_code=0, t_code=.1, t_code_max=0;
+/////////////////////////////              Functions          ///////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////
+
+/////// Function for Initilization the Robot Program ///////
+////////////////////////////////////////////////////////////  
+void Init()
+    {
+    Rob.locate(0,0); Rob.printf("batteries:");
+    Rob.locate(0,1); Rob.printf("%0.2f V",Rob.battery());    
+    wait(1);
+    Rob.cls();
+    Rob.sensor_auto_calibrate();
+            
+    for (int i = 0 ; i<8 ; i++)
+    {   m3pi_led[i] = 1; wait(0.05); m3pi_led[i] = 0; }
+
+    }    
+////////// Function for Ending the Robot Program  //////////
+////////////////////////////////////////////////////////////     
+void End(float Data, int rows, int columns)
+    {
+    Rob.locate(0,0); Rob.printf("Process");
+    Rob.locate(0,1); Rob.printf("done.");
+    Rob.stop();
+    }
+////////// Function for Motor Voltage Saturation  //////////
+////////////////////////////////////////////////////////////  
+void sat(float &v_sat)
+    {
+    if (v_sat > 1)
+        v_sat=1;
+    if (v_sat < -1)
+        v_sat=-1;
+    }
+////////// Function for Automatic Line Following ///////////
+////////////////////////////////////////////////////////////     
+void Linefollowing()    
+    {
+    const double Kp = 0.60;                                                             // [default: 0.8] 
+    float vforw = Kp * 0.5;     
+               
+    e       = Rob.line_position();
+    u       = Kp*e;  
+    vleft   = vforw + u;
+    vright  = vforw - u;
+    sat(vleft); sat(vright);                // actuator saturation 
+                                                                
+    Rob.left_motor(vleft);                  // push to 3pi --> AC-Motor
+    Rob.right_motor(vright);                // push to 3pi --> AC-Motor
+    }
+/////////// Function for Avoiding a FrontCrash /////////////
+////////////////////////////////////////////////////////////    
+void Anticrash()
+    {
+     while (!m3pi_fir)
+        {
+        Rob.stop();
+             
+        for (int i = 0 ; i<8 ; i++)
+        {   m3pi_led[i] = 1; wait(0.05); m3pi_led[i] = 0; }      
+                 
+        }
+             
+    }
+        
+/////////// Function for lap time & lap counter ////////////
+////////////////////////////////////////////////////////////     
+void Counter()
+    {
+        Rob.calibrated_sensor(sen_cal);
+        
+        for (int i = 0 ; i<5 ; i++)
+            {sen_norm[i] = sen_cal[i]/1000;}
+        
+        if ( (sen_norm[0]>=.8) && (sen_norm[2]>=.8) && (sen_norm[4]>=.8)  )
+        {    
+            n_black++;
+            t = timer.read();
+
+            if ( (n_black == 5)&&((t-t_lap0)>2) )
+            {
+                lap++;
+                t_lap = t-t_lap0;
+                t_lap0 = t;
+            
+                Rob.cls();
+                Rob.locate(0,0);
+                Rob.printf("lap: %i",lap);
+                Rob.locate(0,1);   
+                Rob.printf("t: %4.2f",t_lap);
+            
+            }      
+            
+        }
+        else 
+        {    
+        n_black=0;      
+        
+         }    
+    }
+    
+///////////////////////////////              MAIN               /////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////
+int main() 
+{
+//////////////           Initialization      ///////////////
+//////////////////////////////////////////////////////////// 
+Init();
+////////////////////////////////////////////////////////////
+
+float tcmp = 1; 
+
+timer.start();
+    while (true)
+        {
+//        Anticrash();    
+        t       = timer.read();
+        
+        if ( (t-t0) >= dt)
+            {
+            t0      = t;          
+            t0_code = t;
+            
+             
+            Linefollowing();
+            Counter();  
+                                   
+            t_code  = (timer.read()-t0_code)*1000;     
+            tcmp    = t_code/t_code_max;
+            
+            if ( tcmp> 1.5)
+                {t_code_max = t_code;                           
+            Rob.cls(); Rob.locate(0,1); Rob.printf("%3.2f ms",t_code_max);
+                }
+            }
+        } 
+        
+/////////////////////////////////////////////////////////////////////////////////////////////////
+}