/* Claude EMERY - 23/03/2024
Source : Programme de Pascal
Servo x 4 + Joystick x 2
*/

#include <Servo.h>

Servo servoSocle;  // Definition des servo
Servo servoBras1;
Servo servoBras2;
Servo servoPince;

#define joySoclePin A0  // Pin connexion joystick
#define joyBras1Pin A1
#define joyBras2Pin A2
const int joyPincePin = 13;

const int servoSoclePin = 5;  // Pin connexion servo
const int servoBras1Pin = 6;
const int servoBras2Pin = 9;
const int servoPincePin = 3;

int Socle;  // Valeur entrée joystick de 0 à 1023
int offsetSocle;   // Offset mouvement de -5 à +5
int angleSocle;  // Valeur sortie angle de 0  à 180°
int Bras1;
int offsetBras1;
int angleBras1;
int Bras2;
int offsetBras2;
int angleBras2;
int anglePince;

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

  pinMode(joySoclePin, INPUT);  // Definition Pin joystick en INPUT
  pinMode(joyBras1Pin, INPUT);
  pinMode(joyBras2Pin, INPUT);
  pinMode(joyPincePin, INPUT_PULLUP);

  servoSocle.attach(servoSoclePin);  // Attach Pin servo
  servoBras1.attach(servoBras1Pin);
  servoBras2.attach(servoBras2Pin); 
  servoPince.attach(servoPincePin);

  angleSocle = 90;  // Définir position de depart à 90°
  angleBras1 = 90;
  angleBras2 = 90;
  anglePince = 90;

  servoSocle.write(angleSocle);  // Mettre position de depart à 90°
  servoBras1.write(angleBras1);
  servoBras2.write(angleBras2);
  servoPince.write(anglePince);

  delay(500);
}

void loop() {  // Les fonctions ...
  SocleRun();
  Bras1Run();
  Bras2Run();
  Affichage();
  PinceRun();
  delay(150);
}

void SocleRun(){  // Fonction pour la rotation du socle
  Socle = analogRead(joySoclePin);  // Lecture joystick socle (de 0  à 1023)
  offsetSocle = map(Socle,0,1023,-5,5);  // Offset (de -5 a +5) selon position joystick
  angleSocle += offsetSocle;  // Variation position  (de -5 a +5) selon offset
  angleSocle = constrain(angleSocle,0,180);  // Limite l'angle de 0 a 180 °
  servoSocle.write(angleSocle);  // Mouvement du servo suivant l'angle
}

void Bras1Run(){  // Fonction pour le mouvement du bras1
  Bras1 = analogRead(joyBras1Pin);
  offsetBras1 = map(Bras1,0,1023,-5,5);
  angleBras1 += offsetBras1;
  angleBras1 = constrain(angleBras1,0,180);
  servoBras1.write(angleBras1);
}

void Bras2Run(){  // Fonction pour le mouvement du bras2
  Bras2 = analogRead(joyBras2Pin);
  offsetBras2 = map(Bras2,0,1023,-5,5);
  angleBras2 += offsetBras2;
  angleBras2 = constrain(angleBras2,0,180);
  servoBras2.write(angleBras2);
}


void PinceRun(){  // Fonction pour l'ouverture/fermeture de la pince
  if (digitalRead(joyPincePin) == LOW) {
    servoPince.write(0);
    } else {
      servoPince.write(90);
      }
}

void Affichage(){  // Fonction d'affichage des angles. POUR DEBUG ONLY
  /*
  Serial.print(("offset Socle: "));
  Serial.println(offsetSocle);
  Serial.print("Angle de sortie Socle: ");
  Serial.print(angleSocle);
  Serial.println(" Degres");

  Serial.print(("offset Bras1: "));
  Serial.println(offsetBras1);
  Serial.print("Angle de sortie Bras1: ");
  Serial.print(angleBras1);
  Serial.println(" Degres");

  Serial.print(("offset Bras2: "));
  Serial.println(offsetBras2);
  Serial.print("Angle de sortie Bras2: ");
  Serial.print(angleBras2);
  Serial.println(" Degres");
  */
}