Bon voila ou j'en suis j'ai adapté un code (toujours merci Black Templar) pour l'asservissement de mes deux moteurs voir le résultat ci dessous,
Mais la est le problème les commandes moteurs envoyé ressembler a çà est-ce normal ou bien mon code a un problème ?
G - D
0 - 0
6 - 6
6 - 6
0 - 0
6 - 6
6 - 6
0 - 0
6 - 6
6 - 6
6 - 6
0 - 0
6 - 6
0 - 6
5 - 6
6 - 0
6 - 6
0 - 6
6 - 0
6 - 6
0 - 6
5 - 6
6 - 0
#include <SimpleTimer.h>
#define GAUCHE 0
#define DROITE 1
SimpleTimer timer;
unsigned int tick[] = { 0, 0 }; // Compteur de tick de la codeuse
int cmd[] = { 0, 0 }; // Commande du moteur
const int frequence_echantillonnage = 20; // Fréquence d'exécution de l'asservissement
const int rapport_reducteur = 1; // Rapport entre le nombre de tours de l'arbre moteur et de la roue
const int tick_par_tour_codeuse = 20; // Nombre de tick codeuse par tour de l'arbre moteur
float consigne_moteur_nombre_tours_par_seconde[] = { 1.3 , 1.3 }; // Nombre de tours de roue par seconde
float erreur_precedente[] = { consigne_moteur_nombre_tours_par_seconde[GAUCHE] , consigne_moteur_nombre_tours_par_seconde[DROITE] };
float somme_erreur[] = { 0 , 0 }; // Somme des erreurs pour l'intégrateur
float kp = 300; // Coefficient proportionnel
float ki = 5; // Coefficient intégrateur
float kd = 0; // Coefficient dérivateur
/* Routine d'initialisation */
void setup() {
Serial.begin(9600); // Initialisation port COM
Serial2.begin(115200);// Initialisation port COM 2
attachInterrupt(GAUCHE, compteur_gauche, CHANGE); // Interruption sur tick de la codeuse (interruption 0 = pin2 arduino mega)
attachInterrupt(DROITE, compteur_droite, CHANGE); // Interruption sur tick de la codeuse (interruption 1 = pin3 arduino mega)
timer.setInterval(1000/frequence_echantillonnage, asservissement); // Interruption pour calcul du PID et asservissement
}
/* Fonction principale */
void loop(){
timer.run();
delay(10);
}
/* Interruption sur tick de la codeuse */
void compteur_gauche(){
tick[GAUCHE]++; // On incrémente le nombre de tick de la codeuse
}
void compteur_droite(){
tick[DROITE]++; // On incrémente le nombre de tick de la codeuse
}
void asservissement()
{
// Réinitialisation du nombre de tick de la codeuse
int nb_ticks[] = { tick[GAUCHE], tick[DROITE]};
tick[DROITE]=0;
tick[GAUCHE]=0;
// Calcul des erreurs
int frequence_codeuse[] = { frequence_echantillonnage*nb_ticks[GAUCHE] , frequence_echantillonnage*nb_ticks[DROITE] };
float nb_tour_par_sec[] = { (float)frequence_codeuse[GAUCHE]/(float)tick_par_tour_codeuse/(float)rapport_reducteur , (float)frequence_codeuse[DROITE]/(float)tick_par_tour_codeuse/(float)rapport_reducteur };
float erreur[] = { consigne_moteur_nombre_tours_par_seconde[GAUCHE] - nb_tour_par_sec[GAUCHE] , consigne_moteur_nombre_tours_par_seconde[DROITE] - nb_tour_par_sec[DROITE] } ;
somme_erreur[GAUCHE] += erreur[GAUCHE];
somme_erreur[DROITE] += erreur[DROITE];
float delta_erreur[] = { erreur[GAUCHE]-erreur_precedente[GAUCHE] , erreur[DROITE]-erreur_precedente[DROITE] } ;
erreur_precedente[GAUCHE] = erreur[GAUCHE];
erreur_precedente[DROITE] = erreur[DROITE];
// PID : calcul de la commande
cmd[GAUCHE] = kp*erreur[GAUCHE] + ki*somme_erreur[GAUCHE] + kd*delta_erreur[GAUCHE];
cmd[DROITE] = kp*erreur[DROITE] + ki*somme_erreur[DROITE] + kd*delta_erreur[DROITE];
// Normalisation et contrôle du moteur
if(cmd[GAUCHE] < 0) cmd[GAUCHE]=0;
else if(cmd[GAUCHE] > 255) cmd[GAUCHE] = 255;
cmd[GAUCHE]= cmd[GAUCHE]/25.5;
if(cmd[DROITE] < 0) cmd[DROITE]=0;
else if(cmd[DROITE] > 255) cmd[DROITE] = 255;
cmd[DROITE]= cmd[DROITE]/25.5;
Serial2.write('2');
Serial2.write('r');
Serial2.write(int_char(cmd[GAUCHE]));
Serial2.write('\r');
delay(10);
Serial2.write('1');
Serial2.write('r');
Serial2.write(int_char(cmd[DROITE]));
Serial2.write('\r');
delay(10);
// DEBUG
Serial.print(cmd[GAUCHE]);
Serial.print(" - ");
Serial.println(cmd[DROITE]);
}
char int_char(int n){
char resultat='0';
if(n < 1) resultat='0';
else if (n < 2) resultat='1';
else if (n < 3) resultat='2';
else if (n < 4) resultat='3';
else if (n < 5) resultat='4';
else if (n < 6) resultat='5';
else if (n < 7) resultat='6';
else if (n < 8) resultat='7';
else if (n < 9) resultat='8';
else if (n >= 9) resultat='9';
return resultat;
}
Pourquoi j'ais des commandes a 0 en permanence ?
