Bonjour,
J'utilise un algo PID pour contrôler un moteur avec une librairie toute faite.
J'aimerai l'utiliser pour un deuxième moteur dans mon programme et plus tard jusqu'à cinq.
J'ai juste rajouté des 2 à chacun des paramètres mais je ne suis pas du tout sûr de cette méthode.
//The sample code for driving one way motor encoder
#include <PID_v1.h>
const byte encoder0pinA = 2;//A pin -> the interrupt pin 0
const byte encoder0pinB = 3;//B pin -> the digital pin 3
byte encoder0PinALast;
double duration, abs_duration, latestDurationCount;//the number of the pulses
boolean Direction;//the rotation direction
boolean result;
double val_output;//Power supplied to the motor PWM value. (in Volt)
//double speed=val_output;
double Setpoint;// assignated speed (when setpoint = 30) => val-outpout and duration== (41 to 43) when no couple in the motor
double Kp=0.4, Ki=5, Kd=0; // Set Kp 0.5, Ki 5 and Kd "to set the strenght of distortion of the error"
PID myPID(&abs_duration, &val_output, &Setpoint, Kp, Ki, Kd, DIRECT);
//*************************************************
// The sample code for driving a second motor encoder
const byte encoder0pinA2 = 18;//A pin -> the interrupt pin 0
const byte encoder0pinB2 = 19;//B pin -> the digital pin 3
byte encoder0PinALast2;
double duration2, abs_duration2, latestDurationCount2;//the number of the pulses
boolean Direction2;//the rotation direction
boolean result2;
double val_output2;//Power supplied to the motor PWM value. (in Volt)
//double speed=val_output;
double Setpoint2;// assignated speed (when setpoint = 30) => val-outpout and duration== (41 to 43) when no couple in the motor
double Kp2=0.4, Ki2=5, Kd2=0; // Set Kp 0.5, Ki 5 and Kd "to set the strenght of distortion of the error"
PID myPID2(&abs_duration2, &val_output2, &Setpoint2, Kp2, Ki2, Kd2, DIRECT);
void setup()
{
// tempoActive = millis() ;
Serial.begin(9600);//Initialize the serial port
// On remplit le tableau d'échantillons avec des 0 en vue de lisser la vitesse captée
for (int i = 0; i < nEchantillons; i++) {
echantillon[i] = 0;
}
/* // Configuration des ports en mode "sortie" pour moteur A
pinMode(motorPinA1, OUTPUT);
pinMode(motorPinA2, OUTPUT);
pinMode(enablePinA, OUTPUT);
*/
// Configuration des ports en mode "sortie" pour moteur B
pinMode(motorPinB1, OUTPUT);
pinMode(motorPinB2, OUTPUT);
pinMode(enablePinB, OUTPUT);
// Configuration des ports en mode "sortie" C
pinMode(motorPinC1, OUTPUT); //L298N Control port settings direction of motor C (originaly L298P)
pinMode(motorPinC2, OUTPUT); //L298N Control port settings direction of motor C
pinMode(enablePinC, OUTPUT); // powerRate to control speed of motor C
// Configuration des ports en mode "sortie" D
pinMode(motorPinD1, OUTPUT);
pinMode(motorPinD2, OUTPUT);
pinMode(enablePinD, OUTPUT);
//Originaly to Set the output value of the PID // (read between 42 and 43 pulses when setpoint=30)
Setpoint=50;
myPID.SetMode(AUTOMATIC);//PID is set to automatic mode
myPID.SetSampleTime(5);//Set PID sampling frequency was 100ms. Carefull! Put 10 or 5ms
EncoderInit(); //Initialize the module--> must be initialised with advance() or back() i guess?
//Setting the PID number 2
Setpoint2=30;
myPID2.SetMode(AUTOMATIC);//PID is set to automatic mode
myPID2.SetSampleTime(5);//Set PID sampling frequency was 100ms. Carefull! Put 10 or 5ms
EncoderInit2(); //Initialize the module--> must be initialised with advance() or back() i guess?
}
void loop()
{
// A way to program the COUNT
noInterrupts();
latestDurationCount = duration;
interrupts();
abs_duration = abs(latestDurationCount);
// Second encoder COUNT
noInterrupts();
latestDurationCount2 = duration2;
interrupts();
abs_duration2 = abs(latestDurationCount2);
// FIRST PID
result=myPID.Compute();//PID conversion is complete and returns 1
if(result)
{
duration = 0; //Count clear, wait for the next count
Serial.print("val_output"); Serial.println(val_output);// TENSION in MOTOR TO REGULATE SPEED with SETPOINT
Serial.print("Setpoint"); Serial.println(Setpoint);
}
//************************************************************************************
// SECOND PID
result2=myPID2.Compute();//PID conversion is complete and returns 1
if(result2)
{
duration2 = 0; //Count clear, wait for the next count
Serial.print("val_output2"); Serial.println(val_output2);// TENSION in MOTOR TO REGULATE SPEED with SETPOINT
void EncoderInit()
{
Direction = true;//default -> Forward
pinMode(encoder0pinB,INPUT);
attachInterrupt(0, wheelSpeed, CHANGE);
}
void EncoderInit2()
{
Direction2 = true;//default -> Forward
pinMode(encoder0pinB2,INPUT);
attachInterrupt(0, wheelSpeed2, CHANGE);
}
void wheelSpeed()
{
int Lstate = digitalRead(encoder0pinA);
if((encoder0PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder0pinB);
if(val == LOW && Direction)
{
Direction = false; //Reverse
}
else if(val == HIGH && !Direction)
{
Direction = true; //Forward
}
}
encoder0PinALast = Lstate;
if(!Direction) duration++;
else duration--;
}
void wheelSpeed2()
{
int Lstate2 = digitalRead(encoder0pinA2);
if((encoder0PinALast2 == LOW) && Lstate2==HIGH)
{
int val2 = digitalRead(encoder0pinB2);
if(val2 == LOW && Direction2)
{
Direction2 = false; //Reverse
}
else if(val2 == HIGH && !Direction2)
{
Direction2 = true; //Forward
}
}
encoder0PinALast2 = Lstate2;
if(!Direction2) duration2++;
else duration2--;
}
void advance()//Motor Forward
{
digitalWrite(motorPinC1,HIGH);
digitalWrite(motorPinC2,LOW);
analogWrite(enablePinC,val_output);
digitalWrite(motorPinD1,HIGH);
digitalWrite(motorPinD2,LOW);
analogWrite(enablePinD,val_output2);
}
void back()//Motor reverse
{
digitalWrite(motorPinC1,LOW);
digitalWrite(motorPinC2,HIGH);
analogWrite(enablePinC,val_output);
digitalWrite(motorPinD1,LOW);
digitalWrite(motorPinD2,HIGH);
analogWrite(enablePinD,val_output2);
}
void Stop()//Motor stops
{
digitalWrite(enablePinC, LOW);
digitalWrite(enablePinD, LOW);
}
Est-ce une bonne méthode?