tron 19 12 06

#define INTERVAL 66
#define P_TURN 100 // sur 1000 // p=30 pour interv=33
#include "lowlevel.hpp"

/* Constantes pour le timeout */
const unsigned long MEASURE_TIMEOUT = 25000UL; // 25ms = ~8m à 340m/s

typedef struct {
  bool far_l, far_r, close_l, close_r;
} IR_Capture;

typedef enum { INIT, RUN, LEFT, RIGHT, PAUSE, STOP } State ;
bool line_l, line_r; // ligne sur la gauche/droite (capteur infrarouge)
bool obstacle_f; // obstacle devant (capteur ultrason)
bool turn, turn_l; // y a une proba de tourner au cours de la course (marche aléatoire), et une proba pour savoir si on tourne à gauche ou droite
bool setting_up, fatal_error, success, start;
int speedL=0;
int speedR=0;
bool dirL = true;
bool dirR = true;
double prop =1.100;
bool led=false;
int vit=64;
int rand_turn_duration = 400; // ms
int compteur_tour = 0;
const int distance_lim = 1700;

State state;

void setup() {
  // port série
  Serial.begin(115200);

  setup_motors();
  setup_sensors();


  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);

  state=INIT;
}

IR_Capture detect_IR()
{
  // capteurs infrarouges
  IR_Capture cap;

  return cap;
}

void detect_US()
{
  // capteurs ultrasons
  digitalWrite(PINTRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PINTRIG, LOW);
  long measure = pulseIn(PINECHO, HIGH, MEASURE_TIMEOUT);
  //Serial.println(measure);
  obstacle_f = (measure!=0 && measure<distance_lim);
}

void refresh()
{
  // met à jour les globals (conditions)
  detect_IR();
  detect_US();
  if (turn) {
    if(compteur_tour<int(rand_turn_duration/INTERVAL)) compteur_tour++;
    else {
      turn=false;
      compteur_tour=0;
    }
    //Serial.println(compteur_tour);
  }
  else {
    turn = (random(1000)<P_TURN);
    turn_l = (random(2)==1); }
}

void transition()
{
  if (fatal_error || success)
    {
      state = STOP;
      return;
    }
  switch (state)
  {
    case INIT:
      if (setting_up) {
        state = INIT;
      }
      else {
        if (!obstacle_f && !line_l && !line_r && !turn) {
          state = RUN;
        }
        else if (turn && turn_l) {
          state = LEFT;
        }
        else if (turn && !turn_l) {
          state = RIGHT;
        } else
        {
          state = PAUSE;
        }
      }
      break;
    case RUN:
      if (!line_l && !line_r && !turn && !obstacle_f) {
        state = RUN;
      }
      else if (line_r || (!line_l && !line_r && turn && turn_l) && !obstacle_f) {
        state = LEFT;
      }
      else if (line_l || (!line_l && !line_r && turn && !turn_l) && !obstacle_f) {
        state = RIGHT;
      }
      else if (obstacle_f) {
        state = PAUSE;
      }
      else {
        state = STOP;
      }
      break;
    case LEFT:
      if (!line_l && !line_r && !obstacle_f && !turn) {
        state = RUN;
      }
      else if ((line_r && !line_l)||(turn && turn_l)) { // else if ((line_l || line_r && !obstacle_f) || (!line_l && !line_r && turn && turn_l)) {
        state = LEFT;
      }
      else if (obstacle_f) {
        state = PAUSE;
      }
      else {
        state = STOP;
      }
      break;
    case RIGHT:
      if (!line_l && !line_r && !obstacle_f && !turn) {
        state = RUN;
      }
      else if ((line_l && !line_r)||(turn && !turn_l)) { // (line_l || line_r && !obstacle_f)
        state = RIGHT;
      }
      else if (obstacle_f) {
        state = PAUSE;
      }
      else {
        state = STOP;
      }
      break;
    case PAUSE:
      if (!line_l && !line_r && !obstacle_f) {
        state = RUN;
      }
      else if (!line_l) {
        state = LEFT;
      }
      else if (!line_r && line_l) {
        state = RIGHT;
      }
      else {
        state = PAUSE;
      }
      break;
    case STOP:
      if (start) {
        state = INIT;
      }
      else {
        state = STOP;
      }
      break;
    default:
      state = STOP;
      break;
  }

}

void tron_loop() {
  refresh();
  transition();

  switch (state)
  {
    case INIT:
      Serial.println(F("INIT"));
      state=RUN;
      break;
    case RUN:
      Serial.println(F("RUN"));
      led=false;
      dirL=true;
      dirR=true;
      speedL=vit;
      speedR=vit;
      break;
    case LEFT:
      Serial.println(F("LEFT"));
      led=false;
      dirL=false;
      dirR=true;
      speedL=vit;
      speedR=vit;
      break;
    case RIGHT:
      Serial.println(F("RIGHT"));
      led=false;
      dirL=true;
      dirR=false;
      speedL=vit;
      speedR=vit;
      break;
    case PAUSE:
      Serial.println(F("PAUSE"));
      led=true;
      speedL=0;
      speedR=0;
      break;
    case STOP:
      Serial.println(F("STOP"));
      led=false;
      speedL=0;
      speedR=0;
      break;
    default:
      break;
  }

  write_motors(dirL,dirR,speedL,speedR);
  digitalWrite(LED_BUILTIN, (led)?HIGH:LOW);
}

void loop() {
  unsigned long t0=millis();
  tron_loop();
  unsigned long elapsed=millis()-t0;
  if (elapsed>INTERVAL) printf("Excessive execution time\n");
  else delay(INTERVAL-elapsed);
}