c'est le programme que j'utilise pour l'instant 
asservissement.ino 3,95 Ko
56 téléchargement(s)
#include <FlexiTimer2.h>
#include <digitalWriteFast.h>
// Codeur incrémental
#define codeurInterruptionA 0
#define codeurInterruptionB 1
#define codeurPinA 2
#define codeurPinB 3
volatile long ticksCodeur = 0;
// Moteur CC
#define directionMoteur 4
#define pwmMoteur 5
// Cadence d'envoi des données en ms
#define TSDATA 100
unsigned long tempsDernierEnvoi = 0;
unsigned long tempsCourant = 0;
// Cadence d'échantillonnage en ms
#define CADENCE_MS 10
volatile double dt = CADENCE_MS/1000.;
volatile double temps = -CADENCE_MS/1000.;
volatile double omega;
volatile double commande = 0.;
volatile double vref = 6.5;
// PID
volatile double Kp = 0.29;
volatile double Ki = 8.93;
volatile double P_x = 0.;
volatile double I_x = 0.;
volatile double ecart = 0.;
// Initialisations
void setup(void) {
// Codeur incrémental
pinMode(codeurPinA, INPUT); // entrée digitale pin A codeur
pinMode(codeurPinB, INPUT); // entrée digitale pin B codeur
digitalWrite(codeurPinA, HIGH); // activation de la résistance de pullup
digitalWrite(codeurPinB, HIGH); // activation de la résistance de pullup
attachInterrupt(codeurInterruptionA, GestionInterruptionCodeurPinA, CHANGE);
attachInterrupt(codeurInterruptionB, GestionInterruptionCodeurPinB, CHANGE);
// Moteur CC
pinMode(directionMoteur, OUTPUT);
pinMode(pwmMoteur, OUTPUT);
// Liaison série
Serial.begin(9600);
Serial.flush();
// Compteur d'impulsions de l'encodeur
ticksCodeur = 0;
// La routine isrt est exécutée à cadence fixe
FlexiTimer2::set(CADENCE_MS, 1/1000., isrt); // résolution timer = 1 ms
FlexiTimer2::start();
}
// Boucle principale
void loop() {
// Ecriture des données sur la liaison série
ecritureData();
}
void isrt(){
int codeurDeltaPos;
double tensionBatterie;
// Nombre de ticks codeur depuis la dernière fois
codeurDeltaPos = ticksCodeur;
ticksCodeur = 0;
// Calcul de la vitesse de rotation
omega = ((2.*3.141592*((double)codeurDeltaPos))/1632.)/dt; // en rad/s
/******* Régulation PID ********/
// Ecart entre la consigne et la mesure
ecart = vref - omega;
// Terme proportionnel
P_x = Kp * ecart;
// Calcul de la commande
commande = P_x + I_x;
// Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
I_x = I_x + Ki * dt * ecart;
/******* Fin régulation PID ********/
// Envoi de la commande au moteur
tensionBatterie = 7.2;
CommandeMoteur(commande, tensionBatterie);
temps += dt;
}
void ecritureData(void) {
// Ecriture des données en sortie tous les TSDATA millisecondes
tempsCourant = millis();
if (tempsCourant-tempsDernierEnvoi > TSDATA) {
Serial.print(temps);
Serial.print(",");
Serial.print(omega);
Serial.print("\r");
Serial.print("\n");
tempsDernierEnvoi = tempsCourant;
}
}
void CommandeMoteur(double tension, double tensionBatterie)
{
int tension_int;
// Normalisation de la tension d'alimentation par
// rapport à la tension batterie
tension_int = (int)(255*(tension/tensionBatterie));
// Saturation par sécurité
if (tension_int>255) {
tension_int = 255;
}
if (tension_int<-255) {
tension_int = -255;
}
// Commande PWM
if (tension_int>=0) {
digitalWrite(directionMoteur, LOW);
analogWrite(pwmMoteur, tension_int);
}
if (tension_int<0) {
digitalWrite(directionMoteur, HIGH);
analogWrite(pwmMoteur, -tension_int);
}
}
// Routine de service d'interruption attachée à la voie A du codeur incrémental
void GestionInterruptionCodeurPinA()
{
if (digitalReadFast2(codeurPinA) == digitalReadFast2(codeurPinB)) {
ticksCodeur--;
}
else {
ticksCodeur++;
}
}
// Routine de service d'interruption attachée à la voie B du codeur incrémental
void GestionInterruptionCodeurPinB()
{
if (digitalReadFast2(codeurPinA) == digitalReadFast2(codeurPinB)) {
ticksCodeur++;
}
else {
ticksCodeur--;
}
}
pardon pour la blague privée. 
mais perso, j'utilise les chifons à micro fibres à la mano. J'imagine que emmenché sur un axe rotatif, ça doit ramasser de la poussière.
