[Arduino] Autonomus car [Not Working]

#include <VirtualWire.h>
#include <Grove_I2C_Motor_Driver.h> //import de la bibliotheque pour controller les moteurs
#include "PinChangeInt.h" //import bibliotheque pour avoir plus que 2 attach interupt

#define I2C_ADDRESS 0x0f //adresse de la carte pour controller les moteurs

#define captLL 8 // pin du capteur gauche
#define captL 7 // pin du capteur milieu gauche
#define captR 3 // pin du capteur milieu droite
#define captRR 4 // pin du capteur droite
#define captTR 2 // pin du capteur de tour de la roue
#define captLV 1 // pin du capteur de ligne verte /!\Analogique/!\

#define speede 60 //vitesse de croisiere
#define speederap 70 //vitesse rapide (lors de la rotation notament (si on le met a la vitesse de croisiere, il bloque))

#define tempstour 2000 //temp max que dure le fait de tourner
#define tempsav 300 //temp que la voiture avance avant de tourner
#define timerpcorrd 200 //timer de la petite correction
#define timergcorrd 200 //timer de la grosse correction

#define startpoint 8 //point de départ du path finding
#define finishpoint 12 //point d'arriver du path finding
#define usepathfinding true //var qui definit si on utilise la path finding

#define lvstep 300 //Step qui permet au capteur de décider si il y a un obstacle ou pas

String msg;

/*
   Structure qui contient un chemain, ca taille et
   la distance de ce dernier
*/
typedef struct pathdist pathdist;
struct pathdist {
  int path[10];
  int taillepath;
  double dist;
};

/*
   Variable ou est stocker la map.
   C'est composer d'une masterliste qui contient 16
   liste fille qui contiennt toute 4 nombre.
   Les liste fille represente une intersection. Leurs
   place dans la masterliste represente quelle intesection
   c'est et les nombre dedan represente ce qu'il y a a
   droite, en bas, a gauche et en haut.
*/
int point[16][4] = {
  { 2,  6,  0,  0}, // 1
  { 3,  7,  1,  0}, // 2
  { 4,  8,  2,  0}, // 3
  { 5, 10,  3,  0}, // 4
  { 0, 11,  4,  0}, // 5
  { 7,  9,  0,  1}, // 6
  { 8,  0,  6,  2}, // 7
  { 0,  0,  7,  3}, // 8
  {10, 14,  0,  6}, // 9
  {11, 13,  9,  4}, //10
  { 0, 16, 10,  5}, //11
  {13, 15,  0,  0}, //12
  { 0,  0, 12, 10}, //13
  {15,  0,  0,  9}, //14
  {16,  0, 14, 12}, //15
  { 0,  0, 15, 13}  //16
};

/*
   Contient la taille du chemain correspondant a celui
   dans point
*/
int distance[16][4]  = {
  {    297,     210,       0,       0}, // 1
  {297 * 2,     210,     297,       0}, // 2
  {297 * 2,     210, 297 * 2,       0}, // 3
  {    297, 210 * 3, 2 * 297,       0}, // 4
  {      0, 3 * 210,     297,       0}, // 5
  {    297, 2 * 210,       0,     210}, // 6
  {2 * 297,       0,     297,     210}, // 7
  {      0,       0, 2 * 297,     210}, // 8
  {5 * 294, 2 * 210,       0, 2 * 210}, // 9
  {    297,     210, 5 * 297, 3 * 210}, //10
  {      0, 2 * 210,     297, 3 * 210}, //11
  {4 * 297,     210,       0,       0}, //12
  {      0,       0, 4 * 297,     210}, //13
  {    297,       0,       0, 2 * 210}, //14
  {5 * 297,       0,     297,     210}, //15
  {      0,       0, 5 * 297, 2 * 210}  //16
};

/*
   Etats des capteurs.
   (ll = gauche
   l = milieu gauche
   r = milieu droite
   rr = droite
   tr = capteur de toure
   lv = capteur de ligne verte)
*/
volatile boolean ll;
volatile boolean l;
volatile boolean r;
volatile boolean rr;
volatile boolean tr;
volatile int lv;

volatile char etat; //etat de la voiture (a = avancer, d = tourner 90° droite, g = tourner 90° gauche, r = correction droite, l = correction gauche, s = stop, n = avancer avant de tourner a droite, m = la meme a gauche)
volatile int timer; //temps durant lequelle l'etat ne change pas saufe si on doit tourner

char cleanpath[10]; //chemain propre. Suite d'instruction simple que la voiture va comprendre
int dirtypath[10]; //Chemain sale. C'est la ou on a étape par étape les point par lequelle la voiture passe
int pathsize = 0; //Taille de ce chemain
int pathstep = -1; //ou on en est dans ce chemain
boolean changestep = false; //permet de n'encrémenter le step qu'une fois par virage et pas en boucle pdt le virage

char dir = 'g'; //direction de base de la voiture

boolean changetr = false; //Permet de détecter quand le capteur du pinTR change d'etat
boolean changelv = false; //Permet de détecter quand le capteur du pinLV change d'etat

volatile int countlv = 0; //Nombre de cran compter pour la dist qu'on cherche de la ligne verte
volatile int countgene = 0; //Nombre de cran compter pour la dist qu'on cherche avant la ligne verte

int ptdepart = 8;
int ptarrive = 7;
int taille = 20;
int dist = 20;

/*
   Fonction setup ou on definit le serial, les moteurs,
   les capteurs, l'etat et le path finding si besoin
*/
void setup() {
  Serial.begin(9600);

  Serial.println("Setup");
  Serial.println("Launch Motor.begin");
  Motor.begin(I2C_ADDRESS);
  Serial.println("Motor.begin launched");
  vw_setup(2000);//radio comm setup
  vw_set_tx_pin(6);//pin setup

  pinMode(captLL, INPUT);
  pinMode(captL, INPUT);
  pinMode(captR, INPUT);
  pinMode(captRR, INPUT);
  pinMode(captTR, INPUT);

  PCintPort::attachInterrupt(captRR, ai, CHANGE);
  PCintPort::attachInterrupt(captLL, ai, CHANGE);
  PCintPort::attachInterrupt(captR, ai, CHANGE);
  PCintPort::attachInterrupt(captL, ai, CHANGE);

  PCintPort::attachInterrupt(captTR, ai_tr, CHANGE);

  etat = 'a';

  Serial.println("Finding path...");

  findpath();

  Serial.println("Setup done.");
}

/*
   Fonction loop.
*/
void loop() {

  Serial.print(analogRead(captLV));
  Serial.print(" - ");
  Serial.print(timer < millis());
  Serial.print(" - ");
  Serial.print(countlv);
  Serial.print(" - ");
  Serial.print(etat);
  Serial.print(" - ");
  Serial.println(pathstep);
  Serial.flush();

  msg = (String) "11" + (String) "22" + (String) ptdepart + (String) taille + (String) dist, (String) ptarrive;

  /*Serial.println(" - 1");
  Serial.flush();*/

  /*Serial.print(rr);
  Serial.print(" - ");
  Serial.print(r);
  Serial.print(" - ");
  Serial.print(l);
  Serial.print(" - ");
  Serial.println(ll);*/

  /*ll = digitalRead(captLL);
  l = digitalRead(captL);
  r = digitalRead(captR);
  rr = digitalRead(captRR);
  tr = digitalRead(captTR);
  lv = analogRead(captLV);*/

  setOrder();

  /*Serial.println(" - 2");
  Serial.flush();*/

  order();

  /*Serial.println(" - 3");
  Serial.flush();*/

  distcount();

  /*Serial.println(" - 4");
  Serial.flush();*/

  filllv();

  /*Serial.println(" - 5");
  Serial.flush();*/

  for (int i = 0; i < 3; i++){
    sendInfo((String) i + msg);
  }

  /*Serial.println(" - 6");
  Serial.flush();*/

}

void sendInfo(String msg){
  unsigned char payload[20];
  msg.getBytes(payload, 20);
  vw_send((uint8_t *)payload, msg.length()); // On envoie le message
  vw_wait_tx(); // On attend la fin de l'envoi
}

/*
   Fonction qui setup le path finding si nessessaire.
   Elle definit le clearpath
*/
void findpath() {

  if (usepathfinding) {
    Serial.println("Searching path...");

    pathdist path;
    defpath(&path);

    path = pathsearch(startpoint, finishpoint, 7, &path);
    pathtrad(&path, cleanpath);
    pathsize = path.taillepath;

    for (int i = 0; i < path.taillepath; i++) {
      Serial.print(dirtypath[i]);
      if (i != path.taillepath - 1) {
        Serial.print(", ");
      } else {
        Serial.println();
      }
    }

    Serial.println("Path found");

  } else {

    cleanpath[0] = 'h';
    cleanpath[1] = 'h';
    cleanpath[2] = 'd';
    cleanpath[3] = 'h';
    cleanpath[4] = 'h';
    cleanpath[5] = 'h';
    cleanpath[6] = 'd';
    cleanpath[7] = 'h';
    cleanpath[8] = 'h';
    cleanpath[9] = 'h';

    pathsize = 7;

    dirtypath[0] = 8;
    dirtypath[1] = 7;
    dirtypath[2] = 6;
    dirtypath[3] = 9;
    dirtypath[4] = 14;
    dirtypath[5] = 15;
    dirtypath[6] = 12;
    dirtypath[7] = 0;
    dirtypath[8] = 0;
    dirtypath[9] = 0;
  }
}

/*
   Permet de compter la distance entre deux obstacle
*/
void distcount() {
  lv = analogRead(captLV);

  if (lv > lvstep && !changelv) {
    changelv = true;
    if (countlv > 0) {
      filllv();
    }
    countlv = 0;
  } else if (!(lv > lvstep) && changelv) {
    changelv = false;
  }

  if (tr && !changetr) {
    countgene++;
    changetr = true;
    if (lv < lvstep) {
      countlv++;
    }
  } else if (!tr && changetr && lv < lvstep) {
    changetr = false;
  }
}

/*
   Choisis l'action a effectuer en fonction de l'etat
*/
void order() {
  switch(etat) {
    case 'd':
    case 'g':
      tourner();
      break;
    case 'r':
      corredroit();
      break;
    case 'l':
      corregauche();
      break;
    case 's':
      stopp();
      break;
    default:
      avancer();
  }
}

/*
   Permer de changer l'etat de la voiture en fonction
   des capteurs et de l'etat précedent
*/
void setOrder() {
  boolean timee = timer < millis();

  if (etat == 'm' && timee ) {

    etat = 'd';
    timer = millis() + tempstour;
    timee = false;

  } else if (etat == 'n' && timee ) {
    etat = 'g';
    timer = millis() + tempstour;
    timee = false;

  }

  if (changestep && !( etat == 'm' || etat == 'n' ) ) {
    changestep = false;
  } else if (!changestep && ( etat == 'm' || etat == 'n' ) ) {
    pathstep++;
    changestep = true;
  }


  if ( !ll && !l && !r && !rr && timee ) {
    // Forward
    etat = 'a';
    timee = false;

  } else if ( !ll && !l && !r && rr && timee ) {
    //Big correction right
    etat = 'm';
    timer = millis() + tempsav;
    /*etat = 'r';
    timer = millis() + timergcorrd;*/
    timee = false;

  } else if ( ll && !l && !r && !rr && timee ) {
    //Big correction left
    etat = 'n';
    timer = millis() + tempsav;
    /*etat = 'l';
    timer = millis() + timergcorrd;*/
    timee = false;

  } else if ( !ll && !l && r && !rr && timee ) {
    // Little correction right
    etat = 'r';
    timer = millis() + timerpcorrd;
    timee = false;

  } else if ( !ll && l && !r && !rr && timee ) {
    // Little correction left
    etat = 'l';
    timer = millis() + timerpcorrd;
    timee = false;

  } else if ( r && rr && etat != 'g' && etat != 'd' ) {
    // Turn right
    etat = 'm';
    timer = millis() + tempsav;
    timee = false;

  } else if ( ll && l && ( !r || !rr ) && etat != 'g' && etat != 'd' ) {
    // Turn left
    etat = 'n';
    timer = millis() + tempsav;
    timee = false;

  }
}


/*
   Fonction récursive permettant de trouver le meilleur
   chemain
*/
pathdist pathsearch(int frompt, int topt, int tour, pathdist *pathtemp) {
  pathdist path;
  defpath(&path);
  copiepath(&path, pathtemp);

  path.path[path.taillepath] = frompt;
  path.taillepath++;

  if (frompt == topt) {
    return path;
  }

  else if (tour == 0) {
    path.dist = 9999;
    return path;
  }

  else {
    pathdist lst[4];
    int lstsize = 0;
    for (int i = 0; i < 4; i++) {
      if (point[frompt - 1][i] != 0) {
        path.dist += distance[frompt - 1][i];
        lst[lstsize] = pathsearch(point[frompt - 1][i], topt, tour - 1, &path);
        path.dist -= distance[frompt - 1][i];
        lstsize++;
      }
    }

    copiepath(&path, &lst[0]);

    for (int i = 1; i < lstsize; i++) {
      if ( lst[i].dist < path.dist ) {
        copiepath(&path, &lst[i]);
      }
    }
    return path;
  }
}

/*
   Traduit le chemain du path finding (point 1 puis 2 puis 4) en
   direction
*/
void pathtrad(pathdist *path, char order[10]) {
  for (int i = 0; i < 10; i++) {
    dirtypath[i] = path->path[i];
  }
  for (int i = 0; i < path->taillepath - 1; i++) {
    if (point[path->path[i] - 1][0] == path->path[i + 1]) {
      order[i] = traddir('d');
    } else if (point[path->path[i] - 1][1] == path->path[i + 1]) {
      order[i] = traddir('b');
    } else if (point[path->path[i] - 1][2] == path->path[i + 1]) {
      order[i] = traddir('g');
    } else {
      order[i] = traddir('h');
    }
  }
}


/*
   Traduit les dirrection par rapport a la map en dirrectionpar rapport
   a la voiture
*/
char traddir(char dire) {
  switch (dir) {
    case 'h':
      switch (dire) {
        case 'h':
          return 'h';
          break;
        case 'd':
          dir = 'd';
          return 'd';
          break;
        case 'b':
          //exit(0);
          break;
        case 'g':
          dir = 'g';
          return 'g';
          break;
      }
      break;
    case 'd':
      switch (dire) {
        case 'h':
          dir = 'h';
          return 'g';
          break;
        case 'd':
          return 'h';
          break;
        case 'b':
          dir = 'b';
          return 'd';
          break;
        case 'g':
          //exit(0);
          break;
      }
      break;
    case 'b':
      switch (dire) {
        case 'h':
          //exit(0);
          break;
        case 'd':
          dir = 'd';
          return 'g';
          break;
        case 'b':
          return 'h';
          break;
        case 'g':
          dir = 'g';
          return 'd';
          break;
      }
      break;
    case 'g':
      switch (dire) {
        case 'h':
          dir = 'h';
          return 'd';
          break;
        case 'd':
          //exit(0);
          break;
        case 'b':
          dir = 'b';
          return 'g';
          break;
        case 'g':
          return 'h';
          break;
      }
      break;
  }
}

void ai() {
  //noInterrupts();
  rr = digitalRead(captRR);
  r = digitalRead(captR);
  l = digitalRead(captL);
  ll = digitalRead(captLL);

  if ( ( etat == 'd'  && l ) || ( etat == 'g' && r ) ) {
    etat == 'a';
    rr = r = l = ll = 0;
  }
  //interrupts();
  //setOrder();
}

void ai_tr() {
  tr = digitalRead(captTR);

  distcount();
}


/*
   Permet de definir des valeurs par défaut dans une var path
*/
void defpath(pathdist *path) {
  for (int i = 0; i < 10; i++) {
    path->path[i] = 0;
  }
  path->taillepath = 0;
  path->dist = 0;
}

/*
   Permet de copier la deusieme var path dans la 1ere
*/
void copiepath(pathdist *patha, pathdist *pathb) {
  for (int i = 0; i < 10; i++) {
    patha->path[i] = pathb->path[i];
  }
  patha->taillepath = pathb->taillepath;
  patha->dist = pathb->dist;
}

void filllv() {
  ptdepart = cleanpath[pathstep];
  ptarrive = cleanpath[pathstep + 1];
  taille = countlv;
  dist = countgene - countlv;

}

/*
   Fonction permettant de savoir ou tourner lors d'une
   intersection
*/
void tourner() {
  countgene = 0;
  if (pathstep >= pathsize) {
    etat = 's';
    stopp();
    timer = 9999;
  } else {
    Serial.print("           ");
    Serial.print(pathstep);
    Serial.print(" Direction : ");
    Serial.println(cleanpath[pathstep]);
    if (cleanpath[pathstep] == 'h') {
      avancer();
      etat = 'a';
    } else if (cleanpath[pathstep] == 'd') {
      tournerd();
      etat = 'd';
    } else if (cleanpath[pathstep] == 'g') {
      tournerg();
      etat = 'g';
    }
  }
}

/*
   Permer de tourner a gauche
*/
void tournerg() {
  Motor.speed(MOTOR1, speederap);
  Motor.speed(MOTOR2, -speederap);
}

/*
   Permer de tourner a droite
*/
void tournerd() {
  Motor.speed(MOTOR1, -speederap);
  Motor.speed(MOTOR2, speederap);
}

/*
   Permet d'avancer
*/
void avancer() {
  Motor.speed(MOTOR1, speede);
  Motor.speed(MOTOR2, speede);
}

/*
   permet de ce corriger a gauche
*/
void corregauche() {
  Motor.speed(MOTOR1, speede);
  Motor.speed(MOTOR2, -speede);
}

/*
   permet de ce corriger a droite
*/
void corredroit() {
  Motor.speed(MOTOR1, -speede);
  Motor.speed(MOTOR2, speede);
}

/*
   permet au lolipanzer de s'arreter
*/
void stopp() {
  Motor.speed(MOTOR1, 0);
  Motor.speed(MOTOR2, 0);
  exit(0);
}