File content as of revision 9:5701616e3a2c:

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

m3pi m3pi;                                  // Initialise the m3pi
char *replaceWord(const char *s, const char *oldW, const char *newW);
Serial xbee(p28,p27);
DigitalOut resetxbee(p26);
Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging

Timer t;
Ticker tick1;
char affichage[3]={0};
char maze[100]={0};
char *optm;
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>300 ? 1 : 0)

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

char chain[10];

int j = 0;


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

void step2() {
    m3pi.forward(0.12);
    wait(0.06);
}

/*
 * 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();
                strcat(maze,"B");
                m3pi.locate(0,1);
                m3pi.printf(maze);
                m3pi.stop();
               
                result = 2;
                break;
            case 0x1C: case 0x18: case 0x10:
                // Forward/Left or Left Only
                step();             
                current_state();
                if ((sensors == 0x00) || (sensors == 0x10)) {
                    // Turn Left
                    m3pi.cls();
                    strcat(maze,"L");
                    m3pi.locate(0,1);
                    m3pi.printf(maze);
                   
                    result = 3;
                } else {
                    // Forward
                    m3pi.cls();
                    strcat(maze,"F");
                    m3pi.locate(0,1);
                    m3pi.printf(maze);
                    step();
                   
                    result = 1;
                }
                break;
            case 0x07: case 0x03: case 0x01:
                // Forward/Right or Right Only
                m3pi.cls();
                strcat(maze,"R");
                m3pi.locate(0,1);
                m3pi.printf(maze);
               
                step();

                result = 4;
                break;
            case 0x1F:
                // 'T' or Intersection or End
                step();
                current_state();
                if (sensors == 0x1F) {
                    // End
                    m3pi.cls();
                    strcat(maze,"E");
                    m3pi.locate(0,1);
                    m3pi.printf(maze);
                   
                    m3pi.stop();

                    result = 5;
                } else {
                    // 'T' -> Turn Right
                    m3pi.cls();
                    strcat(maze, "R");
                    m3pi.locate(0,1);
                    m3pi.printf(maze);
                   
                    result = 4;
                }
                break;
            case 0x0F: case 0x1E:
                // 'T' or Intersection or End -> LR or RFL
                step2();
                current_state();
                if (sensors == 0x1F) {
                    // End
                    m3pi.cls();
                    strcat(maze,"E");
                    m3pi.locate(0,1);
                    m3pi.printf(maze);
                   
                    m3pi.stop();
                   
                    result = 5;
                } else if ((sensors == 0x10) || (sensors == 0x18)) {
                    // Turn Left
                    m3pi.cls();
                    strcat(maze,"L");
                    m3pi.locate(0,1);
                    m3pi.printf(maze);

                    step2();
                    step2();

                    result = 3;
                } else if ((sensors == 0x14) || (sensors == 0x16) || (sensors == 0x06) || (sensors == 0x1C)) {
                    // Forward
                    m3pi.cls();
                    strcat(maze,"F");
                    m3pi.locate(0,1);
                    m3pi.printf(maze);
                   
                    step2();
                    step2();
                   
                    result = 1;
                } else {
                    // Turn Right
                    m3pi.cls();
                    strcat(maze, "R");
                    m3pi.locate(0,1);
                    m3pi.printf(maze);
                   
                    step2();
                    step2();
                   
                    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 {
                    do{
                    current_state();
                    m3pi.right(0.4*speed);
                    }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
                    m3pi.stop();
                   
                    result = 1;
                }
                break;
            case 3:
                // Turn Left
                if(sensors != 0x10) {
                    m3pi.left(speed);
                   
                    result = 3;
                } else {
                    do{
                    current_state();
                    m3pi.left(0.4*speed);
                    }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
                    m3pi.stop();
                   
                    result = 1;
                }
                break;
            case 4:
                // Turn Right
                if(sensors != 0x01) {
                    m3pi.right(speed);
                   
                    result = 4;
                } else {
                    do{
                    current_state();
                    m3pi.right(0.4*speed);
                    }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
                    m3pi.stop();
                   
                    result = 1;
                }
                break;
        }
        return result;
    }
}

char find(char str[], char c) {
  unsigned char len = strlen(str);
  int i;

  for(i = 0; i < len; i++) {
    if(str[i] == c)
      return 1;
  }

  return 0;
}


char *replaceWord(const char *s, const char *oldW, const char *newW) {
    char *result;
    int i, cnt = 0;
    int newWlen = strlen(newW);
    int oldWlen = strlen(oldW);
 
    // Counting the number of times old word
    // occur in the string
    for (i = 0; s[i] != '\0'; i++) {
        if (strstr(&s[i], oldW) == &s[i]) {
            cnt++;
 
            // Jumping to index after the old word.
            i += oldWlen - 1;
        }
    }
 
    // Making new string of enough length
    result = (char *)malloc(i + cnt * (newWlen - oldWlen) + 1);
 
    i = 0;
    while (*s) {
        // compare the substring with the result
        if (strstr(s, oldW) == s) {
            strcpy(&result[i], newW);
            i += newWlen;
            s += oldWlen;
        } else
            result[i++] = *s++;
    }
 
    result[i] = '\0';
    return result;
}

char *optimiser(char *str) {
  char *buf1 = str;
  char *buf2;

  while(find(buf1,'B')) {
    buf2 = replaceWord(buf1, "RBR", "F");
    buf1 = replaceWord(buf2, "RBL", "B");
    buf2 = replaceWord(buf1, "RBF", "L");

    buf1 = replaceWord(buf2, "LBR", "B");
    buf2 = replaceWord(buf1, "LBL", "F");
    buf1 = replaceWord(buf2, "LBF", "R");

    buf2 = replaceWord(buf1, "FBR", "L");
    buf1 = replaceWord(buf2, "FBL", "R");
    buf2 = replaceWord(buf1, "FBF", "B");

    buf1 = buf2;
  }

  return buf1;
}

/* 1 -> PID
 * 2 -> Turn back
 * 3 -> Turn left
 * 4 -> Turn right
 */
char labyrinth(char commande) {
    if(commande==6) {
        int result = 6;
        
        m3pi.cls();
        m3pi.locate(0, 1);
        m3pi.printf(optm);
        current_state();
        switch(sensors) {
            case 0x00: case 0x1C: case 0x18: case 0x10: case 0x07: case 0x03: case 0x1F: case 0x0F: case 0x1E:
                switch (optm[j]) {
                    case 'B':
                        m3pi.cls();
                        m3pi.locate(0, 1);
                        m3pi.printf("B");
                        step();
                        do{
                        m3pi.right(speed);
                        current_state();
                        }while(sensors!=0x01);
                        do{
                        m3pi.right(0.4*speed);
                        current_state();
                        }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
                        m3pi.stop();
                        
                        break;
                    case 'R':
                        m3pi.cls();
                        m3pi.locate(0, 1);
                        m3pi.printf("R");
                        step();
                        do{
                        m3pi.right(speed);
                        current_state();
                        }while(sensors!=0x01);
                        do{
                        m3pi.right(0.4*speed);
                        current_state();
                        }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
                        //wait(0.1);
                        m3pi.stop();
                        
                        break;
                    case 'L':
                        m3pi.cls();
                        m3pi.locate(0, 1);
                        m3pi.printf("L");
                        step();
                        do{
                        m3pi.left(speed);
                        current_state();
                        }while(sensors!=0x10);
                        do{
                        m3pi.left(0.4*speed);
                        current_state();
                        }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
                        //wait(0.1);
                        m3pi.stop();
                        
                        break;
                    case 'F':
                        m3pi.cls();
                        m3pi.locate(0, 1);
                        m3pi.printf("F");
                        step();
                        break;
                    default:
                        result = 0;
                        break;
                }
                j++;
                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);

                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;
                case 5:
                    // Optimisation
                    optm = optimiser(maze);
                    wait(5.0);
                    m3pi.cls();
                    m3pi.locate(0, 1);
                    m3pi.printf(optm);
                    command = 6;
                    break;
                case 6:
                    // Labyrinth
                    command = labyrinth(command);
                    break;
               
            }
        }
    }
}