/*
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
Servo servo17; //coxa

#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°

struct legPart {
  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]


void setup() {
  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));
}

//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);
    }
}

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

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

void tourneGauche(int vitesse){
  //mouvPatte();  
}

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 loop() {
  all(TIBIA_CALIBRATE,FEMUR_CALIBRATE,COXA_CALIBRATE);
  delay(5000);
  marcheAvant(100);
  marcheAvant(100);
  marcheAvant(100);
  marcheArriere(100);
  marcheArriere(100);
  marcheArriere(100);
  delay(5000);
}