Diff: main.cpp

Revision:

1:6d8c50dcae28

Parent:

0:879aa9d0247b

--- a/main.cpp	Tue Feb 12 17:39:05 2013 +0000
+++ b/main.cpp	Thu May 03 13:00:03 2018 +0000
@@ -1,10 +1,153 @@
+
+
+
+//-------------------------BT CONTROL
 #include "mbed.h"
- 
-Serial pc(USBTX, USBRX); // tx, rx
- 
+#include "m3pi.h"
+//#include "MSCFileSystem.h"
+
+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};
+
+m3pi m3pi;
+
+//MSCFileSystem fs("fs");
+
+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;
+
+
+void getCapt(bool mem = false)
+{
+    int v = 0;
+    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);
+}
+
+
+void set_motors(float mr, float ml)
+{
+    m3pi.left_motor(ml);
+    m3pi.right_motor(mr);
+}
+
+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;
+}
+
+
+
+
+
+
+
+
+
+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);
+}
+
+
+
+
 int main() {
-    pc.printf("Hello World!\n");
+    bt.baud(115200);
+    char c = 'a';
+    bt.printf("Hello World!\n");
     while(1) {
-        pc.putc(pc.getc() + 1);
+        m3pi.stop();
+        c =  bt.getc();
+        bt.printf("Je viens de recevoir : %c \n", c);
+        //move
+        assignDirection(c, 0.2);
+        getCapt();
+        suiviIntersect();
     }
-}
\ No newline at end of file
+}
+