Merci Mike pour tous les conseils.
C bon, je controle mes moteurs dans les mêmes sens et à la même vitesse.
J'ai simplifié comme ça la fonction de commande
void commandMotor (int8_t commandeMoteurDroit, int8_t commandeMoteurGauche) {
if (valeur >0){/
SetpointB = commandeMoteurDroit;
SetpointA = commandeMoteurGauche;
}
else {
SetpointB = -commandeMoteurDroit;
SetpointA = -commandeMoteurGauche;
}
//utilisation de la sortie du PID pour asservir les moteurs
if (valeur >= 0) {
advanceB();
}
if (valeur < 0) {
backB();
}
if (valeur >= 0) {
advanceA();
}
if (valeur < 0) {
backA();
}
J'ai retiré myPIDB.SetOutputLimits(-255, 255), pour lire la tension de sortie toujours positivement et que ça n'embrouille pas le programme (ou moi même).
Par contre petit truc, j'aurai aimé avoir la vitesse de mes moteurs hyper constante pour me rapprocher de la théorie du PID.,
De sorte que, lorsque les moteurs sont censés être arrivés à leurs vitesses qui est stable dans le temps (celle de leur Setpoint), il y ait toujours le même décalage de phase entre eux.
Mes deux moteurs au bout de 2 minutes, ils se décalent de 90°. Ce qui est pas beaucoup sachant qu'ils tournent à 70BPM. Mais je sais pas s'il est peut être possible d'améliorer ce résultat en remplaçant des constantes byte par volatile byte ou unsigned char ou.....
Aussi quand, je bloque mes deux moteurs la tension augmente et quand je les lache, ils retournent à leurs vitesses asservies.
Ce qui est normal.
Mais quand je les bloque plus longtemps jusqu'à ce que leur val_output atteint les 255, le moteur de gauche ne se "force plus" à revenir à son Setpoint il 'est à l'arrêt alors que celui de droite se comporte normalement, il fait des à coup.
.
C'est seulement quand je lache le moteur de droite que ça "débloque" le moteur de gauche qui retourne à son Setpoint.
Là je pense que la vitesse détectée du moteur droit influence le calcule ou les attachinterrupt du moteur gauche.
D'où peut être l'utilité du mask comme dans votre premier programme avec un seul moteur, mais je ne sais pas comment l'intégrer au mien.
state|=B11101011; // mask pour ne regarder que les changements sur 2 et 4
// Modifier le MASK B01111011 par BXXXXXXXX mettre des 0 là où sont les pins utilisés par l'encodeur
if( state!=laststate)
Je mets mon programme
#include <PID_v1.h>
// PORT POUR KIT L298N 2nd (Bleu clair) supllied with 6 V
// MOTEUR A GAUCHE avec myPIDA 8,9,5,3,7,
//Ports de commande du moteur A supplied with 6 volt= 90 round/min.
int motorPinA1 = 9; //13 pin to set H BRIDGE of L298N
int motorPinA2 = 8; //12 pin to set H BRIDGE of L298N
const int enablePinA= 5; //11 // Commande de vitesse moteur, to Output ENA pour L298 the second
// MOTEUR B Droit avec myPIDB 10,11,6,2,4
//Ports de commande du moteur B supplied with 6 volt.
int motorPinB1 = 10; // pin to set H BRIDGE of L298N
int motorPinB2 = 11; //12 pin to set H BRIDGE of L298N
const int enablePinB= 6; //11 // Commande de vitesse moteur, to Output ENA pour L298 the second
//MOTOR GAUCHE pin D 3,7,9,8,5
volatile byte encoderApinA = 3;//A pin -> the interrupt pin 3
byte encoderApinB = 7;//B pin -> the digital pin 7 /// What is better? byte or const byte ?
volatile byte encoderAPinALast; // What is better? volatile byte encoderAPinALast or just byte encoderAPinALast ?
double latestDurationCountA,durationA,abs_durationA;//the number of the pulses
boolean DirectionA;//the rotation direction
boolean resultA;
double val_outputA;//Power supplied to the motor PWM value.
double SetpointA;
double Kp=1, Ki=10, Kd=0.01; // 1 ; 10 ; 0.01
PID myPIDA(&abs_durationA, &val_outputA, &SetpointA, Kp, Ki, Kd, DIRECT);
//**************************************************
//MOTOR DROIT myPIDB motor pinB+encoderB 2,4,11,10,6
volatile byte encoderBpinA = 2;//A pin -> the interrupt pin 1/*volatile byte
byte encoderBpinB = 4;//B pin -> the digital pin 4
volatile byte encoderBPinALast;
double latestDurationCountB, durationB, abs_durationB; //the number of the pulses
boolean DirectionB;//the rotation direction
boolean resultaB;
double val_outputB;//Power supplied to the motor PWM value.
double SetpointB;
//double Kp=0.6, Ki=5, Kd=0; // 0.6 ; 5 ; 0
PID myPIDB(&abs_durationB, &val_outputB, &SetpointB, Kp, Ki, Kd, DIRECT);
unsigned long time0 = 0; // stock un temps à un instant donné
unsigned long timer = 0; // variable qui va contenir le dernier temps enregistré via millis
int8_t valeur = 20; //lecture de la consigne via le moniteur serie pour test, initialisé à 40 pour test rapide du PID
void setup()
{
Serial.begin(19200);//Initialize the serial port
// Configuration des ports en mode "sortie" A moteur GAUCHE
pinMode(motorPinA1, OUTPUT);
pinMode(motorPinA2, OUTPUT);
pinMode(enablePinA, OUTPUT);
// Configuration des ports en mode "sortie" B moteur DROIT
pinMode(motorPinB1, OUTPUT); //L298N Control port settings direction of motor B (originaly L298N)
pinMode(motorPinB2, OUTPUT); //L298N Control port settings direction of motor B
pinMode(enablePinB, OUTPUT); // powerRate to control speed of motor B
// REGLAGE PID MOTOR A (gauche)
SetpointA;
myPIDA.SetMode(AUTOMATIC);//PID is set to automatic mode
myPIDA.SetSampleTime(5);//Set PID sampling frequency is 100ms
// myPIDA.SetOutputLimits(-255, 255);
EncoderInitA();
//REGLAGE PID MOTEUR DROIT (droit)
SetpointB;
myPIDB.SetMode(AUTOMATIC);//PID is set to automatic mode
myPIDB.SetSampleTime(5);//Set PID sampling frequency is 100ms
//myPIDB.SetOutputLimits(-255, 255);
EncoderInitB();
}
void loop()
{ if (Serial.available()) {
time0 = timer;
valeur = Serial.parseInt(); //récupération des caractères sur le port série
Serial.println("-----------------valeur");Serial.println(valeur);
}
if (valeur != 0) {
commandMotor(valeur,valeur);
}
else {
SetpointA=0; SetpointB=0;
stopMotors();
}
timer = millis();
if ( (timer - time0) >120000) { // Timeout !
valeur = 0 ;
stopMotors();
time0 = timer;
}
//MOTOR GAUCHE
noInterrupts();
latestDurationCountA = durationA;
interrupts();
abs_durationA = abs(latestDurationCountA);
resultA=myPIDA.Compute();//PID conversion is complete and returns 1
// advanceA();
if(resultA)
{
Serial.print("LD"); Serial.print(latestDurationCountA);
Serial.print("D : "); Serial.print(durationA); Serial.print(durationA); Serial.print(durationA);
durationA = 0; //Count clear, wait for the next count
//Serial.print("SetpointA"); Serial.println(SetpointA);
Serial.print("V"); Serial.println(val_outputA);// volt to MOTOR= real speed
}
// *******************************************
// SETTING MOTEUR DROIT PIDB avec PIN B plus rapide?
noInterrupts();
latestDurationCountB = durationB;
interrupts();
abs_durationB = abs(latestDurationCountB);
// SetpointB=20; //
resultaB=myPIDB.Compute();//PID conversion is complete and returns 1
// advanceB();
if(resultaB){
Serial.print("LDb"); Serial.print(latestDurationCountB);
Serial.print("Db: "); Serial.print(durationB);Serial.print(durationB);Serial.print(durationB);
durationB = 0; //Count clear, wait for the next count
//Serial.print("SetpointB"); Serial.println(SetpointB);
Serial.print("Va"); Serial.println(val_outputB);// volt to MOTOR= real speed
}
}
void EncoderInitA()
{
DirectionA = true;//default -> Forward
pinMode(encoderApinB,INPUT);
attachInterrupt(1, wheelSpeedA, CHANGE);
}
void wheelSpeedA()
{
byte Lstate = digitalRead(encoderApinA);
if((encoderAPinALast == LOW) && Lstate==HIGH)
{
byte val = digitalRead(encoderApinB);
if(val == LOW && DirectionA)
{
DirectionA = false; //Reverse
}
else if(val == HIGH && !DirectionA)
{
DirectionA = true; //Forward
}
}
encoderAPinALast = Lstate; // volatile byte encoderAPinALast ??
if(!DirectionA) durationA++;
else durationA--;
}
void advanceA()//Motor Forward
{
digitalWrite(motorPinA1,HIGH);
digitalWrite(motorPinA2,LOW);
analogWrite(enablePinA,val_outputA);
}
void backA()//Motor reverse
{
digitalWrite(motorPinA1,LOW);
digitalWrite(motorPinA2,HIGH);
analogWrite(enablePinA,val_outputA);
}
void StopA()//Motor stops
{
digitalWrite(enablePinA, LOW);
}
void EncoderInitB()
{
DirectionB = true;//default -> Forward
pinMode(encoderBpinB,INPUT);
attachInterrupt(0, wheelSpeedB, CHANGE);
}
void wheelSpeedB()
{
byte LstateB = digitalRead(encoderBpinA);
if((encoderBPinALast == LOW) && LstateB==HIGH)
{
byte valB = digitalRead(encoderBpinB);
if(valB == LOW && DirectionB)
{
DirectionB = false; //Reverse
}
else if(valB == HIGH && !DirectionB)
{
DirectionB = true; //Forward
}
}
encoderBPinALast = LstateB;
if(!DirectionB) durationB++;
else durationB--;
}
void advanceB()//Motor Forward
{
digitalWrite(motorPinB1,HIGH);
digitalWrite(motorPinB2,LOW);
analogWrite(enablePinB,val_outputB);
}
void backB()//Motor reverse
{
digitalWrite(motorPinB1,LOW);
digitalWrite(motorPinB2,HIGH);
analogWrite(enablePinB,val_outputB);
}
void StopB()//Motor stops
{
digitalWrite(enablePinB, LOW);
}
void stopMotors() {
StopB();
StopA();
// digitalWrite(enablePinA, 0);
//digitalWrite(enablePinB, 0);
}
void commandMotor (int8_t commandeMoteurDroit, int8_t commandeMoteurGauche) {
if (valeur >0){// || commandeMoteurDroit
SetpointB = commandeMoteurDroit;
SetpointA = commandeMoteurGauche;
}
else {
SetpointB = -commandeMoteurDroit;
SetpointA = -commandeMoteurGauche;
}
//int outputDroit = (int)val_outputB; // creer nouvelle donnée de val-output egale à (int)val-outputB
//int outputGauche = (int)val_outputA; //gauche
//utilisation de la sortie du PID pour asservir les moteurs
if (valeur >= 0) {
advanceB();
}
if (valeur < 0) {
backB();
}
if (valeur >= 0) {
advanceA();
}
if (valeur < 0) {
backA();
}
}
Merci pour votre aide, j'espère n'être pas trop confus.
