Diff: main3.cpp

Revision:

4:22275847875d

Parent:

0:398afdd73d9e

--- a/main3.cpp	Mon Nov 23 23:24:35 2015 +0000
+++ b/main3.cpp	Thu May 03 12:08:03 2018 +0000
@@ -1,31 +1,166 @@
-
 #include "mbed.h"
 #include "m3pi.h"
 #include "MSCFileSystem.h"
 
 m3pi m3pi;                                  // Initialise the m3pi
 
-Serial xbee(p28,p27);
 DigitalOut resetxbee(p26);
-Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging
+DigitalOut led3(LED3);
+DigitalOut led1(LED1);
+DigitalIn push(p21);
+
+Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hangings
+Serial bt(p28,p27);
+
 MSCFileSystem fs("fs"); 
 
 BusOut myleds(LED1, LED2, LED3, LED4);
 
-int main() {
-    resetxbee =0;
-    wait(0.01);
-    resetxbee =1;
+const int THRESH = 200;
+
+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;
+char src='x';
+
+Timer timer;
+
+
+
+
+
+
+
+
+
+Serial& getCurrentSerial()
+{
+    return (src == 'u' ? pc : bt);
+}
+
+
+
+
+void set_motors(float mr, float ml)
+{
+    m3pi.left_motor(ml);
+    m3pi.right_motor(mr);
+}
+
+//------------------------------------------------------------------------------
+
+void getCapt(bool mem = false)
+{
+    int v = 0;
+    nbCapt = 0;
     
-    FILE *p= fopen("/fs/tt.txt","a+");
+    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 suiviIntersect(float intervalTour = 0.0f)
+{
+    bool done = false;
+    //forward = true;
+    unsigned int idx=0;
+    float vl, vr, omega, l_k=0, l_k1=0, sum=0, vmax=1.0f;
+    
+#if 1 // Low speed
+    const float Ki=0.007f, Kp=0.7f, Kd=1.5f, v=0.2;
+#else
+    const float Ki=0.0007f, Kp=0.07f, Kd=1.5f, v=0.80f;
+#endif
     m3pi.sensor_auto_calibrate();
-    wait(1.);
+    wait(1.0);
     
-    fprintf(p,"ecrire dans la cle USB\r\n");
-    fclose(p);
-    
-    while(1) {
+    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;
         
         
+        vl = v+omega < vmax ? (v+omega > -vmax ? v+omega : vmax) : vmax;
+        vr = v-omega < vmax ? (v-omega > -vmax ? v-omega : -vmax): vmax;
+        
+        vr *= 1.2;
+        vl *= 1.2;
+        set_motors(vr,vl);
+        
+        //---------------------INTERSECTION
+        if(sensors[0] || sensors[4]) {
+        char c = 'a';
+        //if ((c<='5') & (c>='1')) {
+        //    leds[c - '1'] = !leds[c - '1'];
+        //}
+        bt.printf("Voici un %c \n", c);
+        }
+        
+        /*
+        if(forward) set_motors(vr,vl);
+        else set_motors(-vl,-vr);
+        */
+        
+        m3pi.cls();
+        //const char* C [1]= {'0' + idx};
+        //m3pi.printf(C);
     }
+    
+    return;
 }
+
+
+
+
+
+
+
+
+
+
+int main() {
+    bt.baud(115200);
+    m3pi.cls();
+    wait(0.5);
+    
+    m3pi.sensor_auto_calibrate();
+    
+    set_motors(0.f,0.f);
+    char c = 'a';
+    bt.printf("Hello World!\n");
+
+    while(1)
+    {
+        getCapt();
+        suiviIntersect();
+    }
+}
\ No newline at end of file