// 1 joystick et 2 servo
// test pour le robot base 
// gestion souple liaison joystick servo


#include <Servo.h>
// Création de l'objet Servo
Servo servo_BRAS1;                 // servo du bras1
Servo servo_BRAS2;                 // servo du bras2

//Constantes
#define JSTBRAS1_PIN A0           // joystick BRAS1 sur broche A0
#define JSTBRAS2_PIN A1           // joystick BRAS2 sur broche A1
const int pinservo_BRAS1 = 6;      // signal PWM du servo_BRAS1 sur broche 6
const int pinservo_BRAS2 = 5;      // signal PWM du servo_BRAS2 sur broche 5

//Variables
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°)



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

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

  delay( 2000);                 // attente 2 s
 }

void loop()
 {
  
      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

      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

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

 delay(150);
}