File content as of revision 4:22275847875d:

#include "mbed.h"
#include "m3pi.h"
#include "MSCFileSystem.h"

m3pi m3pi;                                  // Initialise the m3pi

DigitalOut resetxbee(p26);
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);

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;
    
    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.0);
    
    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();
    }
}