/*********************************************************************/
//        Programme de régulation proportionnelle.                   //
//        Régulation autour d'une consigne pré-enregistrée.          //
//        Réglage du gain par potentiomètre.                         //
/*********************************************************************/

/***********************************/
#include <EEPROM.h>
//Contrôle la commande de moteur brushless "aviation"
//Mesure par capteur infrarouge Sharp sur AN0.
/************************************/
//Contrôle du niveau de la réserve d'air par
//potentiomètre sur AN1
// Le potentiomètre de gain est sur AN2 .
/************************************/
// Méthode de réglage, si nécessaire :
/****************************************************************************************/
/********* Mettre D6 à 0 à l'aide d'un shunt pour se mettre en phase de réglage.*********/
/****************************************************************************************/

// Mettre le potentiomètre de gain en position médiane et le potentiomètre
// de niveau tourné du côté OV.
// Mettre en marche et ajuster le potentiomètre de niveau pour que 
// la réserve d'air se gonfle à la hauteur désirée, sans ou très peu 
// de fuite d'air. Lors de cette phase le disposistif se comporte
// exactement de la même façon que lors du test du programme nommé :
// CommandeNo0. On accélère uniquement le moteur, il n'y a pas de régulation.

// Quand la réserve d'air est à la hauteur désirée :
// Retirer le shunt : les valeurs sont enregistrée en EEPROM.
// Attendre que le programme fasse une phase de démarrage progressif puis
// ajuster le gain afin d'avoir une régulation energique, sans atteindre
// le valeur qui provoque le "pompage" de la réserve d'air.
//
// Le potentiomètre de niveau est devenu inactif en fonctionnement normal.
//
int distance=0;
int offset_puiss= 0; 
int hauteur_base=0;
int gain = 1; // gain ajustable par potentiomètre
int dist_utile=0;
unsigned int temps_pulse;
int sortie_servo=9;
unsigned long debut_periode;
int pinDistance =0,pinOffset=1,pinGain=2;
int pinReglage=6;
  // n° de pin à mettre à zéro pour utiliser le réglage préliminaire
boolean flag_demarre=1,flag_enregistre=0;
long tempo_dist,tempo_gain;
// utilisées quelquefois pour éviter des débordements dans des multiplications.
void mesures(void) 
{
  distance=0;  gain = 0;
 for(int ii=0; ii<16; ii++)
   {
   distance += analogRead(pinDistance);
   gain += analogRead(pinGain);
   }
  dist_utile = (distance >>4);
  dist_utile -= hauteur_base;
  gain >>= 4;   
   /**********************************************************************/
  // L'effet du potentiomètre de gain sera de multiplier la valeur de l'erreur
  // représentée par dist_utile, d'un coefficient qui varie linéairement de
  // 1/4 lorsque gain = 0 à 1 lorsque gain vaut 512.
  // Dans la plage de valeur supérieure, l'erreur représentée par dist_utile
  // sera multipliée par un coefficient qui varie linéairement de 1 pour
  // gain = 512 à 4 pour gain = 1024.
  // En option, une autre formule  pour que ce facteur multiplicatif gain
  // varie de 1 à 8 en plage haute du potentiomètre, ou de 1 à 16 ,
 //  ou de 1/8 à 1 en plage inférieure.
  // L'utilisation d'un entier long pour ce calcul permet de garder une certaine
  // précision dans le calcul en évitant les débordements possibles avec une
  // variable de type int. 
  /**********************************************************************/
   tempo_dist = long(dist_utile);
   tempo_gain = long(gain); 
  if(gain < 513)
  {
    tempo_dist = (tempo_dist * (512 + (3*tempo_gain)));
    tempo_dist >>= 11; // équivalent à la division par 4*
     /*
    // formule pour que le gain varie entre 1/8 et 1
    tempo_dist = (tempo_dist * (512 + (7*tempo_gain)));
    tempo_dist >>= 12; // équivalent à la division par 8*512
    */  
  }
  if (gain > 512)
  {
    // formule pour que que le gain varie de 1 à 4
   tempo_dist = (tempo_dist * ( (3 * gain) - 1024) );
    tempo_dist >>= 9;  // équivalent à la division par 512    
    /*
      // formule pour que que le gain varie de 1 à 8
   tempo_dist = (tempo_dist * ( (7 * gain) - 3072) );
    tempo_dist >>= 9;  // équivalent à la division par 512 
    */
    /*
        // formule pour que que le gain varie de 1 à 16
   tempo_dist = (tempo_dist * ( (15 * gain) - 7168) );
    tempo_dist >>= 9;  // équivalent à la division par 512 
    */
  }
  dist_utile = int(tempo_dist); 
/*********************************************************************/  
  //utiliser la ligne suivante si la tension augmente quand la réserve
  //d'air augmente, sinon il faut la mettre en // commentaire.
/*********************************************************************/ 
/*********************************************************************/
   dist_utile =  - dist_utile;
/*********************************************************************/  
}
void faire_pulse(int commande)
{
   temps_pulse = (990 + constrain(commande,0,1023));
   // constrain utilisé pour que la durée de pulse reste dans les
   // limites de environ 1 à 2 msec, même en cas d'errer sur commande.
  digitalWrite(sortie_servo,HIGH);
  delayMicroseconds(temps_pulse);
  digitalWrite(sortie_servo,LOW);
}

void setup()
{
  pinMode(sortie_servo,OUTPUT);
  digitalWrite(sortie_servo,LOW);
  pinMode(pinReglage,INPUT);
  Serial.begin(9600);// pour déboggage éventuel
  analogReference(DEFAULT);
}
unsigned compte=0; 
void loop()
{
  unsigned int compte_reglage=0;
  
  while( !digitalRead(pinReglage) )
 // teste si on demande un nouveau réglage des consignes de régulation
 // Si oui :
 // Le potentiomètre de niveau règle la puissance moteur,
 // L'entrée A0 est lue par le convertisseur Analogique-Numérique
 // Aucune régulation n'est effectuée.
 // Quand le shunt sur D6 est ôté, le niveau de puissance moteur
 // et la valeur de A0 sont enregistrés en EEPROM pour servir
 // de consigne à la boucle de régulation.
  {
    distance=0;  gain = 0;
  debut_periode = micros();
 for(int ii=0; ii<16; ii++)
   {
   distance += analogRead(pinDistance);
   offset_puiss += analogRead(pinOffset);
   }
   hauteur_base = (distance >>4);
   offset_puiss >>= 4; 
   flag_enregistre = 1;
   faire_pulse(offset_puiss);
   if(compte_reglage>150)
   {
     compte_reglage = 0;
   Serial.print("hauteur_base = ");
   Serial.println(hauteur_base);
   Serial.print("offset_puiss = ");
   Serial.println(offset_puiss);
   }
   compte_reglage++;
   while( (micros()-debut_periode) < 19990); 
  }
  // Si la boucle de réglage a été demandée, enregistrer les consignes à sa sortie :
  if(flag_enregistre)
  {
  
    EEPROM.write(0,highByte(hauteur_base));
    EEPROM.write(1,lowByte(hauteur_base));
    EEPROM.write(2,highByte(offset_puiss));
    EEPROM.write(3,lowByte(offset_puiss));
    flag_enregistre=0;
  }  
  // Sinon : 
  // lire les réglages enregistrés en EEPROM dans un réglage précédent 
   offset_puiss= (EEPROM.read(2)<<8)+EEPROM.read(3);
   hauteur_base = (EEPROM.read(0)<<8)+EEPROM.read(1);
   Serial.print("hauteur_base = ");
   Serial.println(hauteur_base);
   Serial.print("offset_puiss = ");
   Serial.println(offset_puiss);
   delay(1000);
  while(flag_demarre)
  {
   debut_periode= micros();
   
  faire_pulse(0); 
  compte++;
 if(compte>300) // (300 x 20 msec = 6 sec)
    {
       compte = 0;          
       flag_demarre = 0; // pour finir la boucle            
    } 
     while( (micros()-debut_periode) < 19990); 
  }
  
  //refaire une boucle pour une augmentation progressive de la puissance
   flag_demarre = 1; 
  while(flag_demarre) 
  {
   debut_periode = micros(); 
   faire_pulse( compte );
   compte++;
   if(compte > 1023) flag_demarre=0;
   mesures();
   if(compte > dist_utile+offset_puiss) flag_demarre = 0; 
   if(compte < 100) flag_demarre= 1;
  while( (micros()-debut_periode) < 19990){};     
  }
  
 // Passage en fonctionnement normal : boucle infinie
 while(1)
 {
   debut_periode = micros(); 
  mesures();
  faire_pulse(dist_utile + offset_puiss );
   
  while( (micros()-debut_periode) < 19990){};           
 } 
}