fonction filtrage: différentiel électronique RC 1/5 la suite

Bonjour,
Voici ou j’en suis avec mon projet moteur roue:

J’ai un souci de stabilité sur mes commandes de contrôleur.
J’ai essayé d’utiliser la fonction “filter” pour faire un filtrage 1er ordre sur mes commandes mais sans grand succès apparent.
Je joins mon programme si quelqu’un peut m’aider… je pense que je n’utilise pas cette fonction correctement.

cg=lowpassFilter1.input(cg); // filtrage cg
cd=lowpassFilter2.input(cd); // filtrage cd

Cordialement

// inclusion des bibliothèques
#include <LiquidCrystal.h> //pour l’afficheur lcd
#include <Servo.h> //pour la commande des servo ou de vitesse
#include <Filters.h>

// Initialisation de la bibliothèque par rapport au cablage du shield afficheur lcd
LiquidCrystal lcd(7, 6, 5, 4, 3, 2); //utilisation des pins DIGITAL 7, 6, 5, 4, 3, 2

// declaration des sous programmes
void tempodemarrage (void);
void affichage(void);
void lecture(void);

// Definition Objet controle de servo
Servo consigneg; //objet gauche
Servo consigned; //objet droite

//Definition des entrees
const int inputPin = A0; //A0 reservée a la carte affichage, permet par pont diviseur plusieurs tension en fonction des boutons
int signalvit = A3; // signal de consigne de vitesse sur patte 0 en MLI entree
int signaldir = A2; // signal de consigne de direction sur patte 0 en MLI entree

//Variables et constantes

float filterFrequency = 10.0; // fréquence du filtre
double corar = 0.1; //constante de correction arrière
double dec = 0; //variable de decalage ard et arg
int vers = 14; //numero de version

int inputValue = 0; //variable entree bouton
int commande = 0; //variable d acquisition moniteur serie
double c; //varaible de consigne de vitesse
double d; // variable de consigne de direction
double cor; //variable de correction de vitesse droite/gauche arriere
double cg; //variable de consigne arriere gauche
double cd; //variable de consigne arriere droite
unsigned int t; // variable de tempo

// création filtre passe bas
FilterOnePole lowpassFilter1( LOWPASS, filterFrequency );
FilterOnePole lowpassFilter2( LOWPASS, filterFrequency );

//Initialisation
void setup()
{

Serial.begin(9600);
consigneg.attach(9); // attache du servo sur la pin 9 de l’objet consigne arriere gauche
consigned.attach(10); // attache du servo sur la pin 10 de l’objet consigne arrire droite
//mise variable rotation 0 des moteurs (à 90°) pour garantir la non-rotation
cg=90;
cd=90;
consigneg.write(cg); // mise a la position de la commande gauche sortie pwm
consigned.write(cd); // mise a la position de la commande droite sortie pwm

//Affichage d’info en debut de programme et temps d’initilalisation des controleurs
lcd.begin(16, 2);
lcd.print(“Programme 4*2”);
lcd.setCursor(0, 1); // Déplace le curseur sur le caractère 0 de la ligne 1
lcd.print(“diff_4x2_”);
lcd.print(vers);
tempodemarrage(); // tempo de demarrage, temps d initialisation des controleurs de puissance ti ti ti 4 secondes
lcd.begin(16, 2);
lcd.print(“diff_4x2_”);
lcd.print(vers);
lcd.setCursor(0, 1); // Déplace le curseur sur le caractère 0 de la ligne 1
lcd.print(“Initialisation…”);
tempodemarrage();
lcd.begin(16, 2);
lcd.print(“PrgActif_4x2_”);
lcd.print(vers);
}

//Debut du programme
void loop()

{
affichage();
lecture();

c = pulseIn(signalvit, HIGH, 11000); //lecture du signal de la radio consigne de vitesse mise en variable c, variable de 1000 a 2000
c = constrain(c, 1000, 2000);
//apres tatonnement j’ai trouve les deux valeurs de 500 et 2400 pour avoir les bonnes commandes par la suite. 1000 et 2000 par defaut
c = map(c, 500, 2400, 0, 180); //calibrage de l’impulsion 1-1,5-2ms en 0-90-180°
d = pulseIn(signaldir, HIGH, 11000); //lecture du signal de la radio consigne de direction mise en variable c, variable de 1000 a 2000
d = constrain(d, 1000, 2000);
d = map(d, 600, 2400, -90, 90); //calibrage de l’impulsion 1-1,5-2ms en -90,0,90°

if (c <= 96) // si pas de commande de marche avant ou commande marche arrière alors cgauche = cdroite_ pas de différentiel en arrière
{
cd = c; //Consigne d’entree => sorties; pas de correction
cg = c + dec;
}
else //sinon commande marche avant
{
cor = d * corar; // on calcul la correction de vitesse ar en fonction de l’angle
cd = c + cor; // plus de consigne a droite
cg = c - cor + dec; //moins de consigne a gauche
cg = constrain(cg, 90, 220); //contrainte des consignes de 90 à 220° - bloque rotation ar - autorise plus en avant
cd = constrain(cd, 90, 220);
}

cg=lowpassFilter1.input(cg); // filtrage cg
cd=lowpassFilter2.input(cd); // filtrage cd

consigneg.write(cg); //ecriture sur la sortie 9 mise a la position de la commande gauche sortie pwm
consigned.write(cd); //ecriture sur la sortie 10 mise a la position de la commande droite sortie pwm

}
/

Bonjour, Mets ton programme avec les balises codes et lit la charte du forum @+