#define   Fn      10.00

#define   Zeta    0.2

#define   K       1.0

#define   T_ms    10  // 100 Hz

#define   A_step  5.0 // Amplitude

#define   c_step  200  // Période = 2*c_step*T_ms

  c++; c=c%c_step;

  if(!c)

  {

    Step=!Step;

    c=0;

  }

  x_nn=A_step*(double)Step;

  y_nn=Sys2(x_nn, 0.1, Wn, K, T_s);

  Serial.print(x_nn); Serial.print(",");

  Serial.println(y_nn); 

double Sys2(double x_nn, double zeta, double wn, double k, double T)

{

  // Paramètre du système

  double a1=2.0*zeta/wn;

  double a2=1.0/(wn*wn);




  const double b0=(a1/(2.0*T))+(a2/(T*T));

  const double b1=-2.0*a2/(T*T);

  const double b2=(-1.0*a1/(2.0*T))+(a2/(T*T));

  const double b[3]={b0,b1,b2};




  // Variables de l'entrée et la sortie

  static double y[2];

  static double x;

  double y_nn=0.0;




  // Calcul de la nouvelle sortie

  y_nn= -(y[0]*(1.0+b[1]))-(y[1]*b[2])+(k*x); // y[1]: y(n-2), y[0]: y(n-1)

  y_nn/=b[0];




  // Mise à jour de la sortie

  y[1]=y[0];

  y[0]=y_nn;




  // Mise à jour de l'entrée

  x=x_nn;




  // Renvoie du résultat

  return y_nn;

}

/*
               -------------
               -           -
 x(n) ----------   SYS2    ---------- y(n)  
               -           -
               -------------
              
 * 1. Implémentation d'un système du 2nd ordre  
 *    y(n)=> {x(n-1), y(n-1), y(n-2)} 
 * 2. Définition de la fonction Sys2()
 * 3. La réponse indicielle d'un système en 2nd ordre 
 * 4. Etc.
 * 
*/

 
#define   Fn      10.00
#define   Zeta    0.2
#define   K       1.0
#define   T_ms    10  // 100 Hz 

#define   A_step  5.0 // Amplitude
#define   c_step  200  // Période = 2*c_step*T_ms

double Wn=2.0*PI*Fn;
double T_s=(double)T_ms/1000.0;
double x_nn=0.0,y_nn=0.0; 


// Paramètres de l'échelon
unsigned long c=0; // Compteur (période)  
bool Step=false; 


void setup()
{
  // Port série de la réponse du système 
  Serial.begin(9600); 
}

void loop()
{ 
  // Le signal échelon x(n) => [0, A_step]
  c++; c=c%c_step; 
  if(!c) 
  {
    Step=!Step;
    c=0; 
  }
  x_nn=A_step*(double)Step; 
  
  // Calcul de la sortie  
  y_nn=Sys2(x_nn, 0.1, Wn, K, T_s);

  // Affichage x(n)/y(n)
  Serial.print(x_nn); Serial.print(","); 
  Serial.println(y_nn);  

  // Période d'échantillonnage 
  delay(T_ms);
}



double Sys2(double x_nn, double zeta, double wn, double k, double T)
{
  // Paramètre du système 
  double a1=2.0*zeta/wn;
  double a2=1.0/(wn*wn);

  const double b0=(a1/(2.0*T))+(a2/(T*T)); 
  const double b1=-2.0*a2/(T*T); 
  const double b2=(-1.0*a1/(2.0*T))+(a2/(T*T)); 
  const double b[3]={b0,b1,b2};

  // Variables de l'entrée et la sortie 
  static double y[2]; 
  static double x; 
  double y_nn=0.0;
  
  // Calcul de la nouvelle sortie 
  y_nn= -(y[0]*(1.0+b[1]))-(y[1]*b[2])+(k*x); // y[1]: y(n-2), y[0]: y(n-1)
  y_nn/=b[0];
  
  
  // Mise à jour de la sortie  
  y[1]=y[0];
  y[0]=y_nn;
  

  // Mise à jour de l'entrée
  x=x_nn; 
  
  // Renvoie du résultat 
  return y_nn;
}