File content as of revision 1:6d8c50dcae28:

//-------------------------BT CONTROL
#include "mbed.h"
#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() {
    bt.baud(115200);
    char c = 'a';
    bt.printf("Hello World!\n");
    while(1) {
        m3pi.stop();
        c =  bt.getc();
        bt.printf("Je viens de recevoir : %c \n", c);
        //move
        assignDirection(c, 0.2);
        getCapt();
        suiviIntersect();
    }
}