Salut a tous j 'utilise deux moteurs que j 'aimerais asservir en vitesse
voici le code
#include <SimpleTimer.h>
SimpleTimer timer;
// définition des pins de l'Arduino qui contrôlent le moteur droit (jaune)
#define pinIN1 8
#define pinIN2 12
#define pinENA 6 // doit être une pin PWM
// définition des pins de l'Arduino qui contrôlent le moteur gauche(Bleue)
#define pinIN3 13
#define pinIN4 4
#define pinENB 5 // doit être une pin PWM
volatile int comptageImpulsion_link=0; // variable accessible dans la routine interruption externe 0
volatile int comptageImpulsion_right=0; // variable accessible dans la routine interruption externe 1
volatile int i;
volatile int j ;
volatile int vitMoteur = 0.;
volatile double speed_right ;
volatile double speed_link ;
volatile double vit ;
volatile double erreur;
const int pas= 20 ;// nombre de ticks sur le disque codeur
double consigne_moteur= 0.0; // cm/s
// init calculs asservissement PID
double erreur_precedente= consigne_moteur; // (en cm/s)
double somme_erreur = 0;
//Definition des constantes du correcteur PID
double kp = 0.4; // Coefficient proportionnel
double ki = 2; // Coefficient intégrateur
double kd = 1; // Coefficient dérivateur
// duree d asservissement
double time_asservissement = 250; // millisseconde
int timerId;
// duree d 'asservissement en seconde
double dt = time_asservissement /1000 ;
unsigned long duree ;
void setup() {
Serial.begin(9600);
// Moteur Gauche
pinMode(pinIN1, OUTPUT);
pinMode(pinIN2, OUTPUT);
pinMode(pinENA, OUTPUT);
analogWrite(pinENA, 0); // Initialisation sortie moteur à 0
delay(300); // Pause de 0,3 sec pour laisser le temps au moteur de s'arréter si celui-ci est en marche
// Moteur droit
pinMode(pinIN3, OUTPUT);
pinMode(pinIN4, OUTPUT);
pinMode(pinENB, OUTPUT);
analogWrite(pinENB, 0); // Initialisation sortie moteur à 0
delay(300); // Pause de 0,3 sec pour laisser le temps au moteur de s'arréter si celui-ci est en marche
attachInterrupt(1, speedright, RISING );
attachInterrupt(0, speedlink, RISING);
timerId= timer.setInterval(time_asservissement, asservissement);
}
void loop() {
//duree= millis();
//asservissement();
//duree =millis()-duree;
//Serial.print("duree");
//Serial.print(duree);
//Serial.println();
////
timer.run();
delay(10);
//timer.disable(timerId);
}
void asservissement(){
int link = comptageImpulsion_link;
int recht =comptageImpulsion_right;
// reinitialisation de mon tick de la codeuse
comptageImpulsion_link= 0; // reset counter to zero
comptageImpulsion_right= 0;
double rotation_link = ( link/ pas)/dt ;// vitesse du moteur gauche en tour/s
double speed_link = ( 2*3.14*radius)*rotation_link; // cm /s
double rotation_right = (recht / pas)/dt ;// vitesse du moteur droit en 1 tour/s
double speed_right = (2*3.14*radius)*rotation_right; // cm/s
//Serial.print("speed_link");
//Serial.print(speed_link);
//Serial.println();
double vit = (speed_right);
//double vit = (speed_right+speed_link)/2;
Serial.print("peedright:");
Serial.print(speed_right);
Serial.print("speedlink:");
Serial.print(speed_link);
Serial.println();
//Serial.print("Istzustand:");
// Serial.print(vit,8);
// Serial.println();
/******* Régulation PID ********/
double erreur = consigne_moteur-vit;
somme_erreur=somme_erreur+ erreur ;
double delta = erreur -erreur_precedente;
erreur_precedente = erreur;
vitMoteur = (kp*erreur +ki*somme_erreur+ kd*delta) ;
/******* Régulation PID ********/
if (vitMoteur > 255) {
vitMoteur = 255; // sachant que l'on est branché sur un pont en H L298
}
else if (vitMoteur <0) {
vitMoteur = 0;
}
vorne(vitMoteur);
Serial.print("Erreur:");
Serial.print(erreur,4);
Serial.println();
Serial.print("Commande:");
Serial.print(vitMoteur);
Serial.println();
Serial.println();
}
void vorne(int Powerrate){
analogWrite( pinENA, Powerrate );
digitalWrite( pinIN1, false );
digitalWrite( pinIN2, true );
// Moteur droit
analogWrite( pinENB, Powerrate );
digitalWrite( pinIN3, LOW );
digitalWrite( pinIN4, HIGH );
}
void speedlink() {// la fonction appelée par l'interruption externe n°0
comptageImpulsion_link=comptageImpulsion_link+1; // Incrémente la variable de comptage
}
void speedright() {// la fonction appelée par l'interruption externe n°1
comptageImpulsion_right=comptageImpulsion_right+1; // Incrémente la variable de comptage
}
Je voudrais savoir si j 'ai bien calculer les vitesses de mes moteurs 'right' et 'link' en cm/s , et également si j ai bien implémenter mon asservissement ?
Merci