Diff: polulu.cpp

Revision:

0:b839dbffc636

Child:

1:63050abd40e9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/polulu.cpp	Thu May 03 14:03:58 2018 +0000
@@ -0,0 +1,168 @@
+#include "mbed.h"
+#include "m3pi.h"
+
+m3pi m3pi; // Initialise the m3pi
+Serial pc(USBTX, USBRX); //tx, rx
+Serial bt(p28,p27); //bluetooth
+
+
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+DigitalOut leds [4] = {LED1, LED2, LED3, LED4};
+// variables 
+
+BusOut myleds(LED1, LED2, LED3, LED4);
+
+const int THRESH = 300;
+
+unsigned short sensors[5], mems[5], nbCapt, memsNb;
+char tab[4];
+float position, proportional, last_proportional, integral;
+
+unsigned char dir;
+float speed, duration, s1, s2;
+
+
+//------------------ paramétrage moteurs --------------------------//
+
+
+void set_motors(float mr, float ml)
+{
+    m3pi.left_motor(ml);
+    m3pi.right_motor(mr);
+}
+
+//---------------------- arret moteur -----------------------------//
+
+void stop_motors()
+{
+    m3pi.stop();
+} 
+
+
+//--------------- utilisation capteur -------------------------------//
+
+void getCapt(bool mem = false)
+{
+    int v = 0;
+    int nbCapt = 0;
+    
+    m3pi.calibrated_sensors(sensors);
+    
+    for(int i=0 ; i<5 ;i++)
+    {
+        sensors[i] = (sensors[i] > THRESH ? true : false);
+        if(sensors[i])
+        {
+            v += 0x1<<i;
+            nbCapt++;
+        }
+        if(mem)
+        {
+            mems[i] = sensors[i];
+            memsNb = nbCapt;
+        }
+    }
+    m3pi.leds(v);
+}
+
+
+//-------------------- suivi ligne00000000 -------------------------//
+
+
+void suiviIntersect(float intervalTour = 0.0f)
+{
+    bt.printf("startSuivi_");
+    bool done = false;
+    unsigned int idx=0;
+    float vl, vr, omega, l_k=0, l_k1=0, sum=0, vmax=0.50f;
+    
+#if 1 // Low speed
+    const float Ki=0.005f, Kp=0.5f, Kd=1.5f, v=0.2;
+#else
+    const float Ki=0.0005f, Kp=0.05f, Kd=1.5f, v=0.80f;
+#endif
+    m3pi.sensor_auto_calibrate();
+    wait(1.0);
+    bt.printf("---> While in<--- ");
+    Timer t; t.start();
+    while(!done)
+    {
+        idx++;
+        getCapt();
+        // PID
+        l_k1 = l_k;
+        l_k = m3pi.line_position();
+        //v = sp;
+        omega = l_k*Kp; // Proportionnal
+        sum += l_k;
+        omega += sum*Ki; // Integral
+        omega += (l_k-l_k1)*Kd;
+        //omega *= v;
+        
+        
+        vl = v+omega < vmax ? (v+omega > -vmax ? v+omega : vmax) : vmax;
+        vr = v-omega < vmax ? (v-omega > -vmax ? v-omega : -vmax): vmax;
+        
+        
+        //------------------------------------- Pour les zigzags
+        //getCurrentSerial().printf("? hola que pasa ?");
+        
+        //-------------------------------------
+        
+        //-----------------Intersection
+        if(sensors[0] || sensors[4])
+        {
+            done = true;
+        }
+        set_motors(vr,vl);
+    }
+    bt.printf("-->While out<--");
+    
+    return;
+}
+
+
+//----------------- pilotage -> direction ---------------------//
+
+
+void assignDirection(char c, float s)
+{
+    if (c == 'a') m3pi.left(s);
+    if (c == 'c') m3pi.right(s);
+    if (c == 'd') {
+        m3pi.right(s);
+        wait(0.355);
+    }
+    wait(0.355);
+}
+
+
+//-------- paramétrage fréquence (pour bluetooth ) --------//
+
+void deffreq(int x)
+{
+    bt.baud(x);
+}
+
+    
+//---------- renvoi de message à l'utilisateur ------------//
+
+
+void sendmsg(char* msg)
+{
+    bt.printf("%s\n", msg);
+}
+
+
+//-------- recuperation de l info sur la direction ---------//
+
+
+void getdir(char c)
+{
+    c = bt.getc();
+}
+