word Analog2LogiV1(word in, word seuil)
{
   if (in>=seuil) return 1; 
   else return 0; 
}

word Analog2LogiV2(word nn, int pinAx, word seuil)
{
  float somme=0.0, vmoy=0.0; 

  switch (pinAx)
  {
    case 0:
      for(word i=0;i<nn; i++)somme+=(float)analogRead(A0); 
      vmoy=somme/(float)nn; 
      //Serial.print(vmoy); Serial.print(","); 
      if((word)vmoy>=seuil) return 1; 
      else return 0; break; 

    case 1:
      for(word i=0;i<nn; i++)somme+=(float)analogRead(A1); 
      vmoy=somme/(float)nn; 
      //Serial.print(vmoy);Serial.print(","); Serial.println((float)analogRead(A1)); 
      if((word)vmoy>=seuil) return 1; 
      else return 0; break;  
      
    case 2:
      for(word i=0;i<nn; i++)somme+=(float)analogRead(A2); 
      vmoy=somme/(float)nn; 
      if((word)vmoy>=seuil) return 1; 
      else return 0; break;  

    case 3:
      for(word i=0;i<nn; i++)somme+=(float)analogRead(A3); 
      vmoy=somme/(float)nn; 
      if((word)vmoy>=seuil) return 1; 
      else return 0; break; 

    case 4:
      for(word i=0;i<nn; i++)somme+=(float)analogRead(A4); 
      vmoy=somme/(float)nn; 
      if((word)vmoy>=seuil) return 1; 
      else return 0; break; 
      
    case 5:
      for(word i=0;i<nn; i++)somme+=(float)analogRead(A5); 
      vmoy=somme/(float)nn; 
      if((word)vmoy>=seuil) return 1; 
      else return 0; break;  

  
    default:
      return 0; 
  }
}

void InitStateIO(bool *Etapes, word numEtapes, word *Inputs,word numIn,  word *Outputs, word numOut)
{
    // Désactivation des étapes 
    for(word i=0;i<numEtapes;i++)
      Etapes[i]=false; 

    // Activation d'une ou plusieurs étapes par défaut 
    Etapes[0]=true; 

    // Initialisation des entrées 
    for(word i=0;i<numIn;i++)
      Inputs[i]=false; 

    // Initialisation des sorties 
    for(word i=0;i<numOut;i++)
      Outputs[i]=false;   

    // Init des pins des entrées  
    pinMode(2, INPUT);  // Dcy    - Inputs[0] 
    // PrinActif   -A0- Inputs[1] 
    // SecActif    -A1- Inputs[2] 
    
    // Init des pins des sorties  
    pinMode(3, OUTPUT); // PSec   - Outputs[0] 
    pinMode(4, OUTPUT); // StartSec
    pinMode(5, OUTPUT); // P
    pinMode(6, OUTPUT); // S      - Inputs[4]  
}

void GetInputs(word *Inputss)
{
  // Entrées digitales 
  Inputss[0]=digitalRead(2);              // D2
  Inputss[1]=Analog2LogiV2(N,0,VSeuil);   // A0
  Inputss[2]=Analog2LogiV2(N,1,VSeuil);   // A1
}

#define clkPin    2
#define dtPin     3
#define swPin     4   // Bouton Poussoir (Reset)

#define outPWM    8
#define StepPWM   15
#define numPWM    255  // Niveaux PWM


int nReset=1; 

void setup() 
{
  //Init 
  Serial.begin(19200);
  pinMode(clkPin,INPUT);
  pinMode(dtPin,INPUT);
  pinMode(swPin,INPUT_PULLUP);
  
  pinMode(outPWM,OUTPUT);
  analogWrite(outPWM,0);
}

void loop() 
{
  // Lecture de l'encodeur
  nReset=digitalRead(swPin); 
  int R=encodUpDown(clkPin,  dtPin,  numPWM, StepPWM, 200, nReset); 

  // Affichage (Test)
  Serial.print(R); Serial.print("\t");
  Serial.println(R); 
  
  // Mise à jour intensité 
  analogWrite(outPWM,R);
}

int encodUpDown(int clkpin, int dtpin, int rotmax,int stpwm, int delay_ms, int nrst) 
{ 
  static int rotVal  = 0;
  static bool clkState  = LOW, clkLast  = HIGH;

  clkState = digitalRead(clkpin);
  if ((clkLast == LOW) && (clkState == HIGH)) 
  {
    if (digitalRead(dtpin) == HIGH) // Down 
    {
      rotVal-=stpwm;
      if ( rotVal < 0) rotVal = 0;
    }
    else  // Up
    {
      rotVal+=stpwm;
      if ( rotVal >= rotmax)rotVal = rotmax;
    }
    delay(delay_ms);
  }
  clkLast = clkState;

  // Init 
  if (nrst==0)
  {
    rotVal   = 0; 
    clkState = LOW; 
    clkLast  = HIGH;
  }
  return rotVal; 
}

#define clkPin    2
#define dtPin     3
#define swPin     4   // Bouton Poussoir (Reset)
#define numPos    10  // de 0 à numPos-1 


int nReset=1; 

void setup() 
{
  //Init 
  Serial.begin(9600);
  pinMode(clkPin,INPUT);
  pinMode(dtPin,INPUT);
  pinMode(swPin,INPUT_PULLUP);
}

void loop() 
{
  nReset=digitalRead(swPin); 
  int R=getRot(clkPin,  dtPin,  numPos,200, nReset); 
  Serial.print(R); Serial.print("\t");
  Serial.println(R);
}

int getRot(int clkpin, int dtpin, int rotmax, int delay_ms, int nrst) 
{ 
  static int rotVal  = 0;
  static bool clkState  = LOW, clkLast  = HIGH;

  clkState = digitalRead(clkpin);
  if ((clkLast == LOW) && (clkState == HIGH)) 
  {
    if (digitalRead(dtpin) == HIGH) // Down 
    {
      rotVal--;
      if ( rotVal < 0) rotVal = rotmax-1;
    }
    else  // Up
    {
      rotVal++;
      if ( rotVal >= rotmax)rotVal = 0;
    }
    delay(delay_ms);
  }
  clkLast = clkState;

  // Init 
  if (nrst==0)
  {
    rotVal   = 0; 
    clkState = LOW; 
    clkLast  = HIGH;
  }
  return rotVal; 
}

#define     Seuil   600.0
void setup()
{
  // ADC 
  analogReadResolution(12);       // Ajustable <= 12 (4096 Niveaux) 

  // Affichage 
  Serial.begin(250000); 
}

void loop()
{
  // Lecture 
  float V1=(float)analogRead(A0); 
  V1=(V1*3.3/4095.0)*1000.0; 
  float V2=3300.0*(V1<Seuil); 
  
  // Affichage 
  Serial.print(V1); Serial.print("\t");
  Serial.println(V2);
  delay(10); 
}

float getData(float *dataout,int nn, unsigned long ts_us, int analogpin, float gain, int noDC)

float getData(float *dataout,int nn, unsigned long ts_us, int analogpin, float gain, int noDC)
{
  // Lecture des données brutes  
  float ts_f=0.0;
  unsigned long t11; 
  for(int i=0; i<nn; i++)
  {
    t11=micros();
    dataout[i]=(float)analogRead(54+analogpin); 
    delayMicroseconds(ts_us);
    ts_f+=(float)micros()-(float)t11; 
  }
  ts_f=ts_f/(float)nn; 
  
  // Conversion en V
  for(int i=0; i<nn; i++)
    dataout[i]=gain*dataout[i]*3.3/4095.0;
  
  // Suppression de la valeur DC 
  if(noDC==0) 
  {
    float Vm=0.0; 
    for(int i=0; i<nn;i++) Vm+=dataout[i]; 
    Vm/=(float)nn; 
    for(int i=0; i<nn;i++) dataout[i]=dataout[i]-Vm; 
  } 
  return 1E6/ts_f;
}

float setWindow(float *datain, float *dataout, int n, int type)

float getData(float *dataout,int nn, unsigned long ts_us, int analogpin, float gain, int noDC)
{
  // Lecture des données brutes  
  float ts_f=0.0;
  unsigned long t11; 
  for(int i=0; i<nn; i++)
  {
    t11=micros();
    dataout[i]=(float)analogRead(54+analogpin); 
    delayMicroseconds(ts_us);
    ts_f+=(float)micros()-(float)t11; 
  }
  ts_f=ts_f/(float)nn; 
  
  // Conversion en V
  for(int i=0; i<nn; i++)
    dataout[i]=gain*dataout[i]*3.3/4095.0;
  
  // Suppression de la valeur DC 
  if(noDC==0) 
  {
    float Vm=0.0; 
    for(int i=0; i<nn;i++) Vm+=dataout[i]; 
    Vm/=(float)nn; 
    for(int i=0; i<nn;i++) dataout[i]=dataout[i]-Vm; 
  } 
  return 1E6/ts_f;
}




float setWindow(float *datain, float *dataout, int n, int type)
{
  float Hi, t=0.0,t0=0.0, sig=0.0; 
  const float gain[6]={0.5581,0.5934,0.4765,0.6456,0.5581,1.0000};
    
  switch(type)
  {
    //Fenêtre de Hann
    case 0:
      for(int i=0;i<n; i++)
      {
        t=(float)i/float(n);  
        Hi=0.5-0.5*cos(2.0*PI*t); 
        dataout[i]=Hi*datain[i];
      }
      return gain[type];

    //Fenêtre de Hamming
    case 1:
      for(int i=0;i<n; i++)
      {
        t=(float)i/float(n);  
        Hi=0.54-0.46*cos(2.0*PI*t); 
        dataout[i]=Hi*datain[i];
      }
      return gain[type];

    // Fenêtre de Blackman
    case 2:
      for(int i=0;i<n; i++)
      {
        t=(float)i/float(n);  
        Hi=0.42-0.5*cos(2.0*PI*t)+0.08*cos(4.0*PI*t); 
        dataout[i]=Hi*datain[i];
      }
      return gain[type];
      
    //Fenêtre de Gauss
    case 3:
      for(int i=0;i<n; i++)
      {
        t=((float)i/float(n))-0.5;  
        t0=0.5;
        sig=t0/2.0;  
        Hi=exp(-0.5*t*t/(sig*sig)); 
        dataout[i]=Hi*datain[i];
      }
      return gain[type];

    //Fenêtre sin²
    case 4:
      for(int i=0;i<n; i++)
      {
        t=(float)i/float(n);  
        Hi=sin(2.0*0.5*PI*t)*sin(2.0*0.5*PI*t); // sin(x)²=(1-cos(2x))/2
        dataout[i]=Hi*datain[i];
      }
      return gain[type];

    default:
      for(int i=0;i<n; i++)
      {
        dataout[i]=datain[i];
      } 
      return gain[5];  
  }
}

#define squareOut   8       // Sortie Carrée

#define N           256     // Données N=2^n, TFD Nf=2^(n-1)
                            // Agit sur la Résolution fréquentielle!!!!!!!! 
                            // df= Fs/N
                             
#define Ts_us       100      // Ts=1/Fs: Approchée (voir le programme) 
                            // Agit sur la fréquence MAX du spéctre!!!!!!!! 
                            // FMXA = Fs/2
/*
  Type=0: Fenêtre de Hann
  Type=1: Fenêtre de Hamming
  Type=2: Fenêtre de Blackman
  Type=3: Fenêtre Gaussienne
  Type=4: Fenêtre Sin²
  Type=X: Sans Fenêtre 
*/


bool State=false; 
unsigned long T1=0, T2=0, T3=0; 
float Ts=0.0, Fs=0.0; 
float Data[N],Data1[N], Nfft=N>>1; 
float dF=0.0; 
float MaxIdMax[2]={0.0,0.0}; 
float Am, ReIm[2]={0.0,0.0};


const float q=1.0/4096.0; 


void setup()
{
  // ADC 
  analogReadResolution(12);       // Ajustable <= 12 (4096 Niveaux) 

  // Sortie 
  analogWrite(squareOut,128);     // 1 KHz
  
  // Affichage 
  Serial.begin(250000); 
}


void loop()
{ 
  /*
   * Analyse Temporelle des Fenêtres
  */
   
  // 1.1 Lecture des données
  Fs=getData(Data, N, Ts_us, 0, 1000.0,0); 
  //Serial.println(Fs); return; 

  // 1.2 Fenêtrage 
  int N0=6;
  float Datai[N0][N], Coefs[N0];
  for(int i=0;i<N0; i++)
    Coefs[i]=setWindow(Data, Datai[i], N, i);
  
  // 1.3 Visualisation
  static bool state=false;
  if(state==false)
  {
    Serial.println("");
    for (int i=0;i<N;i++)
    {
      Serial.print(Datai[0][i]); Serial.print("\t");
      Serial.print(Datai[1][i]); Serial.print("\t");
      Serial.print(Datai[2][i]); Serial.print("\t");
      Serial.print(Datai[3][i]); Serial.print("\t"); 
      Serial.print(Datai[4][i]); Serial.print("\t");
      Serial.println(Datai[5][i]); 
    }
    Serial.println("");
    state=!state;
  }
  return; 



  /*
   * Analyse Fréquentielle des Fenêtres
  */

  // 2.1 DFT 
  float RealFFT[N0][N], ImagFFT[N0][N];
  for(int i=0;i<N0; i++)
    dF=DFT(Datai[i], N, RealFFT[i], ImagFFT[i], Fs);
    
  // 2.2 Affichage
  /*
  static bool state=false;
  float A; 
  if(state==false)
  {
    Serial.println("");
    for (int i=1;i<N>>1;i++)
    {
      Serial.print((i-1)*dF); Serial.print("\t");
      
      A=sqrt(RealFFT[0][i]*RealFFT[0][i]+ImagFFT[0][i]*ImagFFT[0][i])/(float)N; 
      Serial.print(A/Coefs[0]); Serial.print("\t");
      
      A=sqrt(RealFFT[1][i]*RealFFT[1][i]+ImagFFT[1][i]*ImagFFT[1][i])/(float)N; 
      Serial.print(A/Coefs[1]); Serial.print("\t");

      A=sqrt(RealFFT[2][i]*RealFFT[2][i]+ImagFFT[2][i]*ImagFFT[2][i])/(float)N; 
      Serial.print(A/Coefs[2]); Serial.print("\t");

      A=sqrt(RealFFT[3][i]*RealFFT[3][i]+ImagFFT[3][i]*ImagFFT[3][i])/(float)N; 
      Serial.print(A/Coefs[3]); Serial.print("\t");

      A=sqrt(RealFFT[4][i]*RealFFT[4][i]+ImagFFT[4][i]*ImagFFT[4][i])/(float)N; 
      Serial.print(A/Coefs[4]); Serial.print("\t");
      
      A=sqrt(RealFFT[5][i]*RealFFT[5][i]+ImagFFT[5][i]*ImagFFT[5][i])/(float)N; 
      Serial.println(A/Coefs[5]);
    }
    Serial.println("");
    state=!state;
  }
  */
}


float getData(float *dataout,int nn, unsigned long ts_us, int analogpin, float gain, int noDC)
{
  // Lecture des données brutes  
  float ts_f=0.0;
  unsigned long t11; 
  for(int i=0; i<nn; i++)
  {
    t11=micros();
    dataout[i]=(float)analogRead(54+analogpin); 
    delayMicroseconds(ts_us);
    ts_f+=(float)micros()-(float)t11; 
  }
  ts_f=ts_f/(float)nn; 
  
  // Conversion en V
  for(int i=0; i<nn; i++)
    dataout[i]=gain*dataout[i]*3.3/4095.0;
  
  // Suppression de la valeur DC 
  if(noDC==0) 
  {
    float Vm=0.0; 
    for(int i=0; i<nn;i++) Vm+=dataout[i]; 
    Vm/=(float)nn; 
    for(int i=0; i<nn;i++) dataout[i]=dataout[i]-Vm; 
  } 
  return 1E6/ts_f;
}

float setWindow(float *datain, float *dataout, int n, int type)
{
  float Hi, t=0.0,t0=0.0, sig=0.0; 
  const float gain[6]={0.5581,0.5934,0.4765,0.6456,0.5581,1.0000};
    
  switch(type)
  {
    //Fenêtre de Hann
    case 0:
      for(int i=0;i<n; i++)
      {
        t=(float)i/float(n);  
        Hi=0.5-0.5*cos(2.0*PI*t); 
        dataout[i]=Hi*datain[i];
      }
      return gain[type];

    //Fenêtre de Hamming
    case 1:
      for(int i=0;i<n; i++)
      {
        t=(float)i/float(n);  
        Hi=0.54-0.46*cos(2.0*PI*t); 
        dataout[i]=Hi*datain[i];
      }
      return gain[type];

    // Fenêtre de Blackman
    case 2:
      for(int i=0;i<n; i++)
      {
        t=(float)i/float(n);  
        Hi=0.42-0.5*cos(2.0*PI*t)+0.08*cos(4.0*PI*t); 
        dataout[i]=Hi*datain[i];
      }
      return gain[type];
      
    //Fenêtre de Gauss
    case 3:
      for(int i=0;i<n; i++)
      {
        t=((float)i/float(n))-0.5;  
        t0=0.5;
        sig=t0/2.0;  
        Hi=exp(-0.5*t*t/(sig*sig)); 
        dataout[i]=Hi*datain[i];
      }
      return gain[type];

    //Fenêtre sin²
    case 4:
      for(int i=0;i<n; i++)
      {
        t=(float)i/float(n);  
        Hi=sin(2.0*0.5*PI*t)*sin(2.0*0.5*PI*t); // sin(x)²=(1-cos(2x))/2
        dataout[i]=Hi*datain[i];
      }
      return gain[type];

    default:
      for(int i=0;i<n; i++)
      {
        dataout[i]=datain[i];
      } 
      return gain[5];  
  }
}

float DFT(float *data, int Ndata, float *real, float *imag, float fs)
{
  int nfft=floor((float)Ndata/2.0); 
  float df=(fs/2.0)/(float)nfft; 
  float somme_r=0.0, somme_i=0.0;  
  float wt=0.0; 
  
  for (int i=0; i<nfft; i++)
  {
      for (int j=0; j<Ndata; j++)
      {
        wt=-2.0*PI*(float)i*(float)j/Ndata;
        somme_r+=data[j]*cos(wt); 
        somme_i+=data[j]*sin(wt); 
      }      
      real[i]= somme_r; somme_r=0.0;
      imag[i]= somme_i; somme_i=0.0;
  }
  return df; 
}

clear all; close all; clc;


%% Analyse Temporelle

N=2^13;
f0=1;t0=1/f0;
t=linspace(0,(10+1/3)*t0,N);
ts=t(2)-t(1); fs=1/ts;
s_t=5*sin(2*pi*f0*t)+sin(2*pi*2*f0*t)+sin(2*pi*10*f0*t);

% Fenêtres de Hann
Hh(1,:)=hann(N);      % Fenêtres de Hann
Hh(2,:)=blackman(N);  % Fenêtre de Hamming
Hh(3,:)=hamming(N);   % Fenêtre de Blackman
Hh(4,:)=gaussmf(t,[t(end)/5 t(end)/2]);         % Fenêtre Gaussienne
ti=linspace(0,1,N);Hh(5,:)=sin(2*pi*0.5*ti).^2; % Fenêtre sin² 

figure(1)
plot(t,Hh','lineWidth',2);xlim([t(1) t(end)]);grid on;
legend({'hann','blackman','hamming','Gauss','Sin²'},'fontsize',13);
% return;



%% Analyse Fréquentielle

% Fenêtrage
for i=1:5
    s_tf(i,:)=s_t.*Hh(i,:);
end;
s_tf(6,:)=s_t;


% FFT
nfft=N;
for i=1:6
    s_fm(i,:)=fft(s_tf(i,:), nfft);
end;
df=fs/N;
f=linspace(-fs/2-df,fs/2,nfft);

% Affichage
figure(2);
subplot(211);
plot(t,s_tf,'lineWidth',2); grid on; xlim([t(1) t(end)]);
legend({'hann','blackman','hamming','Gauss','Sin²','Sans Fenêtre'},'fontsize',13);

subplot(212);
plot(f,20*log10(fftshift(abs(s_fm)/(N))),'lineWidth',2); grid on;xlim([-20*f0 20*f0]);
legend({'hann','blackman','hamming','Gauss','Sin²','Sans Fenêtre'},'fontsize',13);

void setup()
{
  // ADC 
  analogReadResolution(12);       // Ajustable <= 12 (4096 Niveaux) 

  // Affichage 
  Serial.begin(250000); 
}

void loop()
{
  // Lecture 
  float V1=(float)analogRead(A0); 
  V1=(V1*3.3/4095.0)*1000.0; 
  float V2=3000.0*(V1<1000.0); 
  
  // Affichage 
  Serial.print(V1); Serial.print("\t");
  Serial.println(V2);
  delay(25); 
}

#define squareOut   8             // Sortie Carrée
#define F0          984.0         // Valeur approchée de la fréquence  


void setup()
{
  // ADC 
  analogReadResolution(12);       // Ajustable <= 12 (4096 Niveaux) 

  // Output
  analogWrite(squareOut,128);     // 0-255

  // Input 
  pinMode(squareOut+1, INPUT);
  
  // Affichage 
  Serial.begin(250000); 
}

void loop()
{
  //Serial.println(analogRead(A0)); delay(100); return; // TEST!!

  
  // 1. Mesure via la fonction pulseIn() 
  /*
  unsigned long T_l=pulseIn(squareOut+1, LOW);
  unsigned long T_h=pulseIn(squareOut+1, HIGH);
  Serial.print(1.0/((float)(T_l)*2.0*1E-6));Serial.print("\t"); 
  Serial.print(1.0/((float)(T_h)*2.0*1E-6));Serial.print("\t"); 
  //Serial.println(1.0/((float)(T_l+T_h)*1E-6));return; 
  Serial.print(1.0/((float)(T_l+T_h)*1E-6));Serial.print("\t"); 
  */
  
  // 2. Mesure via la fonction getFreqHz()
  int nn=1024;   // Filtrage de la fréquence 
  float F=0.0;    // Fréquence en Hz 
  int s_max=1000; // Seuil maximal
  int s_min=100;  // Seuil minimal 
  int Pin=0;      // Pin A0, (1)A1, (2)A2, etc. 
  
  // Mesure de la fréquence (Pins Analogiques!!) 
  for(int i=0; i<nn; i++)F+=getFreqHz(Pin, s_max,s_min); 
  F=F/(float)nn; 
  
  // Affichage 
  Serial.println(F*60.0);
}


float getFreqHz(int Ai, int s_max, int s_min)
{
  static long T_2=0, T_1=0;
  static int  ai_1=0, ai_2=0;
  static float freq=0.0; 

  ai_1=analogRead(54+Ai);
  if((ai_2-ai_1)>s_max)
  {
    T_2=micros()-T_1; 
    freq=1.0/((float)T_2*1E-6); 
    T_1=micros();
  }
  ai_2=ai_1;
  return freq; 
}

pulseIn(pin, value)

pulseIn(pin, value, timeout)

  unsigned long T_l=pulseIn(squareOut+1, LOW);
  unsigned long T_h=pulseIn(squareOut+1, HIGH);
  Serial.print(1.0/((float)(T_l)*2.0*1E-6));Serial.print("\t"); 
  Serial.print(1.0/((float)(T_h)*2.0*1E-6));Serial.print("\t"); 
  //Serial.println(1.0/((float)(T_l+T_h)*1E-6));return; 
  Serial.print(1.0/((float)(T_l+T_h)*1E-6));Serial.print("\t"); 

float getFreqHz(int Ai, int s_max, int s_min)
{
  static long T_2=0, T_1=0;
  static int  ai_1=0, ai_2=0;
  static float freq=0.0; 

  ai_1=analogRead(54+Ai);
  if((ai_2-ai_1)>s_max)
  {
    T_2=micros()-T_1; 
    freq=1.0/((float)T_2*1E-6); 
    T_1=micros();
  }
  ai_2=ai_1;
  return freq; 
}

#define squareOut   8             // Sortie Carrée
#define F0          984.0         // Valeur approchée de la fréquence  


void setup()
{
  // ADC 
  analogReadResolution(12);       // Ajustable <= 12 (4096 Niveaux) 

  // Output
  analogWrite(squareOut,128);     // 0-255

  // Input 
  pinMode(squareOut+1, INPUT);
  
  // Affichage 
  Serial.begin(250000); 
}

void loop()
{
  //Serial.println(analogRead(A0)); delay(100); return; // TEST!!

  
  // 1. Mesure via la fonction pulseIn() 
  /*
  unsigned long T_l=pulseIn(squareOut+1, LOW);
  unsigned long T_h=pulseIn(squareOut+1, HIGH);
  Serial.print(1.0/((float)(T_l)*2.0*1E-6));Serial.print("\t"); 
  Serial.print(1.0/((float)(T_h)*2.0*1E-6));Serial.print("\t"); 
  //Serial.println(1.0/((float)(T_l+T_h)*1E-6));return; 
  Serial.print(1.0/((float)(T_l+T_h)*1E-6));Serial.print("\t"); 
  */
  
  // 2. Mesure via la fonction getFreqHz()
  int nn=1024;   // Filtrage de la fréquence 
  float F=0.0;    // Fréquence en Hz 
  int s_max=1000; // Seuil maximal
  int s_min=100;  // Seuil minimal 
  int Pin=0;      // Pin A0, (1)A1, (2)A2, etc. 
  
  // Mesure de la fréquence (Pins Analogiques!!) 
  for(int i=0; i<nn; i++)F+=getFreqHz(Pin, s_max,s_min); 
  F=F/(float)nn; 
  
  // Affichage 
  Serial.println(F*60.0);
}


float getFreqHz(int Ai, int s_max, int s_min)
{
  static long T_2=0, T_1=0;
  static int  ai_1=0, ai_2=0;
  static float freq=0.0; 

  ai_1=analogRead(54+Ai);
  if((ai_2-ai_1)>s_max)
  {
    T_2=micros()-T_1; 
    freq=1.0/((float)T_2*1E-6); 
    T_1=micros();
  }
  ai_2=ai_1;
  return freq; 
}

#define squareOut   8       // Sortie Carrée

#define N           128     // Données N=2^n, TFD Nf=2^(n-1)
                            // Agit sur la Résolution fréquentielle!!!!!!!! 
                            // df= Fs/N
                             
#define Ts_us       500     // Ts=1/Fs: Approchée (voir le programme) 
                            // Agit sur la fréquence MAX du spéctre!!!!!!!! 
                            // FMXA = Fs/2

bool State=false; 
unsigned long T1=0, T2=0; 
float Ts=0.0, Fs=0.0; 
float Data[N], Nfft=N>>1; 
float dF=0.0; 
float RealFFT[N>>1], ImagFFT[N>>1], ModFFT[N>>1];
float MaxIdMax[2]={0.0,0.0}; 
float Am, ReIm[2]={0.0,0.0};
float sMoy=0.0; 
int n=0; 



const double q=1.0/4096.0; 


void setup()
{
  // Square Out - PWM (Due) - Non utilisée 
  /* 
  pinMode(squareOut, OUTPUT); 
  analogWriteResolution(12);     // Ajustable <= 12 (4096 Niveaux) 
  analogWrite(squareOut, 2048);  // Pins 2-13, 1000 Hz !!
  */
  
  // ADC 
  analogReadResolution(12);       // Ajustable <= 12 (4096 Niveaux) 

  
  // Affichage 
  Serial.begin(250000); 
}

void loop()
{ 
  // 1. Lecture des données
  float A0_i=0.0,A1_i=0.0;
  Ts=(float)Ts_us;
  for(int i=0; i<N; i++)
  {
    T1=micros();
    A0_i=(float)analogRead(A0)+(0.5*q*rand10()); 
    A1_i=(float)analogRead(A1)+(0.5*q*rand10()); 
    Data[i]=(A0_i-A1_i);
    Data[i]*=3.3/4096.0; 
    delayMicroseconds(Ts_us);
    Ts+=(float)micros()-(float)T1; 
    Ts=Ts/2.0;
    //Serial.println(1000.0*Data[i]); 
  }
  Fs=1E6/Ts; 
  //Serial.println(Fs); return; 

  // 2. Calcul du spectre 
  dF=DFT(Data, N, RealFFT, ImagFFT, Fs); 

  // Calcul du Module de la DFT
  getModul(RealFFT, ImagFFT, ModFFT, N>>1); 

  // Extraction de la valeur Max
  ModFFT[0]=0.0;getMax(ModFFT, N>>1, MaxIdMax);

  // Affichage (f0) 
  //Serial.print(MaxIdMax[0]); Serial.print(" \t");
  //Serial.print(MaxIdMax[1]); Serial.print(" \t");
  Serial.println(MaxIdMax[1]*dF);
  return; 

  // 3. Visualisation du spectre  
  static bool state=false;
  if(state==false)
  {
    Serial.println("");
    for (int i=0;i<(N>>1)-1;i++)
    {
      if (i!=0) 
      {
        Serial.print(dF*(float)i); Serial.print("\t");
        Serial.print(2.0*1000.0*ModFFT[i]/(float)N); Serial.print("\t");
        Serial.println(1000*Data[2*i]); 
      }
      else 
      {
        Serial.print(dF*(float)i); Serial.print("\t");
        Serial.print(1000.0*ModFFT[i]/(float)N); Serial.print("\t"); 
        Serial.println(1000*Data[2*i]);
      }
    }
    Serial.println("");
    state=!state;
  }
}

float DFT(float *data, int Ndata, float *real, float *imag, float fs)
{
  int nfft=floor((float)Ndata/2.0); 
  float df=(fs/2.0)/(float)nfft; 
  float somme_r=0.0, somme_i=0.0;  
  float wt=0.0; 
  
  for (int i=0; i<nfft; i++)
  {
      for (int j=0; j<Ndata; j++)
      {
        wt=-2.0*PI*(float)i*(float)j/Ndata;
        somme_r+=data[j]*cos(wt); 
        somme_i+=data[j]*sin(wt); 
      }      
      real[i]= somme_r; somme_r=0.0;
      imag[i]= somme_i; somme_i=0.0;
  }
  return df; 
}

float DFTn(float *data, int Ndata, float *realImag, int n, float fs)
{
  int nfft=floor((float)Ndata/2.0); 
  float df=(fs/2.0)/(float)nfft; 
  float somme_r=0.0, somme_i=0.0;  
  float wt=0.0; 

  if (n>=nfft) return 0.0; 
  
  int i=n;
  for (int j=0; j<Ndata; j++)
  {
    wt=-2.0*PI*(float)i*(float)j/(float)Ndata;
    somme_r+=data[j]*cos(wt); 
    somme_i+=data[j]*sin(wt); 
  }      
  realImag[0]= somme_r;
  realImag[1]= somme_i; 
  return df; 
}

 void getMax(float *datain, int n, float *maxid)
 {
    float maxe=datain[0];
    float id=0; 

    for (int i=1;i<n; i++)
    {
      if (datain[i]>=maxe)
      {
        maxe=datain[i]; 
        id=i; 
      }
    }
    maxid[0]=maxe; 
    maxid[1]=id; 
 }

void getModul(float *real, float *imag, float *modul, int n)
{
  for (int i=0;i<n; i++)
    modul[i]=sqrt((imag[i]*imag[i]) + (real[i]*real[i])); 
}

double rand10(void)
{
  double rn1=2.0*((random(1e10)/1e10)-0.5); 
  double rn2=2.0*((random(1e10)/1e10)-0.5); 
  double rn=tanh((rn1-rn2)); 

  return rn; 
}

 

...
  float f0=10.0, t_i=0.0, ts=0.0;
  float A0=1.0, DC=1.0; 
  for(int i=0; i<N; i++)
  {
    t_i=(float)i/(float)N; 
    Data[i]=DC + A0*sin(2.0*PI*f0*t_i); // + A0*sin(2.0*PI*0.5*f0*t_i)+ A0*sin(2.0*PI*(1.0/3.0)*f0*t_i); // DC, f0, f0/2
    ts=(float)1.0/(float)N; 
  }
  Fs=1/ts; 
...

...
  Ts=(float)Ts_us;
  for(int i=0; i<N; i++)
  {
    T1=micros();
    Data[i]=(float)analogRead(A0);
    Data[i]*=5.0/1023.0; 
    delayMicroseconds(Ts_us);
    Ts+=(float)(micros()-T1); 
    Ts=Ts/2.0;
  }
  Fs=1E6/Ts; 
  Serial.println(Fs);return; 
...

#define Sortie  8       // Non utilisée 
#define N       128     // Données N=2^n, TFD Nf=2^(n-1)
#define Ts_us   1000    // Ts=1/Fs: Approchée (voir le programme) 

/*
 * Objectifs:               
 *  1. Savoir implémenter la DFT
 *  2. Test de la DFT avec un Sinus 
 *  3. Test de la DFT avec ADC (Données réelles) 
 *  4. Analyse de la vitesse Mega/Due 
 */

bool State=false; 
unsigned long T1=0, T2=0; 
float Ts=0.0, Fs=0.0; 
float Data[N], Nfft=N>>1; 
float dF=0.0; 
float RealFFT[N>>1], ImagFFT[N>>1]; 
float sMoy=0.0; 
float Am, ReIm[2];
int n=0; 

void setup()
{
  // Affichage 
  Serial.begin(115200); 
}

void loop()
{ 
  // 1. Lecture des données/ Test de la DFT 
  /*
  float f0=10.0, t_i=0.0, ts=0.0;
  float A0=1.0, DC=1.0; 
  for(int i=0; i<N; i++)
  {
    t_i=(float)i/(float)N; 
    Data[i]=DC + A0*sin(2.0*PI*f0*t_i); // + A0*sin(2.0*PI*0.5*f0*t_i)+ A0*sin(2.0*PI*(1.0/3.0)*f0*t_i); // DC, f0, f0/2
    ts=(float)1.0/(float)N; 
  }
  Fs=1/ts; 
  */
  
  // 1. Lecture des données 
  Ts=(float)Ts_us;
  for(int i=0; i<N; i++)
  {
    T1=micros();
    Data[i]=(float)analogRead(A0);
    Data[i]*=5.0/1023.0; 
    delayMicroseconds(Ts_us);
    Ts+=(float)(micros()-T1); 
    Ts=Ts/2.0;
  }
  Fs=1E6/Ts; 
  //Serial.println(Fs);return; 
  

  // 2. Calcul de la FFT
  T1=micros();
  dF=DFT(Data, N, RealFFT, ImagFFT, Fs);
  T2=micros()-T1;
  Serial.print("Temps FFT(s): ");Serial.println((float)T2/1E6); 
  //Serial.print("Résolution Fréq(Hz): ");Serial.println(dF);
  //return; 

  // 3. Afffichage 
  static bool state=false;
  if(state==false)
  {
    for(int i=0; i<Nfft; i++)
    {
      if (i<Nfft-1) 
      {
        float Amp=sqrt((RealFFT[i]*RealFFT[i])+ (ImagFFT[i]*ImagFFT[i]));
        
        Serial.print((float)i*dF);Serial.print(" \t");
        Serial.print((int)floor(1000.0*Amp/((float)N)));Serial.print(" \t"); //||DFT||/N(mV)
        Serial.print(1000.0*Data[2*i+1]);Serial.print(" \t");
        Serial.println(""); 
      }
    }
    state=!state;
  }
  
}


float DFT(float *data, int Ndata, float *real, float *imag, float fs)
{
  int nfft=floor((float)Ndata/2.0); 
  float df=(fs/2.0)/(float)nfft; 
  float somme_r=0.0, somme_i=0.0;  
  float wt=0.0; 
  
  for (int i=0; i<nfft; i++)
  {
      for (int j=0; j<Ndata; j++)
      {
        wt=-2.0*PI*(float)i*(float)j/Ndata;
        somme_r+=data[j]*cos(wt); 
        somme_i+=data[j]*sin(wt); 
      }      
      real[i]= somme_r; somme_r=0.0;
      imag[i]= somme_i; somme_i=0.0;
  }
  return df; 
}

%% Signal temporel 
N=2^12;
f0=1;t0=1/f0;
t=linspace(0,5*t0,N);
ts=t(2)-t(1); fs=1/ts; 

s_0=1;                      % DC
s_1=2*sin(2*pi*f0*t);       % f0
s_2=4*sin(2*pi*3*f0*t);     % 3f0
s_3=6*sin(2*pi*7*f0*t);     % 7f0
s_b=0*randn(1,N);           % Bruit/Normal 
s_t=s_0+s_1+s_2+s_3+s_b;    % Signal  

% Affichage 
figure(1);
subplot(211); 
plot(t,s_1,'r-o','LineWidth',2); hold on; grid on; 
plot(t,s_2,'m-o','LineWidth',2);
plot(t,s_3,'k-o','LineWidth',2);
xlabel('Temps(s)','fontsize',13);
xlim([0 2*t0]);
ylabel('Amplitude(V)','fontsize',13);
legend({'s_1(t), f_0','s_2(t), 3f_0','s_3(t), 7f_0'},'fontsize',13); 
set(gca,'color','none'); 

subplot(212); 
plot(t,s_t,'b','LineWidth',3); hold on; grid on; 
xlabel('Temps(s)','fontsize',13);
xlim([0 2*t0]);
ylabel('Amplitude(V)','fontsize',13);
legend({'s(t)'},'fontsize',13); 
set(gca,'color','none'); 

%% Init
clear all;
close all; 
clc;
 

%% Signal temporel 
N=2^12;
f0=1;t0=1/f0;
t=linspace(0,5*t0,N);
ts=t(2)-t(1); fs=1/ts; 

s_0=1;                      % DC
s_1=2*sin(2*pi*f0*t);       % f0
s_2=4*sin(2*pi*3*f0*t);     % 3f0
s_3=6*sin(2*pi*7*f0*t);     % 7f0
s_b=0*randn(1,N);           % Bruit/Normal 
s_t=s_0+s_1+s_2+s_3+s_b;    % Signal  

% Affichage 
figure(1);
subplot(211); 
plot(t,s_1,'r-o','LineWidth',2); hold on; grid on; 
plot(t,s_2,'m-o','LineWidth',2);
plot(t,s_3,'k-o','LineWidth',2);
xlabel('Temps(s)','fontsize',13);
xlim([0 2*t0]);
ylabel('Amplitude(V)','fontsize',13);
legend({'s_1(t), f_0','s_2(t), 3f_0','s_3(t), 7f_0'},'fontsize',13); 
set(gca,'color','none'); 

subplot(212); 
plot(t,s_t,'b','LineWidth',3); hold on; grid on; 
xlabel('Temps(s)','fontsize',13);
xlim([0 2*t0]);
ylabel('Amplitude(V)','fontsize',13);
legend({'s(t)'},'fontsize',13); 
set(gca,'color','none'); 



%% Spectre fréquentiel  

% Calcul de la DFT (Manuelle) 
nfft=floor(N); 
df=fs/nfft;
f=linspace(-fs/2-df,fs/2,nfft);% 
s_f=zeros(1,nfft); 


for k=0:nfft-1
    somme=0.0; 
    for j=0:N-1
        somme=somme+(s_t(j+1)*exp(-1i*2*pi*k*j/N)); 
    end
    s_f(k+1)= somme;
end
 

% Calcul de la DFT (Matlab) 
s_fm=fft(s_t,nfft);  

% Affichage 
figure(2); 
subplot(211); 
plot(f,fftshift(abs(s_f)),'r-o','LineWidth',3); hold on; grid on; 
plot(f,fftshift(abs(s_fm)),'b-','LineWidth',3);
xlabel('Fréquence(Hz)','fontsize',13);
% xlim([f(1) f(end)]);
xlim([-10*f0, 10*f0]);
ylabel('||FFT||(V)','fontsize',13);
legend({'FFT Manuelle','FFT Matlab'},'fontsize',13); 
set(gca,'color','none'); 

subplot(212); 
plot(f,fftshift((abs(s_f))/(N)),'r-o','LineWidth',3); hold on; grid on; 
plot(f,fftshift((abs(s_fm))/(N)),'b-','LineWidth',3);
xlabel('Fréquence(Hz)','fontsize',13);
% xlim([f(1) f(end)]);
xlim([-10*f0, 10*f0]);
ylabel('||FFT||/(N/2)(V)','fontsize',13);
legend({'FFT Manuelle','FFT Matlab'},'fontsize',13); 
set(gca,'color','none'); 

int getCmd(unsigned long *cmdOut, int n_0, int numphase) 
{
  int n_out=0; 
  int i,j; 
  unsigned long SyqT1=0x00000000, SyqT2=0x00000000, Syq0=0x00000000; //32-bits 

  if ((numphase<=1)  || (numphase>15)) return; 
  for (i=0;i<n_0; i++) cmdOut[i]=0; 

  /***************************************************/
  /*********** Monophasé - Cas particulier ***********/  
  /***************************************************/
  if (numphase==2)
  {
    cmdOut[0]=0x00000001; //Serial.println(cmd[0], BIN); //1:B01 
    cmdOut[1]=0x00000002; //Serial.println(cmd[1], BIN); //2:B10 
    n_out=2;
    return n_out;  
  }

  /***************************************************/
  /********************* Polyphasé *******************/
  /***************************************************/
  // A. Génération de la séquence principale 
  n_out=2*numphase; 
  SyqT1=0x00000000; Syq0=0x00000001;
  for(j=0;j<numphase; j++)
  {
    SyqT1|=Syq0;
    Syq0= Syq0<<1;
  } 
  SyqT2=SyqT1<<(numphase); 

  // B. Génération des séquences binaires 
  unsigned long T1i_B0, T2i_B0;
  unsigned long T1i[numphase], T2i[numphase], T12i[n_out];
  for(int j=0;j<numphase; j++)
  {
    T1i[j]=0; T2i[j]=0;
  }
  T1i[0]=SyqT1;T2i[0]=SyqT2; 
  //Serial.print(T1i[0], BIN); Serial.print(" , "); Serial.println(T2i[0], BIN);

  for(j=1;j<numphase; j++)
  {
    // 1. Extraction du bit b(0)
    T1i_B0=(0x00000001&SyqT1);
    T2i_B0=(0x00000001&SyqT2);

    // 2. Décalage à droite 
    T1i[j]=SyqT1>>1; 
    T2i[j]=SyqT2>>1; 

    // 3. b(n-1) =b(0) 
    T1i[j]|=T1i_B0<<(n_out-1); 
    T2i[j]|=T2i_B0<<(n_out-1);

    // 4. Mise à jour des séqs Init 
    SyqT1=T1i[j];
    SyqT2=T2i[j];
  }

  // Combinaision des tableaux 
  int ii=0, jj=0; 
  for(j=0;j<n_out; j++)
  {
    if(j%2==0) 
    {
      T12i[j]=T1i[ii];
      ii++;
      //Serial.print(j);Serial.print(" , ");Serial.println(T12i[j], BIN);
    }
    if(j%2==1) 
    {
      T12i[j]=T2i[jj];
      jj++;
      //Serial.print(j);Serial.print(" , ");Serial.println(T12i[j], BIN);
    }
  }
  
  // C. Génération des séquences finales 
  Serial.println("****************************"); 
  Serial.println("T1|T2|T3|T4|T5|T6|T7|T8|T9|... ");  
  Serial.println("****************************"); 
  unsigned long cmd_i=0;
  for(i=0;i<n_out; i++)
  {
    cmd_i=0;
    for(j=0;j<n_out; j++)cmd_i|=(((T12i[j]>>i)&0x1)<<(j));
    cmdOut[i]=cmd_i;
    for (int k=0;k<32;k++)Serial.print(((cmd_i>>k)&0x1));
    Serial.println("");
  }
  Serial.println("****************************"); 
  
  return n_out;
}

void setCmd(unsigned long *cmdOut, int n_0) 
{
  static int K=0; 

  // Commenté les lignes non utilisées 
  PORTA=cmdOut[K]      &0x000000ff;   // (0) -(1*8-1) --LSB
  //PORTB=(cmdOut[K]>>8) &0x000000ff;   // (8) -(2*8-1)   
  //PORTC=(cmdOut[K]>>16)&0x000000ff;   // (16)-(3*8-1) 
  //PORTD=(cmdOut[K]>>24)&0x000000ff;   // (24)-(4*8-1) --MSB 

  // Affichage du contenu des ports sur 8-bits (A commenté) 
  /*
  for (int k=0;k<8;k++) Serial.print(((PORTA>>k)&0x1));Serial.print(",");
  for (int k=0;k<8;k++) Serial.print(((PORTB>>k)&0x1));Serial.print(",");
  for (int k=0;k<8;k++) Serial.print(((PORTC>>k)&0x1));Serial.print(",");
  for (int k=0;k<8;k++) Serial.print(((PORTD>>k)&0x1));Serial.print(",");
  Serial.println("");
  */
   
  K++;
  if (K==n_0) K=0; 
}

/*
 * Algorithme de la commande Symétrique Générique 
 * La commande peut etre codée sur 32 bits (15 Phases Maximales)
 * via 4 ports sur 8-bits (µC 8-bits) 
 * (ou bien un bus sur 32-bits (µC 32-bits))
 * 
 * PORTA: de (0)  à (1*8-1)     --LSB 
 * PORTB: de (8)  à (2*8-1)   
 * PORTC: de (16) à (3*8-1) 
 * PORTD: de (24) à (4*8-1)     --MSB 
 * 
*/

#define T0_us   333           // Période de l'onduleur (Symétrique) : T~n*T0 (µs)  
#define Nphs    15             // Nb de phases [2,15]




int             n;            // Nombre de séquences/ Période  
const int       nCmd=35;      // !!! Ne pas toucher!  
unsigned long   CmdTab[nCmd]; // !!! Ne pas toucher! 



void setup() 
{
  // Pinout 
  DDRA =0xff; PORTA=0x00;
  //DDRB =0xff; PORTA=0x00;
  //DDRC =0xff; PORTA=0x00;
  //DDRD =0xff; PORTA=0x00;

  // Afficahge 
  Serial.begin(115200);

  // Synthèse de la commande getCmd()
  n= getCmd(CmdTab, nCmd, Nphs); 
  Serial.print("Nseq=");Serial.println(n);
}






void loop()
{
  // Génération de la commande setCmd()
  setCmd(CmdTab, n);
  //delayMicroseconds(T0_us); 
}


int getCmd(unsigned long *cmdOut, int n_0, int numphase) 
{
  int n_out=0; 
  int i,j; 
  unsigned long SyqT1=0x00000000, SyqT2=0x00000000, Syq0=0x00000000; //32-bits 

  if ((numphase<=1)  || (numphase>15)) return; 
  for (i=0;i<n_0; i++) cmdOut[i]=0; 

  /***************************************************/
  /*********** Monophasé - Cas particulier ***********/  
  /***************************************************/
  if (numphase==2)
  {
    cmdOut[0]=0x00000001; //Serial.println(cmd[0], BIN); //1:B01 
    cmdOut[1]=0x00000002; //Serial.println(cmd[1], BIN); //2:B10 
    n_out=2;
    return n_out;  
  }

  /***************************************************/
  /********************* Polyphasé *******************/
  /***************************************************/
  // A. Génération de la séquence principale 
  n_out=2*numphase; 
  SyqT1=0x00000000; Syq0=0x00000001;
  for(j=0;j<numphase; j++)
  {
    SyqT1|=Syq0;
    Syq0= Syq0<<1;
  } 
  SyqT2=SyqT1<<(numphase); 

  // B. Génération des séquences binaires 
  unsigned long T1i_B0, T2i_B0;
  unsigned long T1i[numphase], T2i[numphase], T12i[n_out];
  for(int j=0;j<numphase; j++)
  {
    T1i[j]=0; T2i[j]=0;
  }
  T1i[0]=SyqT1;T2i[0]=SyqT2; 
  //Serial.print(T1i[0], BIN); Serial.print(" , "); Serial.println(T2i[0], BIN);

  for(j=1;j<numphase; j++)
  {
    // 1. Extraction du bit b(0)
    T1i_B0=(0x00000001&SyqT1);
    T2i_B0=(0x00000001&SyqT2);

    // 2. Décalage à droite 
    T1i[j]=SyqT1>>1; 
    T2i[j]=SyqT2>>1; 

    // 3. b(n-1) =b(0) 
    T1i[j]|=T1i_B0<<(n_out-1); 
    T2i[j]|=T2i_B0<<(n_out-1);

    // 4. Mise à jour des séqs Init 
    SyqT1=T1i[j];
    SyqT2=T2i[j];
  }

  // Combinaision des tableaux 
  int ii=0, jj=0; 
  for(j=0;j<n_out; j++)
  {
    if(j%2==0) 
    {
      T12i[j]=T1i[ii];
      ii++;
      //Serial.print(j);Serial.print(" , ");Serial.println(T12i[j], BIN);
    }
    if(j%2==1) 
    {
      T12i[j]=T2i[jj];
      jj++;
      //Serial.print(j);Serial.print(" , ");Serial.println(T12i[j], BIN);
    }
  }
  
  // C. Génération des séquences finales 
  Serial.println("****************************"); 
  Serial.println("T1|T2|T3|T4|T5|T6|T7|T8|T9|... ");  
  Serial.println("****************************"); 
  unsigned long cmd_i=0;
  for(i=0;i<n_out; i++)
  {
    cmd_i=0;
    for(j=0;j<n_out; j++)cmd_i|=(((T12i[j]>>i)&0x1)<<(j));
    cmdOut[i]=cmd_i;
    for (int k=0;k<32;k++)Serial.print(((cmd_i>>k)&0x1));
    Serial.println("");
  }
  Serial.println("****************************"); 
  
  return n_out;
}

void setCmd(unsigned long *cmdOut, int n_0) 
{
  static int K=0; 

  // Commenté les lignes non utilisées 
  PORTA=cmdOut[K]      &0x000000ff;   // (0) -(1*8-1) --LSB
  //PORTB=(cmdOut[K]>>8) &0x000000ff;   // (8) -(2*8-1)   
  //PORTC=(cmdOut[K]>>16)&0x000000ff;   // (16)-(3*8-1) 
  //PORTD=(cmdOut[K]>>24)&0x000000ff;   // (24)-(4*8-1) --MSB 

  // Affichage du contenu des ports sur 8-bits (A commenté) 
  /*
  for (int k=0;k<8;k++) Serial.print(((PORTA>>k)&0x1));Serial.print(",");
  for (int k=0;k<8;k++) Serial.print(((PORTB>>k)&0x1));Serial.print(",");
  for (int k=0;k<8;k++) Serial.print(((PORTC>>k)&0x1));Serial.print(",");
  for (int k=0;k<8;k++) Serial.print(((PORTD>>k)&0x1));Serial.print(",");
  Serial.println("");
  */
   
  K++;
  if (K==n_0) K=0; 
}

unsigned char State=0x00; 

void setup() 
{
  DDRA =0xff;PORTA=State; 
}

void loop() 
{
  State=0xff*(!State); 
  PORTA=State; 
  delayMicroseconds(8); 
}

/*
 * Commande Symétrique: SetCMD1() & SetCMD2()
 * Commande Décalée: SetCMD3() & SetCMD4()
 * Commande PWM(MLI): Voir le programme 
*/

#define Tr11            22         // Transistor 1/Bras 1 
#define Tr12            23         // Transistor 2/Bras 1 
#define Tr21            24         // Transistor 1/Bras 2 
#define Tr22            25         // Transistor 2/Bras 2 
#define Tr31            26         // Transistor 1/Bras 3 
#define Tr32            27         // Transistor 2/Bras 3 

#define T0_us           333        // Période de l'onduleur (Symétrique) : T=6*T0 (µs)     
                                   // Période de l'onduleur (Décalée)    : T=12*T0 (µs)
                                   // sPWM: Période d'incrémentation du Timer 
                                   // sPWM: Période globale T=2*N*T0_us => F=1/F: Fréquence de l'Onduleur
/* Params sPWM */   
#define   N           64           // Niveaux de l'onduleur: Nombre d’échantillons (foramt 2^n)                               
#define   TimerStep   13           // Pas d'incrémentation du Timer rapide (>>1) 


int             Trans[6]; // Ou bien PORTA
unsigned long   MedSine[N];
unsigned long   TimerSPWM=0;
unsigned long   i_sin[3]={0,0,0};
bool            sPWM_l[3]={false,false,false};
bool            sPWM_r[3]={true,true,true};
bool            sinePolar[3]={false,false,false};
double          sine_val=0.0, A=0.0;  
int             Nbit=0;   // Amplitude du  signal sinusoïdal 2^8 

void setup() 
{
  // Pinout 
  Trans[0]=Tr11; Trans[1]=Tr12;
  Trans[2]=Tr21; Trans[3]=Tr22;
  Trans[4]=Tr31; Trans[5]=Tr32;
  for (int i=0;i<6; i++) 
  {
    pinMode(Trans[i], OUTPUT);
    digitalWrite(Trans[i], LOW); 
  }
  // Ou bien: Init du port A en sortie 
  //DDRA =0xff; PORTA=0x00;

  // Cogade binaire de l'amplitude 
  Nbit=(int)round((log((float)N)/log(2.0)));

  // Init Timers // Déphasage de 60°/Bras (60°=>T/6) 
  i_sin[0]=0;                       // Bras 1 - 0°
  i_sin[1]=round((float)N/6.0);     // Bras 2 - 60°
  i_sin[2]=round(2.0*(float)N/6.0); // Bras 3 - 120°

  // Génération du signal sinusoïdal: 1/2 Période (2*N échantillons/période)
  A=(double)(pow(2.0,(double)Nbit)-1.0);
  for (int i=0; i<N; i++)
  {
    sine_val=A*sin(PI*(double)i/(double)(N)); 
    MedSine[i]=(int)(abs(floor(sine_val)));  
  } 

  // Afficahge 
  Serial.begin(9600);
  pinMode(3,OUTPUT); 

  // Configuration de l'Interruption (Trigger Externe)  
  
  // Câblage du pin et configuration en entrée: INT0 => PD0 (pin 21 de la carte)
  DDRD =0x00; // Port D en entrée

  // Activation de l'interruption globale (registre SREG) 
  SREG|=0x80;

  // Validation de l'interruption INT0 (Registre EIMSK)
  EIMSK|=0x01; // INT0

  // Choix du mode de détection: Front montant dans INT0
  EICRA|=0x01;
}

void loop(){}


//ISR(_VECTOR(1)) // Ou _VECTOR(2) 
ISR(INT0_vect)
{
  //SetCMD1(Trans);
  //SetCMD2(Trans);
  //SetCMD3(Trans);
  SetCMD2(Trans);




  /*********** sPWM ***********/
  // Incrémentation du Timer: Compteur rapide (signal triangulaire)
  /*
  TimerSPWM+=TimerStep; 
  if (TimerSPWM>N) TimerSPWM=0;

  // Génération de la commande 
  for (int i=0; i<3; i++)
  {
    // 1. Incrémentation du Timer du signal: Compteur long (signal sinusoïdal) 
    i_sin[i]+=1; 
    if (i_sin[i]>N) i_sin[i]=0;

    // 2. Inversion de la polarité de la période 
    if(!i_sin[i]) sinePolar[i]=!sinePolar[i]; 
    
    // 3. Génération des signaux PWM 
    if(sinePolar[i])  // Période (+)
    {
      sPWM_l[i] =MedSine[i_sin[i]] > TimerSPWM; 
      sPWM_r[i] =false; 
    }
    else              // Période (-)
    {
      sPWM_l[i] = false; 
      sPWM_r[i] =MedSine[i_sin[i]] > TimerSPWM; 
    }
  }

  // Envoie des signaux au Port A
  PORTA=(B00000001*sPWM_l[0])+ (B00000010*sPWM_r[0])+ 
        (B00000100*sPWM_l[1])+ (B00001000*sPWM_r[1])+
        (B00010000*sPWM_l[2])+ (B00100000*sPWM_r[2]);

  */
}



void SetCMD1( int *pins)
{
  static int I=0; 
  const bool Cmd[6][6]={{0,1,1,0,0,1}, 
                        {1,0,1,0,0,1}, 
                        {1,0,0,1,0,1},
                        {1,0,0,1,1,0}, 
                        {0,1,0,1,1,0}, 
                        {0,1,1,0,1,0}};
                        
  for (int i=0;i<6; i++) digitalWrite(pins[i], Cmd[I][i]); 
  I++; I%=6; 

  return 0; 
}





void SetCMD2( int *pins)
{
  static unsigned char I=0;  
  const unsigned char Cmd[6]={B00011001, 
                              B00101001, 
                              B00100101,
                              B00100110, 
                              B00010110, 
                              B00011010};

  PORTA=Cmd[I];
  I++; 
  
  //I%=6;             // Fréquence: + 
  //I=I*(I<6);        // Fréquence: ++
  if (I>=6)I=0;       // Fréquence: +++
  
  return 0; 
}



void SetCMD3( int *pins)
{
  static unsigned char I=0;  
  const bool Cmd[12][6]={
                        {0,1,1,0,0,0},
                        {0,0,1,0,0,1}, 
                        {0,0,1,0,0,1}, 
                        {1,0,0,0,0,1}, 
                        {1,0,0,0,0,1},
                        {1,0,0,1,0,0},
                        {1,0,0,1,0,0}, 
                        {0,0,0,1,1,0}, 
                        {0,0,0,1,1,0}, 
                        {0,1,0,0,1,0},
                        {0,1,0,0,1,0},
                        {0,1,1,0,0,0}, 
                        };
                        
  for (int i=0;i<6; i++) digitalWrite(pins[i], Cmd[I][i]); 
  I++; 
  if (I>=12)I=0;

  return 0; 
}


void SetCMD4( int *pins)
{
  static unsigned char I=0;  
  const unsigned char Cmd[12]= {
                               B00011000,
                               B00001001, 
                               B00001001, 
                               B00100001, 
                               B00100001,
                               B00100100,
                               B00100100, 
                               B00000110, 
                               B00000110, 
                               B00010010,
                               B00010010,
                               B00011000
                               };

  PORTA=Cmd[I];
  I++; 
  if (I>=12)I=0;

  return 0; 
}

/*
 * Commande Symétrique : SetCMD1() & SetCMD2()
 * Commande Décalée : SetCMD3() & SetCMD4()
 * Commande PWM(MLI) : Voir le programme 
*/

#define Tr11            22         // Transistor 1/Bras 1 
#define Tr12            23         // Transistor 2/Bras 1 
#define Tr21            24         // Transistor 1/Bras 2 
#define Tr22            25         // Transistor 2/Bras 2 
#define Tr31            26         // Transistor 1/Bras 3 
#define Tr32            27         // Transistor 2/Bras 3 

#define T0_us           333        // Période de l'onduleur (Symétrique) : T=6*T0 (µs)     
                                   // Période de l'onduleur (Décalée)    : T=12*T0 (µs)
                                   // sPWM: Période d'incrémentation du Timer 
                                   // sPWM: Période globale T=2*N*T0_us => F=1/F: Fréquence de l'Onduleur
/* Params sPWM */   
#define   N           64           // Niveaux de l'onduleur : Nombre d’échantillons (format 2^n)                               
#define   TimerStep   13           // Pas d'incrémentation du Timer rapide (>>1) 


int             Trans[6]; // Ou bien PORTA
unsigned long   MedSine[N];
unsigned long   TimerSPWM=0;
unsigned long   i_sin[3]={0,0,0};
bool            sPWM_l[3]={false,false,false};
bool            sPWM_r[3]={true,true,true};
bool            sinePolar[3]={false,false,false};
double          sine_val=0.0, A=0.0;  
int             Nbit=0;   // Amplitude du signal sinusoïdal 2^8 

void setup() 
{
  // Pinout 
  Trans[0]=Tr11; Trans[1]=Tr12;
  Trans[2]=Tr21; Trans[3]=Tr22;
  Trans[4]=Tr31; Trans[5]=Tr32;
  for (int i=0;i<6; i++) 
  {
    pinMode(Trans[i], OUTPUT);
    digitalWrite(Trans[i], LOW); 
  }
  // Ou bien: Init du port A en sortie 
  //DDRA =0xff; PORTA=0x00;

  // Codage binaire de l'amplitude 
  Nbit=(int)round((log((float)N)/log(2.0)));

  // Init Timers // Déphasage de 60°/Bras (60°=>T/6) 
  i_sin[0]=0;                       // Bras 1 - 0°
  i_sin[1]=round((float)N/6.0);     // Bras 2 - 60°
  i_sin[2]=round(2.0*(float)N/6.0); // Bras 3 - 120°

  // Génération du signal sinusoïdal : 1/2 Période (2*N échantillons/période)
  A=(double)(pow(2.0,(double)Nbit)-1.0);
  for (int i=0; i<N; i++)
  {
    sine_val=A*sin(PI*(double)i/(double)(N)); 
    MedSine[i]=(int)(abs(floor(sine_val)));  
  } 

  // Affichage 
  Serial.begin(9600);
  pinMode(3,OUTPUT); 
}

void loop() 
{  
  /*
  SetCMD1(Trans);
  SetCMD2(Trans);
  SetCMD3(Trans);
  SetCMD4(Trans);
  delayMicroseconds(T0_us); 
  return; 
  */

  /*
  delayMicroseconds(11); 
  digitalWrite(3, sinePolar[0]);
  */

  // Prévisualisation des signaux du Bras 1 + sin(t) 
  /*
  Serial.print(MedSine[i_sin[0]]); Serial.print(","); 
  Serial.print(i_sin[0]); Serial.print(","); 
  Serial.print(A*sPWM_l[0]); Serial.print(","); 
  Serial.println(-A*sPWM_r[0]);
  */
  
  // Visualisation des signaux de l'onduleur 
  /*
  Serial.print(sPWM_l[0]+0); Serial.print(","); 
  Serial.print(sPWM_r[0]+1); Serial.print(","); 
  Serial.print(sPWM_l[1]+2); Serial.print(","); 
  Serial.print(sPWM_r[1]+3); Serial.print(","); 
  Serial.print(sPWM_l[2]+4); Serial.print(","); 
  Serial.println(sPWM_r[2]+5); 
  */

  // Incrémentation du Timer: Compteur rapide (signal triangulaire)
  TimerSPWM+=TimerStep; 
  if (TimerSPWM>N) TimerSPWM=0;

  // Génération de la commande 
  for (int i=0; i<3; i++)
  {
    // 1. Incrémentation du Timer du signal : Compteur long (signal sinusoïdal) 
    i_sin[i]+=1; 
    if (i_sin[i]>N) i_sin[i]=0;

    // 2. Inversion de la polarité de la période 
    if(!i_sin[i]) sinePolar[i]=!sinePolar[i]; 
    
    // 3. Génération des signaux PWM 
    if(sinePolar[i])  // Période (+)
    {
      sPWM_l[i] =MedSine[i_sin[i]] > TimerSPWM; 
      sPWM_r[i] =false; 
    }
    else              // Période (-)
    {
      sPWM_l[i] = false; 
      sPWM_r[i] =MedSine[i_sin[i]] > TimerSPWM; 
    }
  }

  // Envoie des signaux au Port A
  PORTA=(B00000001*sPWM_l[0])+ (B00000010*sPWM_r[0])+ 
        (B00000100*sPWM_l[1])+ (B00001000*sPWM_r[1])+
        (B00010000*sPWM_l[2])+ (B00100000*sPWM_r[2]);
}



void SetCMD1( int *pins)
{
  static int I=0; 
  const bool Cmd[6][6]={{0,1,1,0,0,1}, 
                        {1,0,1,0,0,1}, 
                        {1,0,0,1,0,1},
                        {1,0,0,1,1,0}, 
                        {0,1,0,1,1,0}, 
                        {0,1,1,0,1,0}};
                        
  for (int i=0;i<6; i++) digitalWrite(pins[i], Cmd[I][i]); 
  I++; I%=6; 

  return 0; 
}





void SetCMD2( int *pins)
{
  static unsigned char I=0;  
  const unsigned char Cmd[6]={B00011001, 
                              B00101001, 
                              B00100101,
                              B00100110, 
                              B00010110, 
                              B00011010};

  PORTA=Cmd[I];
  I++; 
  
  //I%=6;             // Fréquence: + 
  //I=I*(I<6);        // Fréquence: ++
  if (I>=6)I=0;       // Fréquence: +++
  
  return 0; 
}



void SetCMD3( int *pins)
{
  static unsigned char I=0;  
  const bool Cmd[12][6]={
                        {0,1,1,0,0,0},
                        {0,0,1,0,0,1}, 
                        {0,0,1,0,0,1}, 
                        {1,0,0,0,0,1}, 
                        {1,0,0,0,0,1},
                        {1,0,0,1,0,0},
                        {1,0,0,1,0,0}, 
                        {0,0,0,1,1,0}, 
                        {0,0,0,1,1,0}, 
                        {0,1,0,0,1,0},
                        {0,1,0,0,1,0},
                        {0,1,1,0,0,0}, 
                        };
                        
  for (int i=0;i<6; i++) digitalWrite(pins[i], Cmd[I][i]); 
  I++; 
  if (I>=12)I=0;

  return 0; 
}


void SetCMD4( int *pins)
{
  static unsigned char I=0;  
  const unsigned char Cmd[12]= {
                               B00011000,
                               B00001001, 
                               B00001001, 
                               B00100001, 
                               B00100001,
                               B00100100,
                               B00100100, 
                               B00000110, 
                               B00000110, 
                               B00010010,
                               B00010010,
                               B00011000
                               };

  PORTA=Cmd[I];
  I++; 
  if (I>=12)I=0;

  return 0; 
}

void SetCMD3( int *pins)
{
  static unsigned char I=0;  
  const bool Cmd[12][6]={
                        {0,1,1,0,0,0},
                        {0,0,1,0,0,1}, 
                        {0,0,1,0,0,1}, 
                        {1,0,0,0,0,1}, 
                        {1,0,0,0,0,1},
                        {1,0,0,1,0,0},
                        {1,0,0,1,0,0}, 
                        {0,0,0,1,1,0}, 
                        {0,0,0,1,1,0}, 
                        {0,1,0,0,1,0},
                        {0,1,0,0,1,0},
                        {0,1,1,0,0,0}, 
                        };
                        
  for (int i=0;i<6; i++) digitalWrite(pins[i], Cmd[I][i]); 
  I++; 
  if (I>=12)I=0;

  return 0; 
}

void SetCMD4( int *pins)
{
  static unsigned char I=0;  
  const unsigned char Cmd[12]= {
                               B00011000,
                               B00001001, 
                               B00001001, 
                               B00100001, 
                               B00100001,
                               B00100100,
                               B00100100, 
                               B00000110, 
                               B00000110, 
                               B00010010,
                               B00010010,
                               B00011000
                               };

  PORTA=Cmd[I];
  I++; 
  if (I>=12)I=0;

  return 0; 
}

#define Tr11            22         // Transistor 1/Bras 1 
#define Tr12            23         // Transistor 2/Bras 1 
#define Tr21            24         // Transistor 1/Bras 2 
#define Tr22            25         // Transistor 2/Bras 2 
#define Tr31            26         // Transistor 1/Bras 3 
#define Tr32            27         // Transistor 2/Bras 3 

#define T0_us           333       // Période de l'onduleur (Symétrique) : T=6*T0 (µs)     
                                  // Période de l'onduleur (Décalée)    : T=12*T0 (µs)
                                   
int Trans[6];

void setup() 
{
  // Pinout 
  Trans[0]=Tr11; Trans[1]=Tr12;
  Trans[2]=Tr21; Trans[3]=Tr22;
  Trans[4]=Tr31; Trans[5]=Tr32;
  
  for (int i=0;i<6; i++) 
  {
    pinMode(Trans[i], OUTPUT);
    digitalWrite(Trans[i], LOW); 
  }
  // Ou bien: Init du port A en sortie 
  //DDRA =0xff; PORTA=0x00;

  Serial.begin(9600); 
}

void loop() 
{
  SetCMD4(Trans);
  //delayMicroseconds(T0_us); 
}







void SetCMD1( int *pins)
{
  static int I=0; 
  const bool Cmd[6][6]={{0,1,1,0,0,1}, 
                        {1,0,1,0,0,1}, 
                        {1,0,0,1,0,1},
                        {1,0,0,1,1,0}, 
                        {0,1,0,1,1,0}, 
                        {0,1,1,0,1,0}};
                        
  for (int i=0;i<6; i++) digitalWrite(pins[i], Cmd[I][i]); 
  I++; I%=6; 

  return 0; 
}





void SetCMD2( int *pins)
{
  static unsigned char I=0;  
  const unsigned char Cmd[6]={B00011001, 
                              B00101001, 
                              B00100101,
                              B00100110, 
                              B00010110, 
                              B00011010};

  PORTA=Cmd[I];
  I++; 
  
  //I%=6;             // Fréquence: + 
  //I=I*(I<6);        // Fréquence: ++
  if (I>=6)I=0;       // Fréquence: +++
  
  return 0; 
}



void SetCMD3( int *pins)
{
  static unsigned char I=0;  
  const bool Cmd[12][6]={
                        {0,1,1,0,0,0},
                        {0,0,1,0,0,1}, 
                        {0,0,1,0,0,1}, 
                        {1,0,0,0,0,1}, 
                        {1,0,0,0,0,1},
                        {1,0,0,1,0,0},
                        {1,0,0,1,0,0}, 
                        {0,0,0,1,1,0}, 
                        {0,0,0,1,1,0}, 
                        {0,1,0,0,1,0},
                        {0,1,0,0,1,0},
                        {0,1,1,0,0,0}, 
                        };
                        
  for (int i=0;i<6; i++) digitalWrite(pins[i], Cmd[I][i]); 
  I++; 
  if (I>=12)I=0;

  return 0; 
}


void SetCMD4( int *pins)
{
  static unsigned char I=0;  
  const unsigned char Cmd[12]= {
                               B00011000,
                               B00001001, 
                               B00001001, 
                               B00100001, 
                               B00100001,
                               B00100100,
                               B00100100, 
                               B00000110, 
                               B00000110, 
                               B00010010,
                               B00010010,
                               B00011000
                               };

  PORTA=Cmd[I];
  I++; 
  if (I>=12)I=0;

  return 0; 
}

void SetCMD1( int *pins)
{
  static int I=0; 
  const bool Cmd[6][6]={{0,1,1,0,0,1}, 
                        {1,0,1,0,0,1}, 
                        {1,0,0,1,0,1},
                        {1,0,0,1,1,0}, 
                        {0,1,0,1,1,0}, 
                        {0,1,1,0,1,0}};

  for (int i=0;i<6; i++) digitalWrite(pins[i], Cmd[I][i]); 
  I++; I%=6; 

  return 0; 
}

void SetCMD2( int *pins)
{
  static int I=0; 
  const unsigned char Cmd[6]={B00011001, 
                              B00101001, 
                              B00100101,
                              B00100110, 
                              B00010110, 
                              B00011010};

  PORTA=Cmd[I];
  I++; I%=6; 

  return 0; 
}

#define Tr11            22         // Transistor 1/Bras 1 
#define Tr12            23         // Transistor 2/Bras 1 
#define Tr21            24         // Transistor 1/Bras 2 
#define Tr22            25         // Transistor 2/Bras 2 
#define Tr31            26         // Transistor 1/Bras 3 
#define Tr32            27         // Transistor 2/Bras 3 

#define T0_us           3333       // Période de l'onduleur: T=6*T0 (µs)     

int Trans[6];

void setup() 
{
  // Pinout 
  Trans[0]=Tr11; Trans[1]=Tr12;
  Trans[2]=Tr21; Trans[3]=Tr22;
  Trans[4]=Tr31; Trans[5]=Tr32;
  for (int i=0;i<6; i++) 
  {
    pinMode(Trans[i], OUTPUT);
    digitalWrite(Trans[i], LOW); 
  }
  // Ou bien: Init du port A en sortie 
  DDRA =0xff; PORTA=0x00;
}

void loop() 
{
  SetCMD1(Trans);
  delayMicroseconds(T0_us); 
}

void SetCMD1( int *pins)
{
  static int I=0; 
  const bool Cmd[6][6]={{0,1,1,0,0,1}, 
                        {1,0,1,0,0,1}, 
                        {1,0,0,1,0,1},
                        {1,0,0,1,1,0}, 
                        {0,1,0,1,1,0}, 
                        {0,1,1,0,1,0}};

  for (int i=0;i<6; i++) digitalWrite(pins[i], Cmd[I][i]); 
  I++; I%=6; 

  return 0; 
}

void SetCMD2( int *pins)
{
  static int I=0; 
  const unsigned char Cmd[6]={B00011001, 
                              B00101001, 
                              B00100101,
                              B00100110, 
                              B00010110, 
                              B00011010};

  PORTA=Cmd[I];
  I++; I%=6; 

  return 0; 
}

#define Tr11            22          // Transistor 1/Bras 1 


const unsigned long     T0=250;     // T=2*T0 (µs)      
bool State=false; 

void setup() 
{
  // Pinout 
  pinMode(Tr11, OUTPUT);
  digitalWrite(Tr11, State);
}

void loop() 
{  
  digitalWrite(Tr11, State); 
  delay(T0); 
  State=!State; 
}

  // Remise à zéro de l'actionneur (Position initiale)  
  const unsigned long TCourse=10; // Temps de la course max en secondes 
  unsigned long T=millis();
  while((millis()-T)<TCourse*1000)
  {
    UpDown(PinOut, 1, 255); // Vitesse max
  }
  UpDown(PinOut, 0, 0); delay(2000); 

#define L_EN            6         // Left Enable   
#define R_EN            7         // Right Enable 
#define L_PWM           8         // Left PWM  
#define R_PWM           9         // Right PWM  
#define RapCyc          30.0      // Duty Cycle (%)                         

#define Consigne_mm     30.0      // Consigne en (mm)
#define Epsilon_mm      3.0       // Consigne en (mm)

#define Echo_in_pin     2         // Entrée: Retour de l'onde
#define Trig_out_pin    3         // Sortie: Déclenchement de l'onde
#define Speed_sound     340.0e-3  // Vitesse du son (mm/us)
#define NMoy            150       // Filtrage de la distance 

void UpDown(int* Pins, int Type,  unsigned int rapCyc)
{
  switch (Type)
  {
    // Sens 1 
    case 1:
      digitalWrite(Pins[0], HIGH);
      digitalWrite(Pins[1], HIGH);
      analogWrite(Pins[2], rapCyc); 
      analogWrite(Pins[3], 0);
      break;  

    // Sens 2 
    case 2:
      digitalWrite(Pins[0], HIGH);
      digitalWrite(Pins[1], HIGH);
      analogWrite(Pins[2], 0); 
      analogWrite(Pins[3], rapCyc); 
      break;

   // Arrêt 
   default:
      digitalWrite(Pins[0], LOW); 
      digitalWrite(Pins[1], LOW); 
      analogWrite(Pins[2], 0);
      analogWrite(Pins[3], 0); 
  }
}

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 
  tabMoy[J]=(vin0+VMFiltre)/2.0;
  J++; J%=Nm;

  // Retour de la VM
  return VMFiltre; 
}

double getDistmm(int trigpin, int echopin, double speedson) 
{
  // Déclanchement mesure avec l'envoie d'une impulsion de 10µs
  digitalWrite(trigpin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigpin, LOW);
  
  // Lecture du temps aller/retour de l'onde
  double HC_val = pulseIn(echopin, HIGH, 30000);
  
  // Calcul de distance d(mm), V=d/t==> v*t
  // Le coefficient 2 pour la distance aller/retour
  double Dist_mm = (HC_val/2.0) * speedson; 
  

  // Renvoi de la distance en (mm) 
  if (Dist_mm <= 10.0) return 10.0; // Zone aveugle du capteur 
  else return Dist_mm;  
}

  // Sens 1 - Down
  if (dist_mm> Consigne_mm+Epsilon_mm) UpDown(PinOut, 1, RP);  

  // Sens 2 - Up
  else if (dist_mm< Consigne_mm-Epsilon_mm) UpDown(PinOut, 2, RP); 

  // Arrêt  
  else UpDown(PinOut, 0, RP);

#define L_EN            6         // Left Enable   
#define R_EN            7         // Right Enable 
#define L_PWM           8         // Left PWM  
#define R_PWM           9         // Right PWM  
#define RapCyc          90.0      // Duty Cycle (%)                         

void setup() 
{
  // Pinout Module PWM
  pinMode(L_EN, OUTPUT);
  pinMode(R_EN, OUTPUT);
  pinMode(L_PWM, OUTPUT);
  pinMode(R_PWM, OUTPUT);

  // Init Module PWM
  digitalWrite(L_EN, LOW); 
  digitalWrite(R_EN, LOW); 
  analogWrite(L_PWM, 0); 
  analogWrite(R_PWM, 0); 

  // Init tableau des pins 
  PinOut[0]=L_EN;   
  PinOut[1]=R_EN ;
  PinOut[2]=L_PWM;
  PinOut[3]=R_PWM;

  // Init rapport cyclique
  RP=(unsigned short)(RapCyc*255.0/100.0);  
  
  // Affichage (Test)
  Serial.begin(115200); 
}

void UpDown(int* Pins, int Type,  unsigned int rapCyc)
{
  switch (Type)
  {
    // Sens 1 
    case 1:
      digitalWrite(Pins[0], HIGH);
      digitalWrite(Pins[1], HIGH);
      analogWrite(Pins[2], rapCyc); 
      analogWrite(Pins[3], 0);
      break;  

    // Sens 2 
    case 2:
      digitalWrite(Pins[0], HIGH);
      digitalWrite(Pins[1], HIGH);
      analogWrite(Pins[2], 0); 
      analogWrite(Pins[3], rapCyc); 
      break;

   // Arrêt 
   default:
      digitalWrite(Pins[0], LOW); 
      digitalWrite(Pins[1], LOW); 
      analogWrite(Pins[2], 0);
      analogWrite(Pins[3], 0); 
  }
}

  // Sens 1 
  if (Vin> 2.5+1.0)UpDown(PinOut, 1, RP);  

  // Sens 2 
  else if (Vin< 2.5-1.0) UpDown(PinOut, 2, RP);

  // Arrêt  
  else UpDown(PinOut, 0, RP);

#define L_EN            6         // Left Enable   
#define R_EN            7         // Right Enable 
#define L_PWM           8         // Left PWM  
#define R_PWM           9         // Right PWM  
#define RapCyc          90.0      // Duty Cycle (%)                         

float Vin=0.0; 
unsigned short RP= 0; 
int PinOut[4];

void setup() 
{
  // Pinout Module PWM
  pinMode(L_EN, OUTPUT);
  pinMode(R_EN, OUTPUT);
  pinMode(L_PWM, OUTPUT);
  pinMode(R_PWM, OUTPUT);

  // Init Module PWM
  digitalWrite(L_EN, LOW); 
  digitalWrite(R_EN, LOW); 
  analogWrite(L_PWM, 0); 
  analogWrite(R_PWM, 0); 

  // Init tableau des pins 
  PinOut[0]=L_EN;   
  PinOut[1]=R_EN ;
  PinOut[2]=L_PWM;
  PinOut[3]=R_PWM;

  // Init rapport cyclique
  RP=(unsigned short)(RapCyc*255.0/100.0);  
  
  // Affichage (Test)
  Serial.begin(115200); 
}

void loop() 
{
  // Lecture & Affichage du pin A0 
  Vin=(float)analogRead(A0)*5.0/1023.0;
  //Serial.println(Vin); return; // Test A0 

  // Sens 1 
  if (Vin> 2.5+1.0)UpDown(PinOut, 1, RP);  

  // Sens 2 
  else if (Vin< 2.5-1.0) UpDown(PinOut, 2, RP);

  // Arrêt  
  else UpDown(PinOut, 0, RP);
}

void UpDown(int* Pins, int Type,  unsigned int rapCyc)
{
  switch (Type)
  {
    // Sens 1 
    case 1:
      digitalWrite(Pins[0], HIGH);
      digitalWrite(Pins[1], HIGH);
      analogWrite(Pins[2], rapCyc); 
      analogWrite(Pins[3], 0);
      break;  

    // Sens 2 
    case 2:
      digitalWrite(Pins[0], HIGH);
      digitalWrite(Pins[1], HIGH);
      analogWrite(Pins[2], 0); 
      analogWrite(Pins[3], rapCyc); 
      break;

   // Arrêt 
   default:
      digitalWrite(Pins[0], LOW); 
      digitalWrite(Pins[1], LOW); 
      analogWrite(Pins[2], 0);
      analogWrite(Pins[3], 0); 
  }
}

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; 
}

#define Out         9       
#define In          A0       
#define Consigne    2.5        // Tension de la consigne  
#define N           150        // Moyenne: valeur initiale (setup) 
#define NMoy        100        // Moyenne glissante (loop)   
#define Epsilon     1E-3       // Epsilon
#define StepCyc     1E-2       // Pas d'incrémentation du Rapport Cyclique / Cycle 


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; //Consigne*255.0/5.0; 


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

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

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

  // Affichage 
  Serial.begin(115200); 
}

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

  // 2. Filtrage: Calcul de la Moyenne Glissante 
  for (int i=0; i<NMoy; i++) somme+=valMoy[i]; 
  v_fil=(somme/(float)NMoy);
  somme=0.0; 

  // 3. Mise à jour du tableau des VM 
  valMoy[I]=(v_fil+vin_volat)/2.0;
  I++; I%=NMoy;
  
  /*4. Mise à jour de la commande */ 
  // 4.1: Consigne Atteinte 
  if (((v_fil>=Consigne -1.5*Epsilon) && (v_fil<=Consigne +1.5*Epsilon))) 
  {
    return; // Ne rien  faire!
  }

  // 4.2: Incrémentation  
  if ((v_fil<Consigne))  
  {
    OutPWM+=StepCyc; 
    if (OutPWM>=255.0) OutPWM-=StepCyc; 
  }
    
  // 4.3: Décrémentation  
  if ( (v_fil>Consigne))  
  {
    OutPWM-=StepCyc; 
    if (OutPWM<=0.0) OutPWM+=StepCyc; 
  } 

  //5. Génération de la commande PWM
  analogWrite(Out, (unsigned int)OutPWM); 
  v_fil_old= v_fil; 

  // 6. Affichage (Test Seulement)
  Serial.print(vin_volat);  
  Serial.print(","); 
  Serial.print(Consigne); 
  Serial.print(","); 
  Serial.print(v_fil); 
  Serial.print(","); 
  Serial.println(OutPWM*5.0/255.0); 

  /*
  LoopCyc=!LoopCyc; 
  Serial.println(LoopCyc); 
  */
}

#define Out         9

#define In          A0

#define Consigne    4.0       // Tension de la consigne

#define N           100       // Filtrage de l'entrée

#define Step        1E-3      // Erreur sur la consigne (Epsilon)

#define StepCyc     1E-1      // Pas d'incrémentation du Rapport Cyclique / Cycle

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

  // Init Vin 
  for (int i=0; i<N; i++) vinTab[i]=analogRead(A0); 

  // Affichage 
  Serial.begin(115200); 
}

Vin_A0=analogRead(A0);

vinTab[I]=Vin_A0;

I=I+1;

if (I==N) I=0;

  for (int i=0; i<N; i++)
  {
    vin_volat=(float)vinTab[i]*5.0/1023.0; 
    somme+=vin_volat; 
  }
  v_fil=(somme/(float)N); 
  somme=0.0; 

  if ((v_fil>=Consigne -1.5*Step) && (v_fil<=Consigne +1.5*Step))  
    return; // Ne rien  faire!  

  if ((v_fil<Consigne))  
  {
    OutPWM+=StepCyc; 
    if (OutPWM>=255.0) OutPWM-=StepCyc; 
  }

  if ( (v_fil>Consigne))  
  {
    OutPWM-=StepCyc; 
    if (OutPWM<=0.0) OutPWM+=StepCyc; 
  }

analogWrite(Out, (unsigned int)OutPWM);

Serial.print((float)Vin_A0*5.0/1023.0);

Serial.print(",");

Serial.print(Consigne);

Serial.print(",");

Serial.print(v_fil);

Serial.print(",");

Serial.println((float)OutPWM*5.0/255.0);

#define Out         9       
#define In          A0       

#define Consigne    4.0       // Tension de la consigne        
#define N           100       // Filtrage de l'entrée 
#define Step        1E-3      // Epsilon  
#define StepCyc     1E-1      // Pas d'incrémentation du Rapport Cyclique / Cycle 



float v_fil=0.0; 
float somme =0.0;
float vin_volat=0.0; 
int   vinTab[N];
int   Vin_A0=0; 
int   I=0; 
float OutPWM=180.0; 


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

  // Init Vin 
  for (int i=0; i<N; i++) vinTab[i]=analogRead(A0); 

  // Affichage 
  Serial.begin(115200); 
}

void loop() 
{
  
  // 1. Lecture de l'entrée A0
  Vin_A0=analogRead(A0); 
  vinTab[I]=Vin_A0; 

  // 2. Mise à jour de l'indice 
  I=I+1; 
  if (I==N) I=0; 

  // 3. Filtrage: Calcul de la Moyenne Glissante 
  for (int i=0; i<N; i++)
  {
    vin_volat=(float)vinTab[i]*5.0/1023.0; 
    somme+=vin_volat; 
  }
  v_fil=(somme/(float)N); 
  somme=0.0; 
  
  /*4. Mise à jour de la sortie */ 
  // 4.1: Consigne Atteinte 
  if ((v_fil>=Consigne -1.5*Step) && (v_fil<=Consigne +1.5*Step))  
    return; // Ne rien  faire!  

  // 4.2: Incrémentation  
  if ((v_fil<Consigne))  
  {
    OutPWM+=StepCyc; 
    if (OutPWM>=255.0) OutPWM-=StepCyc; 
  }
    
  // 4.3: Décrémentation  
  if ( (v_fil>Consigne))  
  {
    OutPWM-=StepCyc; 
    if (OutPWM<=0.0) OutPWM+=StepCyc; 
  } 

  //5. Génération de la commande PWM
  analogWrite(Out, (unsigned int)OutPWM); 
   

  // 6. Affichage 
  Serial.print((float)Vin_A0*5.0/1023.0);  
  Serial.print(","); 
  Serial.print(Consigne); 
  Serial.print(","); 
  Serial.print(v_fil); 
  Serial.print(","); 
  Serial.println((float)OutPWM*5.0/255.0); 
}