/*=================== Mixage 2 voies pour 2 variateurs====================

                    Programme pour Arduino Nano 

         Servo Variateur droit sur le pin°A1
         Servo Variateur gauche sur le pin°A2
         
         Récepteur signal "GAZ" sur le pin D2
         Récepteur signal "DIRECTION" sur le pin D4

         Correction du bruit en fonction du taux de moyennage pour éviter vibration 
         Nécessite librairie "SoftRcPulseOut" (RC Navy)---> https://github.com/digistump/DigisparkArduinoIntegration
===========================================================================
*/

#include <SoftRcPulseOut.h>


SoftRcPulseOut myservo1;               // Création du servo variateur droit
SoftRcPulseOut myservo2;               // Création du servo variateur gauche



#define SERVO_PIN1                  A1 // Servo Variateur droit attaché au Pin A1
#define SERVO_PIN2                  A2 // Servo Variateur gauche attaché au Pin A2

#define signal1                      2 // Signal radio "GAZ" attaché au Pin D2
#define signal2                      4 // Signal radio "DIRECTION" attaché au Pin D4

#define REFRESH_PERIOD_MS 20           // Période de rafraîchissement

#define MOY_SUR_1_VALEURS       0
#define MOY_SUR_2_VALEURS       1
#define MOY_SUR_4_VALEURS       2
#define MOY_SUR_8_VALEURS       3
#define MOY_SUR_16_VALEURS      4
#define MOY_SUR_32_VALEURS      5

#define TAUX_DE_MOYENNAGE       MOY_SUR_4_VALEURS  /* Choisir ici le taux de moyennage parmi les valeurs precedentes possibles listees ci-dessus */

#define MOYENNE(Valeur_A_Moyenner,DerniereValeurRecue,TauxDeMoyEnPuissanceDeDeux)  Valeur_A_Moyenner=((((Valeur_A_Moyenner)*((1<<(TauxDeMoyEnPuissanceDeDeux))-1)+(DerniereValeurRecue))/(1<<(TauxDeMoyEnPuissanceDeDeux)))+(TauxDeMoyEnPuissanceDeDeux-1))

int neutre = 1500;                           //neutre des servos dans le calcul moyennage

int val1;                         // Variable pour stocker la valeur du signal "GAZ"
int val2;                         // Variable pour stocker la valeur du signal "DIRECTION"

int pos;                          // Variable pour stocker la position du servo

int valA, valB, valxA, valxB, diff ;       // Déclare les variables pour les servos 
int posmin = 45;                           // Déclare en degrés la position mini des servos
int posmax = 135;                          // Déclare en degrés la position maxi des servos
int neutral = 90;                           // Déclare en degrés la position neutre des servos

void setup()
{
 
  myservo1.attach(SERVO_PIN1);  // Servo attaché au pin SERVO_PIN A1
  myservo2.attach(SERVO_PIN2);  // Servo attaché au pin SERVO_PIN A2
 
}

void loop()
{

  unsigned long currentMillis = millis();                         // Affectation du compteur de temps




#define TAUX_DE_MOYENNAGE       MOY_SUR_4_VALEURS                // Choix du taux de moyennage
  static int ValMoyennee1;                                       // Valeur à moyennée
  val1 = pulseIn(signal1, HIGH, 30000);                          // Lecture du signal radio pour le servo  Variateur droit 
  val1 = map(val1, neutre - 500, neutre + 500, 0, 180);          // Mise à l’échelle
  val1 = constrain(val1, 0, 180);                                // Conversion en degres
  ValMoyennee1 = MOYENNE(ValMoyennee1, val1, TAUX_DE_MOYENNAGE); // Correction du bruit en fonction du taux de moyennage
  val1 = ValMoyennee1;
  
//---------------------------------------------------------------------------------- 
#define TAUX_DE_MOYENNAGE       MOY_SUR_4_VALEURS                // Choix du taux de moyennage
  static int ValMoyennee2;                                       // Valeur à moyennée
  val2 = pulseIn(signal2, HIGH, 30000);                          // Lecture du signal radio pour le servo  Variateur gauche  
  val2 = map(val2, neutre - 500, neutre + 450, 0, 180);          // Mise à l’échelle
  val2 = constrain(val2, 0, 180);                                // Conversion en degres
  ValMoyennee2 = MOYENNE(ValMoyennee2, val2, TAUX_DE_MOYENNAGE); // Correction du bruit en fonction du taux de moyennage
  val2 = ValMoyennee2;

//---------------------------------------------------------------------------------- 
  valxA = val1;      // Donne à la variable (valxA) la valeur de signal de "GAZ"
  valxB = val2;      // Donne à la variable (valxB) la valeur de signal de "DIRECTION"
  
  diff = (valxB - neutral);                         // Calcul l'ecart entre les deux variateurs
  valA = valxA - diff;  valB = valxA + diff;        // Calcul la position des servos
  valA=constrain(valA, posmin, posmax);             // Calibre les valeurs pour le variateur dans la plage acceptée
  valB=constrain(valB, posmin, posmax);             // Calibre les valeurs pour le variateur dans la plage acceptée
  
//---------------------------------------------------------------------------------- 
  
  myservo1.write(valA);                                          // Positionne le servo Variateur droit
  myservo2.write(valB);                                          // Positionne le servo Variateur gauche
  
  delay(REFRESH_PERIOD_MS);                                      // Temps nécessaire au mouvement des servos

  SoftRcPulseOut::refresh();                                     // Rafraîchissement de SoftRcPulseOut


}
//=============================FIN DE PROGRAMME ===========================================