#include <SoftwareSerial.h>

/*--------------------PROTOCOLE----------------------------*/
#define SET_TARGET_BYTE 0x04   // represents Pololu protocol set target command
#define DEVICE_NUM 0x0C     // default device number for Pololu protocol
#define PROTOCOL_BYTE 0xAA    // Pololu protocol identifier (0xAA)
#define POLOLU_BIT_MASK 0x7F
#define SPEED_BYTE 7
#define GETMOVING_STATE 0x13
/*--------------------Mini-Maestro 12----------------------*/
#define rxPin 3  // pin 3 TX (pas utilisé)
#define txPin 4  // pin 4 RX

SoftwareSerial liaisonSerie(rxPin, txPin);

// void deplaceServo(unsigned char servo, unsigned int target);
// void Set_Speed(unsigned char servo, unsigned int vitesse);

int pattes[4][3] = {
  {2, 1, 0},
  {5, 4, 3},
  {8, 7, 6},
  {11, 10, 9}
};

const int base = 0;   // CHEVILLE   =   BASE      = 0
const int milieu = 1; // HANCHE     =   MILIEU    = 1
const int bout = 2;   // TIBIAS     =   BOUT      = 2

void setup()
{
 liaisonSerie.begin(9600);
 Serial.begin(9600);
}

void loop()
{
  char inChar;
  while (Serial.available() > 0) {
    inChar = Serial.read();
    Serial.println(inChar);
  }
  switch(inChar) {
    case 'a':
      lever(0);
      lever(1);
      lever(2);
      lever(3);
      break;
    case 'b':
      baisser(0);
      baisser(1);
      baisser(2);
      baisser(3);
      break;
  }
}

void tourner(int patte, int angle) {
  setAngle(pattes[patte][base], angle, 40);
}

void lever(int patte) {
  setAngle(pattes[patte][milieu], 0, 40);
  setAngle(pattes[patte][bout], 55, 40);
}

void baisser(int patte) {

  setAngle(pattes[patte][milieu], 180, 20);
  setAngle(pattes[patte][bout], 55, 20);
}

/************************************************/
/*  Déplace le servo moteur. servo (de 0 à 11)  */
/*  angle (0 à 180) vitesse (1 à 127)           */
/************************************************/
void setAngle(unsigned char servo, unsigned int angle, unsigned int vitesse)
{
    //On modifie les paramètre suivant les caractéristique de son servomoteur.
    // de 0 a 180 degree. 992ms a 2000 PWM.
    angle = (map(angle, 0, 180, 992, 2000) * 4);

    // VITESSE
    byte command[] =
    { PROTOCOL_BYTE, DEVICE_NUM, SPEED_BYTE, servo, vitesse & POLOLU_BIT_MASK, (vitesse >> 7) & POLOLU_BIT_MASK};
    for(int i = 0; i < 6; i++)
    {
      liaisonSerie.write(command[i]);
    }

    // ANGLE
    byte command2[] =
    { PROTOCOL_BYTE, DEVICE_NUM, SET_TARGET_BYTE,
      servo, angle & POLOLU_BIT_MASK, (angle >> 7) & POLOLU_BIT_MASK
    };
    for(int i = 0; i < 6; i++)
    {
      liaisonSerie.write(command2[i]);
    }
}