File content as of revision 4:dcad3508bdfb:

#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
//MSCFileSystem fs("fs"); 
Timer tm1;
Ticker tic1;

BusOut myleds(LED1, LED2, LED3, LED4);

// all variables are float
#define D_TERM 0.0
#define I_TERMO 0.1
#define I_TERM  0.1
#define P_TERM  0.9
#define MAX 0.3
#define MIN -0.2
float current_pos_of_line,derivative,proportional,power,integral,right,left,previous_pos_of_line;
float speed =0.3;


unsigned short tabsensor[5];
#define seuil(x) (x>400 ? 1 : 0)
unsigned char statcapt;
char PIDf(char commande){
    unsigned char i;
    m3pi.calibrated_sensors(tabsensor);
    statcapt=0;
    for(i=0;i<5;i++){
            statcapt = (statcapt <<1)  | seuil(tabsensor[i]);
    }
    if(commande == 1) {
            if((statcapt==0 )||(statcapt==0x1F )) {
                        m3pi.left_motor(0.0);
                        m3pi.right_motor(0.0); 
                        myleds=0xF;
                        return 0;   
            }else{
                    // Get the position of the line.
                        current_pos_of_line = m3pi.line_position();        
                        proportional = current_pos_of_line;    
                    // Compute the derivative
                        derivative = current_pos_of_line - previous_pos_of_line;
                    // Compute the integral
                        integral = (integral+ I_TERMO*proportional)/(1+I_TERMO);
                    // Remember the last position.
                        previous_pos_of_line = current_pos_of_line;
                    // Compute the power
                        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
                    // Compute new speeds
                        right = speed-(power*MAX);
                        left  = speed+(power*MAX);
                    // limit checks on motor control
                         //MIN <right < MAX
                        // MIN <left < MAX
                        right = (right>MAX ? MAX :(right<MIN ? MIN : right));
                    // send command to motors
                        m3pi.left_motor(left);
                        m3pi.right_motor(right);
                }
        }
        return 1;
}

volatile char flag10ms;
void inter1(){
        flag10ms=1;
}

int v;
unsigned char delai600ms;
char chain[10];
int main() {
    resetxbee =0;
    wait(0.01);
    resetxbee =1;
    
 //   FILE *p= fopen("/fs/tt.txt","a+");
    m3pi.sensor_auto_calibrate();
    wait(1.);
        tic1.attach(&inter1,0.01);
  //  fprintf(p,"ecrire dans la cle USB\r\n");
  //  fclose(p);
    
    while(1) {
            if(flag10ms==1){
                    flag10ms=0;
                    tm1.reset();
                    tm1.start();
                    PIDf(1);
                    tm1.stop();
                    v=tm1.read_us();
                    delai600ms++;
              }
              if(delai600ms>=60){
                    sprintf(chain,"%5u",v);     
                    m3pi.locate(0,0);
                    m3pi.print(chain,strlen(chain));
                    delai600ms=0;
               }
            //pc.printf("%u %u %u %u %u\r\n",seuil(tabsensor[0]),seuil(tabsensor[1]),
            //                                    seuil(tabsensor[2]),seuil(tabsensor[3]),seuil(tabsensor[4]));
    }
    
}