Controler un moteur et un servo avec arduino

#include <FlexiTimer2.h> #include <digitalWriteFast.h>  
#include <Servo.h>   
#define codeurInterruptionA 0 
#define codeurInterruptionB 1 
#define codeurPinA 2 
#define codeurPinB 3 
volatile long ticksCodeur = 0; 
static int codeurDeltaPos;  
#define directionMoteur  4 
#define pwmMoteur  5 
double tensionAlim = 11.;  
#define TIMEOUT 2 
unsigned long timeLastReceived = 0; 
static int timedOut = 0;  
#define TSDATA 20 
unsigned long tempsDernierEnvoi = 0;
unsigned long tempsCourant = 0;   
#define CADENCE_MS 20 
volatile double dt = CADENCE_MS/1000.; 
volatile double temps = -CADENCE_MS/1000.;  
static double commande = 0.; 
static double omega = 0.;  
const int NOMBRE_DE_CHAMPS = 5; 
// nombre de champs de données reçues sur la liaison série 
int champIndex = 0; 
// champ courant reçu 
int valeursRecues[NOMBRE_DE_CHAMPS]; 
// tableau contenant les champs de données reçues static int typeSignal = 0; 
static double offset = 0.; 
static double amplitude = 0.; 
static double frequence = 0.; 
static double crc = 0.; 
static double crc_check = 0.;  
Servo ser; int angle=90; char ordre;  
void setup() {   
    ser.attach(9);
   ser.write(angle);
   Serial.begin(9600);
      pinMode(codeurPinA, INPUT);
         pinMode(codeurPinB, INPUT);
         digitalWrite(codeurPinA, HIGH);
     digitalWrite(codeurPinB, HIGH);
     attachInterrupt(codeurInterruptionA, GestionInterruptionCodeurPinA, CHANGE);
   attachInterrupt(codeurInterruptionB, GestionInterruptionCodeurPinB, CHANGE);
    pinMode(directionMoteur, OUTPUT);
   pinMode(pwmMoteur, OUTPUT);
    Serial.begin(115200);
   Serial.flush();
      FlexiTimer2::set(CADENCE_MS, 1/1000., isrt); // résolution timer = 1 ms
   FlexiTimer2::start();  
}
  void loop() {
    readData();    
}  
void isrt(){
    codeurDeltaPos = ticksCodeur;
   ticksCodeur = 0;
    omega = ((2.*3.141592*((double)codeurDeltaPos))/3200.)/dt;
        if (typeSignal==0) { // signal carré
     if (frequence > 0) {
       if (temps - ((double)((int)(temps*frequence)))/frequence < 1/(2*frequence)) {
         commande = offset + amplitude;       
}       
else {
         commande = offset;       
}
     }
     else {
       commande = offset + amplitude;
     }
   }
   else { //sinus
     if (frequence > 0) {
       commande = offset + amplitude * sin(2*3.141592*frequence*temps);
     }
     else {
       commande = offset + amplitude;
     }
   }
          CommandeMoteur(commande, tensionAlim);
    temps += dt;
 }  
void ecritureData(void) {
       tempsCourant = millis();
   if (tempsCourant-tempsDernierEnvoi > TSDATA) {
     Serial.print(temps);
          Serial.print(",");
     Serial.print(commande);
     Serial.print(",");
     Serial.print(omega);
         Serial.print("\r");
     Serial.print("\n");
          tempsDernierEnvoi = tempsCourant;
   }
   }
  void readData(void) {
    champIndex = 0;
   for(int i=0; i < NOMBRE_DE_CHAMPS; i++) {
     valeursRecues[i] = 0;
   }
    while(true) {
      ecritureData();
        if (Serial.available()>0) {
        timeLastReceived = millis();
       timedOut = 0;
       char ch = Serial.read(); 
      if (ch >= '0' && ch <= '9') {
          valeursRecues[champIndex] = (valeursRecues[champIndex] * 10) + (ch - '0');
       }
       else if (ch == ',') {
          if(champIndex < NOMBRE_DE_CHAMPS-1) {
           champIndex++;
          }
       }
       else {
         break;
        }
      }
     else {
        if ((millis()-timeLastReceived > TIMEOUT*1000) && (timedOut==0)) {
         for(int i=0; i < NOMBRE_DE_CHAMPS; i++) {
           offset = 0.;
           amplitude = 0.;
           frequence = 0.;
         }
         timedOut = 1;
       }
     }
       }
      if (champIndex == NOMBRE_DE_CHAMPS-1) {
      crc = valeursRecues[NOMBRE_DE_CHAMPS-1];
     crc_check = 0;
     for(int i=0; i < NOMBRE_DE_CHAMPS-1; i++) {
       crc_check += valeursRecues[i];
     }
          if (crc == crc_check) {
        typeSignal = valeursRecues[0];
       offset = ((double)valeursRecues[1]-60)/10.;
       amplitude = ((double)valeursRecues[2])/10.;
       frequence = ((double)valeursRecues[3])/100.;
     }
     else {
     }
   }
   else {
    }
 }
  void CommandeMoteur(double commande, double tensionAlim) {
   int tension_int;
   double tension;
      tension = redresse_commande(commande, tensionAlim);
      tension_int = (int)(255*(tension/tensionAlim));
      if (tension_int>255) {
     tension_int = 255;
   }
   if (tension_int<-255) {
     tension_int = -255;
   } 
     if (tension_int>=0) {
     digitalWrite(directionMoteur, LOW);
     analogWrite(pwmMoteur, tension_int);
   }
   if (tension_int<0) {
     digitalWrite(directionMoteur, HIGH);
     analogWrite(pwmMoteur, -tension_int);
   }
 }
  double redresse_commande(double commande, double tensionAlim) {
   if (commande>0) {
     return(commande + 0.*tensionAlim/tensionAlim);
    }
   else if (commande<0) {
     return(commande - 0.*tensionAlim/tensionAlim);
    }
   else {
     return(0);
    }
 }
    void GestionInterruptionCodeurPinA() {
     if (digitalReadFast2(codeurPinA) == digitalReadFast2(codeurPinB)) {
     ticksCodeur++;
   }
   else {
     ticksCodeur--;
   }
 }
  void GestionInterruptionCodeurPinB() {
   if (digitalReadFast2(codeurPinA) == digitalReadFast2(codeurPinB)) {
     ticksCodeur--;
   }
   else {
     ticksCodeur++;
   }
   if (Serial.available()>0) {
ordre = Serial.read ();
 switch(ordre) {
 case 'd' : 
case 'D' :
 angle=angle+10;
 if (angle>160) {
angle=160;
}
 ser.write(angle);
 break;
 case 'g' :
 case 'G' :
 angle=angle-10;
 if (angle<0) {
angle=0;
 }
 ser.write(angle); break; 
} 
} 
}