Projet Arduino: Asservissement PWM – Correcteur Proportionnel (P)

float getMean(float *tabMoy, int Nm, float vin0)
{
  // Variables locales 
  static int J=0; 
  float somme_1=0.0; 
  float VMFiltre=0.0; 
  
  // Filtrage: Calcul de la Moyenne Glissante 
  for (int i=0; i<Nm; i++) somme_1+=tabMoy[i]; 
  VMFiltre=(somme_1/(float)Nm);

  // Mise à jour du tableau des VM 
  valMoy[J]=(vin0+VMFiltre)/2.0;
  J++; J%=Nm;

  // Retour de la VM
  return VMFiltre; 
}

double Satur(double vin, double vmax)
{
  double vout=0.0; 
  double x=0.0; 
  const double Eps=1E-12 ; // Eviter la division /0
  
  vout=vmax*tanh(vin/(vmax+Eps));

  // Ou bien  
  /*
  x=vin/(vmax+Eps);  // tanh(x)=(1.0-exp(-2.0*x))/(1.0+exp(-2.0*x)); 
  vout=vmax*(1.0-exp(-2.0*x))/(1.0+exp(-2.0*x)); 
  */
  
  return vout; 
}

#define Out         9         // Sortie PWM  
#define In          A0        // Entrée analogique 
#define OutTOR      2         // Sortie TOR 
#define Consigne    3.5       // Tension de la consigne  
#define K           5.0       // Action Proportionnelle  
#define NMoy        150       // Moyenne glissante  
#define TCycMicroS  20000     // Délai de la boucle en µS


float   v_fil=0.0; 
float   v_fil_old=0.0; 
bool    LoopCyc=false; 
float   somme =0.0;
float   vin_volat=0.0; 
float   valMoy[NMoy];
int     Vin_A0=0; 
int     I=0; 
float   OutPWM=0.0; 
float   Cmd=0.0; 
float   Erreur_t=0.0;


void setup() 
{
  // Pinout 
  pinMode(Out, OUTPUT);
  pinMode(OutTOR, OUTPUT);

  // Init PWM
  analogWrite(Out, 0); 
  
  // Calcul de la moyenne actuelle 
  for (int i=0; i<NMoy; i++)
  {
    vin_volat=(float)analogRead(A0)*5.0/1023.0; 
    somme+=vin_volat; 
  }
  v_fil=(somme/(float)NMoy);
  somme=0.0; 

  // Initialisation du tableau des VM 
  for (int i=0; i<NMoy; i++) valMoy[i]= v_fil; 

  // Affichage 
  Serial.begin(250000); 
}


void loop() 
{
  // 1. Lecture de l'entrée A0
  vin_volat=(float)analogRead(A0)*5.0/1023.0; 

  // 2. Extraction de la valeur moyenne 
  v_fil= getMean(valMoy, NMoy, vin_volat);

  //3.1 Calul de l'erreur
  Erreur_t=(Consigne - v_fil);  // [-5, 5]

  //3.2 Génération du signal de la commande 
  Cmd=(K*Erreur_t)*255.0/5.0;   // [-X, X]

  //3.3 Saturation
  OutPWM=Satur(Cmd, 255.0);     // [-255, 255] 

  //4. Génération de la commande PWM
  analogWrite(Out, (unsigned char)abs(round(OutPWM))); // [0, 255] 

  //5. Affichage (Uniquement pour le test) 
  Serial.print(0.5*vin_volat-2.5);      Serial.print(","); 
  Serial.print(Consigne);       Serial.print(","); 
  Serial.print(v_fil);          Serial.print(","); 
  //Serial.println(Cmd*5.0/255.0); 
  Serial.println(OutPWM*5.0/255.0); 

  //6. Délai de la boucle 
  delayMicroseconds(TCycMicroS); 

  /*
  LoopCyc=!LoopCyc; 
  digitalWrite(OutTOR, LoopCyc); 
  */ 
}


double Satur(double vin, double vmax)
{
  double vout=0.0; 
  double x=0.0; 
  const double Eps=1E-12 ; // Eviter la division /0
  
  vout=vmax*tanh(vin/(vmax+Eps));

  // Ou bien  
  /*
  x=vin/(vmax+Eps);  // tanh(x)=(1.0-exp(-2.0*x))/(1.0+exp(-2.0*x)); 
  vout=vmax*(1.0-exp(-2.0*x))/(1.0+exp(-2.0*x)); 
  */
  
  return vout; 
}



float getMean(float *tabMoy, int Nm, float vin0)
{
  // Variables locales 
  static int J=0; 
  float somme_1=0.0; 
  float VMFiltre=0.0; 
  
  // Filtrage: Calcul de la Moyenne Glissante 
  for (int i=0; i<Nm; i++) somme_1+=tabMoy[i]; 
  VMFiltre=(somme_1/(float)Nm);

  // Mise à jour du tableau des VM 
  valMoy[J]=(vin0+VMFiltre)/2.0;
  J++; J%=Nm;

  // Retour de la VM
  return VMFiltre; 
}