// ****** ROBOT ********
// BASE  1 potentiometre et 1 servo  
// BRAS 1 et BRAS 2 , 1 joystick avec 2 axes et 2 servos
// PINCE 1 Bouton poussoir et 1 servo , 2 BP valeur max serrage


#include <Servo.h>
// Création des objets Servo
Servo servo_BASE;                  // servo de la base
Servo servo_BRAS1;                 // servo du bras1
Servo servo_BRAS2;                 // servo du bras2
Servo servo_PINCE;                 // servo de la pince

//Constantes
#define POTBASE_PIN A0            // potentiometre Base sur broche A0
#define JSTBRAS1_PIN A1           // joystick BRAS1 sur broche A1
#define JSTBRAS2_PIN A2           // joystick BRAS2 sur broche A2
#define BPPINCE_PIN 2             // Bouton Poussoir ouverture PINCE sur broche 2
#define BPvmax_plus_PIN 7         // Bouton Poussoir plus V max fermeture PINCE sur broche 7
#define BPvmax_moins_PIN 8        // Bouton Poussoir moins V max fermeture PINCE sur broche 8

const int pinservo_BASE = 3;       // signal PWM du servo_BASE sur broche 3
const int pinservo_BRAS1 = 5;      // signal PWM du servo_BRAS1 sur broche 5
const int pinservo_BRAS2 = 6;      // signal PWM du servo_BRAS2 sur broche 6
const int pinservo_PINCE = 9;      // signal PWM du servo PINCE sur broche 9

//Variables
int base;                   // valeur entrée potentiometre base (de 0 à 1023 ) 
int angle_base;             // valeur sortie angle axe base (de 0 à 180°)

int bras1;                  // valeur entrée joystick  bras1 (de 0 à 1023 )
int offset1;                // offset mouvement bras1   (de -5  à +5 )
int angle_bras1;            // valeur sortie angle axe bras1 (de 0 à 180°)

int bras2;                  // valeur entrée joystick  bras2 (de 0 à 1023 )
int offset2;                // offset mouvement bras2   (de -5  à +5 )
int angle_bras2;            // valeur sortie angle axe bras2 (de 0 à 180°)

bool bp_pince;              //  état BP pince
int val_max_pince;          // valeur maximun angle fermeture pince (de 0 à 90°)
int angle_pince;            // valeur sortie angle pince (de 0 à 90°)

void setup() 
{
  Serial.begin(9600);                    //initialisation moniteur série a 9600 bauds

//    BASE
  pinMode(POTBASE_PIN, INPUT);           //potentiometre base en entrée
  servo_BASE.attach(pinservo_BASE);      // attache le servo base a pinservo
  angle_base = 90;                       // définir position de depart BASE a 90°
  servo_BASE.write(angle_base);          // mettre position de depart BASE a 90°

//    BRAS 1
  pinMode(JSTBRAS1_PIN, INPUT);          //joystick bras1 en entrée
  servo_BRAS1.attach(pinservo_BRAS1);    // attache le servo bras1 a pinservo
  angle_bras1 = 90;                      // définir position de depart BRAS1 a 90°
  servo_BRAS1.write(angle_bras1);        // mettre position de depart BRAS1 a 90°

//    BRAS 2
  pinMode(JSTBRAS2_PIN, INPUT);          //joystick bras2 en entrée
  servo_BRAS2.attach(pinservo_BRAS2);    // attache le servo bras2 a pinservo
  angle_bras2 = 90;                      // définir position de depart BRAS2 a 90°
  servo_BRAS2.write(angle_bras2);        // mettre position de depart BRAS2 a 90°

//    PINCE
  pinMode(BPvmax_plus_PIN, INPUT_PULLUP);    // Bouton poussoir Vmax + pince en entrée digitale
  pinMode(BPvmax_moins_PIN, INPUT_PULLUP);   // Bouton poussoir Vmax - pince en entrée digitale
  pinMode(BPPINCE_PIN, INPUT_PULLUP);        // Bouton poussoir pince en entrée digitale   
  servo_PINCE.attach(pinservo_PINCE);        // attache le servo pince a pinservo
  angle_pince = 90;                          // définir position de depart pince a 90° (ouverte)
  servo_PINCE.write(angle_pince);            // mettre position de depart pince a 90° (ouverte)
  val_max_pince=  5 ;                        // définir position finale pince a 5°(fermeture)

  delay( 2000);                          // attente 2 s
 }

void loop()
 {
//  BASE
      base =analogRead(POTBASE_PIN);                  // lecture potentiometre base (de 0  à 1023)
      angle_base =map(base,0,1023,0,180);             // conversion en dégrés
      servo_BASE.write(angle_base);                   // effectue position base
//  BRAS 1
      bras1 = analogRead(JSTBRAS1_PIN);                 // lecture joystick bras1 (de 0  à 1023)
      offset1 = map(bras1,0,1023,-5,5);                 // offset1 variable (de -5 a +5) selon position joystick
      angle_bras1 += offset1;                           // variation position  (de -5 a +5) selon offset
      angle_bras1 = constrain(angle_bras1,0,180);       // limite position de 0 a 180 °
      servo_BRAS1.write(angle_bras1);                   // effectue position bras1
//  BRAS 2
      bras2 = analogRead(JSTBRAS2_PIN);                 // lecture joystick bras2 (de 0  à 1023)
      offset2 = map(bras2,0,1023,-5,5);                 // offset2 variable (de -5 a +5) selon position joystick
      angle_bras2 += offset2;                           // variation position  (de -5 a +5) selon offset
      angle_bras2 = constrain(angle_bras2,0,180);       // limite position de 0 a 180 °
      servo_BRAS2.write(angle_bras2);                   // effectue position bras2
 //  PINCE  
     if (digitalRead(BPvmax_plus_PIN)==LOW) {                            // si appui BP val +
                                            val_max_pince +=1;           // incrémenter valeur max pince 
                                            }
      if (digitalRead(BPvmax_moins_PIN)==LOW) {                           // si appui BP val +
                                               val_max_pince -=1;         // décrémenter valeur max pince 
                                              }
      val_max_pince = constrain(val_max_pince,5,90);       // limite position fermeture de 5 a 90 °  

      bp_pince = digitalRead(BPPINCE_PIN);                 // lecture etat BP pince
      if (bp_pince==HIGH) { 
                           angle_pince=val_max_pince;              // si pas appui pince fermé selon valeur max
                         }
      else {
             angle_pince=90; ;              // sinon ouvrir pince selon valeur max
           }
      servo_PINCE.write(angle_pince);                   // effectue position pince

 //  AFFICHAGE
  Serial.print("Base: ");
  Serial.print(angle_base);
  Serial.println(" Degres");

  Serial.print("offset1: ");
  Serial.print(offset1);
  Serial.print("  Angle de sortie Bras 1: ");
  Serial.print(angle_bras1);
  Serial.println(" Degres"); 

  Serial.print("offset2: ");
  Serial.print(offset2);
  Serial.print("  Angle de sortie Bras 21: ");
  Serial.print(angle_bras2);
  Serial.println(" Degres"); 

 
  Serial.print("Angle de sortie Pince: ");
  Serial.print(angle_pince);
  Serial.println(" Degres");

  Serial.println();      // ligne vide de separation

 delay(150);  // valeur 150
}