/*
==============================================================================================================
Pour que le programme fonctionne correctement les servos moteurs doivent être connecté de la façon suivante :
Pour une patte les 3 servos doivent se suivrent (ex : pin0 ,pin1, pin2)
Les tibia doivent être sur les pins 0 - 3 - 6 - 9 - 12 - 15
Les femur doivent être sur les pins 1 - 4 - 7 - 10 - 13
Les coxa doivent être sur les pins 2 - 5 - 8 - 11 - 14 
==============================================================================================================
 */
 
#include <Wire.h>
#include <Servo.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
Servo servo16; //femur patte 5
Servo servo17; //coxa patte 5

#define TIBIA_MIN  150 //tibia en haut
#define TIBIA_MAX  400 //tibia en bas
#define TIBIA_CALIBRATE 220 //62°

#define FEMUR_MIN  200 //200
#define FEMUR_MAX  490 //500
#define FEMUR_CALIBRATE 420 //115°

#define COXA_MIN  100 //coxa à gauche
#define COXA_MAX  550 //coxa à droite
#define COXA_CALIBRATE 320 //90°

#define AVANT 0
#define AVANT_GAUCHE 1
#define AVANT_DROITE 5
#define ARRIERE 3
#define ARRIERE_GAUCHE 4
#define ARRIERE_DROITE 2

struct legPart { //Structure donnant accées aux paramètres d'une patte
  int legMin;
  int legMax;
  int legCalibrate;
} legPart;

struct legPart tibia;
struct legPart femur;
struct legPart coxa;

int patte[6][3];  //chaque patte est un tableau contenant le numero du pin pour tibia/femur/coxa. Ex : acceder au pin du gémur patte 2 -> patte[1][0]

struct capteurs { //structure donnant accées aux valeurs des 3 capteurs
  int avant;
  int droite;
  int gauche;
} capteurs;

struct capteurs cap;

//capteur ultrason
const byte TRIGGER_AVANT = 2; // Broche TRIGGER
const byte ECHO_AVANT = 3;    // Broche ECHO
const byte TRIGGER_DROITE = 4; // Broche TRIGGER
const byte ECHO_DROITE = 5;    // Broche ECHO
const byte TRIGGER_GAUCHE = 6; // Broche TRIGGER
const byte ECHO_GAUCHE = 7;    // Broche ECHO
const unsigned long MEASURE_TIMEOUT = 25000UL; // 25ms = ~8m à 340m/s
const float SOUND_SPEED = 340.0 / 1000;

void setup() 
{
  pinMode(TRIGGER_AVANT, OUTPUT);
  pinMode(TRIGGER_DROITE, OUTPUT);
  pinMode(TRIGGER_GAUCHE, OUTPUT);
  digitalWrite(TRIGGER_AVANT, LOW); // La broche TRIGGER doit être à LOW au repos
  digitalWrite(TRIGGER_DROITE, LOW); // La broche TRIGGER doit être à LOW au repos
  digitalWrite(TRIGGER_GAUCHE, LOW); // La broche TRIGGER doit être à LOW au repos
  pinMode(ECHO_AVANT, INPUT);
  pinMode(ECHO_DROITE, INPUT);
  pinMode(ECHO_GAUCHE, INPUT);
  
  
  servo16.attach(10); //femur
  servo17.attach(11); //coxa
  Serial.begin(9600);
  pwm.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
  
  tibia={TIBIA_MIN,TIBIA_MAX,TIBIA_CALIBRATE};
  femur={FEMUR_MIN,FEMUR_MAX,FEMUR_CALIBRATE};
  coxa={COXA_MIN,COXA_MAX,COXA_CALIBRATE};

  int numPin=0; //assigne les pins
  for(int i=0; i<6; i++)
  {
    for(int j=0; j<3; j++)
    {
      patte[i][j]=numPin;
      numPin++;
    }
      patte[5][1]=100;
      patte[5][2]=100;
   }
delay(10);
}


// Permet de convertir les degrés en pulselen
int degToPuls(int degrees, struct legPart leg) {
  return(map(degrees, 0, 180, leg.legMin, leg.legMax));
}
// Permet de convertir les pulselen en degrés
int pulsToDeg(int pulselen, struct legPart leg) {
  return(map(pulselen, leg.legMin, leg.legMax, 0, 180));
}

/*
========================================
Fonctions de contôle des servos moteurs
========================================
*/

//commander une patte
void mouvPatte(int patteNum, int tibiaPuls, int femurPuls, int coxaPuls){
    
    pwm.setPWM(patte[patteNum][0],0, tibiaPuls);
    
    if(patte[patteNum][1]==100)
      {servo16.write(pulsToDeg(femurPuls,femur));}
    else
      {pwm.setPWM(patte[patteNum][1],0,femurPuls);}
    
    if(patte[patteNum][2]==100)
      {servo17.write(pulsToDeg(coxaPuls,coxa));}
    else
      {pwm.setPWM(patte[patteNum][2],0, coxaPuls);}
}


// Mettre toutes les pattes à la même position
void all(int tibiaPuls, int femurPuls, int coxaPuls){ 
  for(int i=0; i<6; i++){
    mouvPatte(i, tibiaPuls, femurPuls, coxaPuls);
    }
}

/*
 ==================
 Mouvements de base
 ==================
 */
 
void tourneGauche(int vitesse){
     mouvPatte(1,TIBIA_CALIBRATE,350,410);
     mouvPatte(4,TIBIA_CALIBRATE,350,410);
     delay(vitesse);
     mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
     mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
     delay(vitesse);
     mouvPatte(2,TIBIA_CALIBRATE,350,410);
     mouvPatte(5,TIBIA_CALIBRATE,350,410);
     delay(vitesse);
     mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
     mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
     delay(vitesse);
     mouvPatte(0,TIBIA_CALIBRATE,350,410);
     mouvPatte(3,TIBIA_CALIBRATE,350,410);
     delay(vitesse);
     mouvPatte(0,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
     mouvPatte(3,TIBIA_CALIBRATE,FEMUR_CALIBRATE,410);
     delay(vitesse);
     all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
     delay(vitesse);
}

void tourneDroite(int vitesse){
     mouvPatte(1,TIBIA_CALIBRATE,350,230);
     mouvPatte(4,TIBIA_CALIBRATE,350,230);
     delay(vitesse);
     mouvPatte(1,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
     mouvPatte(4,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
     delay(vitesse);
     mouvPatte(2,TIBIA_CALIBRATE,350,230);
     mouvPatte(5,TIBIA_CALIBRATE,350,230);
     delay(vitesse);
     mouvPatte(2,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
     mouvPatte(5,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
     delay(vitesse);
     mouvPatte(0,TIBIA_CALIBRATE,350,230);
     mouvPatte(3,TIBIA_CALIBRATE,350,230);
     delay(vitesse);
     mouvPatte(0,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
     mouvPatte(3,TIBIA_CALIBRATE,FEMUR_CALIBRATE,230);
     delay(vitesse);
     all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
     delay(vitesse);
}

void lever(void){
  all(TIBIA_MIN,FEMUR_MIN,COXA_CALIBRATE);
  delay(200);
  all(TIBIA_MIN,250,COXA_CALIBRATE);
  delay(200);
  all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
  delay(500);
}

void faireUnPas(int mode, int vitesse) //mode = direction
{
mouvPatte((2+mode)%6,TIBIA_CALIBRATE, 350, 230);
  delay(vitesse);
  mouvPatte((2+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, 230);
  delay(vitesse);
  mouvPatte((4+mode)%6,TIBIA_CALIBRATE, 350, 410);
  delay(vitesse);
  mouvPatte((4+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, 410);
  delay(vitesse);
  mouvPatte((0+mode)%6,TIBIA_CALIBRATE, 350, COXA_CALIBRATE);
  mouvPatte((3+mode)%6,TIBIA_CALIBRATE, 350, COXA_CALIBRATE);
  delay(vitesse);
  mouvPatte((2+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  mouvPatte((4+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  mouvPatte((1+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, 410);
  mouvPatte((5+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, 230);
  delay(vitesse);
  mouvPatte((0+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  mouvPatte((3+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  delay(vitesse);
  mouvPatte((1+mode)%6,TIBIA_CALIBRATE, 350, COXA_CALIBRATE);
  delay(vitesse);
  mouvPatte((1+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  delay(vitesse);
  mouvPatte((5+mode)%6,TIBIA_CALIBRATE, 350, COXA_CALIBRATE);
  delay(vitesse);
  mouvPatte((5+mode)%6,TIBIA_CALIBRATE, FEMUR_CALIBRATE, COXA_CALIBRATE);
  delay(vitesse);
}

/*
 =============================
 Fonctions liées aux capteurs
 =============================
 */
void getCapteurs()
{
  digitalWrite(TRIGGER_AVANT, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_AVANT, LOW);
  long measure_avant = pulseIn(ECHO_AVANT, HIGH, MEASURE_TIMEOUT);
  cap.avant = measure_avant / 20.0 * SOUND_SPEED;

  digitalWrite(TRIGGER_DROITE, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_DROITE, LOW);
  long measure_droite = pulseIn(ECHO_DROITE, HIGH, MEASURE_TIMEOUT);
  cap.droite = measure_droite / 20.0 * SOUND_SPEED;

  
  digitalWrite(TRIGGER_GAUCHE, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_GAUCHE, LOW);
  long measure_gauche = pulseIn(ECHO_GAUCHE, HIGH, MEASURE_TIMEOUT);
  cap.gauche = measure_gauche / 20.0 * SOUND_SPEED;
}

void afficheCapteurs()
{
  Serial.print("Avant : ");
  Serial.println(cap.avant);
  Serial.print("Droite : ");
  Serial.println(cap.droite);
  Serial.print("Gauche ");
  Serial.println(cap.gauche);
  Serial.println("==============");
}

/*
 ========================
 Séquences de mouvements 
 ========================
 */

void marcheClassique(int vitesse)
{
  getCapteurs();
  if((cap.avant<20 and cap.droite< cap.gauche))
  {
    tourneGauche(vitesse);
    delay(300);
    tourneGauche(vitesse);
    delay(300);
    tourneGauche(vitesse);
    delay(300);
    tourneGauche(vitesse);
    delay(300);
  }
  else if((cap.avant<20 and cap.droite> cap.gauche) or cap.gauche<20)
  {
    tourneDroite(vitesse);
    delay(300);
    tourneDroite(vitesse);
    delay(300);
    tourneDroite(vitesse);
    delay(300);
    tourneDroite(vitesse);
    delay(300);
  }
  else if(cap.droite<20)
  {
    tourneGauche(vitesse);
  }
  else if(cap.gauche<20)
  {
    tourneDroite(vitesse);
  }
  else
  {
    faireUnPas(AVANT,vitesse);
  }
}

void longeMur(int vitesse)
{
  getCapteurs();
  if(cap.droite<15)
  {
    faireUnPas(AVANT_GAUCHE,vitesse);
  }
  else if(cap.droite>25)
  {
    faireUnPas(AVANT_DROITE,vitesse);
  }
  else 
  {
    faireUnPas(AVANT,vitesse);
  }
}

void loop()
{

marcheClassique(50);
delay(500);

}