File content as of revision 7:ca53c1bde37c:

#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 t;
Ticker tick1;

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

#define D_TERM  0.0
#define I_TERM  0.1
#define I_TERMO 0.1
#define P_TERM  0.9
#define MAX     0.3
#define MIN    -0.2

#define seuil(x)  (x>400 ? 1 : 0)

float current_pos_of_line,
      previous_pos_of_line,
      derivate,
      proportional,
      power,
      integral,
      left,
      right,
      speed=0.3;

char chain[10];
int v;
      
unsigned short tabsensor[5];      
volatile unsigned char sensors;

volatile char flag10ms;

char command=1;

void inter1() {
     flag10ms=1;
}

void current_state() {
    unsigned char i;
    sensors=0;
    m3pi.calibrated_sensors(tabsensor);
    for(i=0; i<5; i++) {
        sensors = (sensors << 1) | seuil(tabsensor[i]);
    }
}

void step() {
    m3pi.forward(0.12);
    wait(0.18);
    // m3pi.stop();    
}

/*
 * Results
 * 1 -> PID
 * 2 -> Turn back
 * 3 -> Turn left
 * 4 -> Turn right
 */
char PIDf(char commande) {
    if(commande==1) {
        char result;
        current_state();
        switch(sensors) {
            case 0x00:
                // Deadend
                // Back
                m3pi.cls();
                m3pi.print("Back", 5);
                m3pi.stop();
                result = 2;
                break;
            case 0x18: case 0x1C: case 0x1E:
                // Forward/Left or Left Only
                step();                
                current_state();
                if ((sensors == 0x04) || (sensors == 0x06) || (sensors == 0x0C) || (sensors == 0x14) || (sensors == 0x16) || (sensors == 0x07)) {
                    // Forward
                    m3pi.cls();
                    m3pi.print("Forw2", 5);
                    result = 1;
                } else if (sensors == 0x1F) {
                    // End
                    m3pi.cls();
                    m3pi.print("The End2", 8);
                    result = 0;
                } else if((sensors == 0x00) || (sensors == 0x10) || (sensors == 0x18)) {
                    // Turn Left
                    m3pi.cls();
                    m3pi.print("Left", 5);
                    result = 3;
                } else {
                    // Turn Right
                    m3pi.cls();
                    m3pi.print("Right2", 6);
                    result = 4;
                }
                break;
            case 0x07: case 0x03:
                // Forward/Right or Right Only -> RF
                // m3pi.stop();
                m3pi.cls();
                m3pi.print("Right", 6);
                step();
                result = 4;
                break;
            case 0x1F: case 0x0F:
                // 'T' or Intersection or End -> LR or RFL
                // m3pi.stop();
                step();
                current_state();
                if (sensors == 0x1F) {
                    // End
                    m3pi.cls();
                    m3pi.print("The End", 8);
                    result = 0;
                } else {
                    // Turn Right
                    m3pi.cls();
                    m3pi.print("Inter", 6);
                    result = 4;
                }
                break;
            case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08:
                //PID
                // Get the position of the line
                current_pos_of_line = m3pi.line_position();
                proportional = current_pos_of_line;
                // Compute the derivate
                derivate = current_pos_of_line - previous_pos_of_line;
                // Compute the integral
                integral = (integral+I_TERMO*proportional)/(1+I_TERMO);
                // Remember the last postion
                previous_pos_of_line = current_pos_of_line;
                // Compute the power
                power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivate*(D_TERM));
                // Compute new speeds
                right = speed-(power*MAX);
                left = speed+(power*MAX);
                // Limits checks on motor control
                right = (right>MAX ? MAX :(right<MIN ? MIN : right));
                left = (left>MAX ? MAX :(left<MIN ? MIN : left));
                // Send command to motors
                m3pi.left_motor(left);
                m3pi.right_motor(right);
                result = 1;
                break;
            default:
                // Faire rien
                m3pi.stop();
                result = 0;
                break;
        }
        return result; 
    }
}


/*
 * Results
 * 1 -> PID
 * 2 -> Turn back
 * 3 -> Turn left
 * 4 -> Turn right
 */
char turn(char command) {
    if(command > 1 && command < 5) {
        char result;
        current_state();
        switch(command) {
            case 2:
                // Turn Back
                if(sensors != 0x01) {
                    m3pi.right(speed);
                    result = 2;
                } else {
                    m3pi.right(0.12);
                    wait(0.12);
                    m3pi.stop();
                    result = 1;
                }
                break;
            case 3:
                // Turn Left
                if(sensors != 0x10) {
                    m3pi.left(speed);
                    result = 3;
                } else {
                    m3pi.left(0.12);
                    wait(0.1);
                    m3pi.stop();
                    result = 1;
                }
                break;
            case 4:
                // Turn Right
                if(sensors != 0x01) {
                    m3pi.right(speed);
                    result = 4;
                } else {
                    m3pi.right(0.12);
                    wait(0.1);
                    m3pi.stop();
                    result = 1;
                }
                break;  
        }
        return result;
    }
}

    
int main() {
    resetxbee=0;
    wait(0.01);
    resetxbee =1;
    
    // FILE *p= fopen("/fs/tt.txt","a+");
    m3pi.sensor_auto_calibrate();
    wait(1.);
    tick1.attach(&inter1,0.01);
    
    // fprintf(p,"ecrire dans la cle USB\r\n");
    // fclose(p);

    while(1) {
        if(flag10ms==1) {
            switch(command) {
                case 0:
                    // Faire Rien
                    m3pi.stop();
                    break;
                case 1:
                    // PID
                    command = PIDf(command);
                    break;
                case 2: case 3: case 4:
                    // 2 -> Back
                    // 3 -> Left
                    // 4 -> Right
                    command = turn(command);
                    break;
            } 
        }
    }
}