/*  Claude EMERY - 21/11/2023 - Servomoteur et joystick
    Droite/Gauche du socle - Servo1
    Avant/Arriere du bras - Serv02
    Ouverture de la pince - Servo3
*/
/*
 * Claude EMERY
 * 08/08/2023
 * 4 servomoteur + 2 joystick HU-M16 + Arduino Uno
 */
#include <Servo.h>
  
Servo servo1;  //Servomotor
Servo servo2;
Servo servo3;

int Horz =A0;  //Sur A0 - Pilote Servo1 - Rotation socle
int Vert = A1;  //Sur A1 - Pilote Servo2 - Bras 1
int Bouton = 2;  //Sur D2 - Pilote Servo3 - Pince
int Led = 3;
int servoVal;
  
void setup(){
  servo1.attach(11);
  servo2.attach(10);
  servo3.attach(9);
  Serial.begin(9600);
  pinMode(Bouton, INPUT_PULLUP);
  pinMode(Led, OUTPUT);
}
  
void loop(){
  //Servo1 - Horz - Rotation socle
  servoVal = analogRead(Horz);
  servoVal = map(servoVal, 0, 1023, 0, 180);  //Angle 0°-180°
  servo1.write(servoVal);
  //Serial.println(servoVal);
  
  //Servo2 - Vert - Bras 1
  servoVal = analogRead(Vert);
  servoVal = map(servoVal, 0, 1023, 20, 160); //Angle 20°-160°
  servo2.write(servoVal);
  //Serial.println(servoVal);

  //Servo3 - Bouton - Pinces
    if (digitalRead(Bouton) == LOW){
      servo3.write(160);
      digitalWrite(Led, HIGH);
      Serial.println("OK"); 
      }
    else
      Serial.println("NOK"); 
      servo3.write(90);
      digitalWrite(Led, LOW);

  //delay(15);  
}