D'accord voilà le programme
// Manipulation des ports
// PIND : RX TX 2 3 4 5 6 7
// https://www.arduino.cc/en/Reference/PortManipulation
#include <PID_v1.h> // Pour le PID
#include <TimerOne.h> // Pour le timer
// commande du moteur droit
#define ENCODEURDROITA 2
#define ENCODEURDROITB 4
#define MOTEURDROITA 6 // contrôle vitesse moteur 1; PWM
#define MOTEURDROITB 10 // controle direction moteur 1
#define MOTEURDROITC 11
// commande du moteur 2
#define ENCODEURGAUCHEA 3
#define ENCODEURGAUCHEB 7
#define MOTEURGAUCHEA 5 // contrôle vitesse moteur 2; PWM
#define MOTEURGAUCHEB 8 // controle direction moteur 2
#define MOTEURGAUCHEC 9 // controle direction moteur 2
double SetpointDroit, InputDroit, OutputDroit;
double SetpointGauche, InputGauche, OutputGauche;
double Kp = 0.8, Ki = 5, Kd = 0;
PID PIDDROIT(&InputDroit, &OutputDroit, &SetpointDroit, Kp, Ki, Kd, DIRECT);
PID PIDGAUCHE(&InputGauche, &OutputGauche, &SetpointGauche, Kp, Ki, Kd, DIRECT);
// volatile => pour toute variable qui seront utilisées dans les interruptions
volatile int8_t countDroit = 0; /* comptage de tick d'encoder qui sera incrémenté sur interruption " On change " sur l'interruption 0 max 127 attention à l'overflow*/
volatile int8_t countGauche = 0; /* comptage de tick d'encoder qui sera incrémenté sur interruption " On change " sur l'interruption 1 max 127 attention à l'overflow */
volatile double speedDroit = 0; // vitesse du moteur
volatile double speedGauche = 0; // vitesse du moteur
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 = 0; //lecture de la consigne via le moniteur serie pour test, initialisé à 40 pour test rapide du PID
void setup()
{
//initialisation moniteur serie
Serial.begin(9600); // facultatif uniquement pour feedback
Serial.println("cool");
// on initialise les entrées et sorties
pinMode(ENCODEURDROITA, INPUT_PULLUP);
pinMode(ENCODEURDROITB, INPUT_PULLUP);
pinMode(ENCODEURGAUCHEA, INPUT_PULLUP);
pinMode(ENCODEURGAUCHEB, INPUT_PULLUP);
pinMode(MOTEURDROITA, OUTPUT);
pinMode(MOTEURDROITB, OUTPUT);
pinMode(MOTEURDROITC, OUTPUT);
pinMode(MOTEURGAUCHEA, OUTPUT);
pinMode(MOTEURGAUCHEB, OUTPUT);
pinMode(MOTEURGAUCHEC, OUTPUT);
// moteurs à l'arret
stopMotors();
// Encoder quadrature counter
attachInterrupt(0, counterDroit, CHANGE); // lorsqu'il y une modification sur le pin 2, on interrompte le programme pour enclencher le comptage
attachInterrupt(1, counterGauche, CHANGE); // lorsqu'il y une modification sur le pin 3, on interrompte le programme pour enclencher le comptage
Timer1.initialize(100);//80000 // set a timer of length 0.08 sec => permet d'avoir speed le plus proche de 127 sans dépasser 127 en vitesse max !
Timer1.attachInterrupt( timerIsr );
// PID
InputDroit = 0;
SetpointDroit = 0;
InputGauche = 0;
SetpointGauche = 0;
//active le PID
PIDDROIT.SetMode(AUTOMATIC);
PIDDROIT.SetOutputLimits(-255, 255);
PIDGAUCHE.SetMode(AUTOMATIC);
PIDGAUCHE.SetOutputLimits(-255, 255);
}
void loop() {
//while(1){test();}
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) {
runMotorSpeed(valeur,valeur);
}
else {
stopMotors();
}
timer = millis();
if ( (timer - time0) > 90000) { //Timeout !
valeur = 0 ;
stopMotors();
time0 = timer;
}
Serial.println("**************************speed1");
Serial.println(speedDroit*100); // ne voit pas la speed evolué
Serial.println("**************************speed2");
Serial.println(speedGauche*100); // just 100 ou -100
Serial.println("'''''''''''''''''"); // à commenter, utilisé pour débug
}
void counterDroit()
{
byte state = PIND;
(((state >> ENCODEURDROITA) & 1) ^ ((state >> ENCODEURDROITB) & 1)) ? countDroit-- : countDroit++; // régler ++ et -- pour que faire avancer => +
}
// Encoder counter 2
void counterGauche()
{
byte state = PIND;
(((state >> ENCODEURGAUCHEA) & 1 ) ^ ((state >> ENCODEURGAUCHEB) & 1)) ? countGauche++ : countGauche--; // régler ++ et -- pour que faire avancer => +
}
// Timer pour calculer la vitesse grace aux encodeurs
void timerIsr()
{
speedDroit = countDroit;
countDroit = 0;
speedGauche = countGauche;
countGauche = 0;
}
void stopMotors() {
digitalWrite(MOTEURDROITA, HIGH);
digitalWrite(MOTEURDROITB, HIGH);
digitalWrite(MOTEURGAUCHEA, HIGH);
digitalWrite(MOTEURGAUCHEB, HIGH);
}
void test() {
digitalWrite(MOTEURDROITA, LOW);
digitalWrite(MOTEURDROITB, LOW); // low fait avancer
digitalWrite(MOTEURGAUCHEA, HIGH);
digitalWrite(MOTEURGAUCHEB, LOW); // Low fait reculer
Serial.println("speed1");
Serial.println(speedDroit*100); // à commenter, utilisé pour débug
Serial.println("speed2");
Serial.println(speedGauche*100); // à commenter, utilisé pour débug
Serial.println("'''''''''''''''''"); // à commenter, utilisé pour débug
}
void avancerMoteurDroit(uint8_t a) // En avant
{
analogWrite (MOTEURDROITA, a); // Contrôle de vitesse en PWM, moteur 1
digitalWrite(MOTEURDROITB, LOW); // sens de marche avec HIGH
digitalWrite(MOTEURDROITC, HIGH); // sens de marche avec HIGH
}
void avancerMoteurGauche(uint8_t a) // En avant
{
analogWrite (MOTEURGAUCHEA, 255 - a); // Contrôle de vitesse en PWM, moteur 2
digitalWrite(MOTEURGAUCHEB, HIGH);
digitalWrite(MOTEURGAUCHEC, LOW);
}
void reculerMoteurDroit (uint8_t a) // En arrière
{
analogWrite (MOTEURDROITA, 255 - a);
digitalWrite(MOTEURDROITB, HIGH);
digitalWrite(MOTEURDROITC, LOW);
}
void reculerMoteurGauche (uint8_t a) // En arrière
{
analogWrite (MOTEURGAUCHEA, a);
digitalWrite(MOTEURGAUCHEB, LOW);
digitalWrite(MOTEURGAUCHEC, HIGH);
}
void runMotorSpeed( int8_t commandeMoteurDroit, int8_t commandeMoteurGauche) {
SetpointDroit = commandeMoteurDroit;
SetpointGauche = commandeMoteurGauche;
InputDroit = speedDroit;
InputGauche = speedGauche;
PIDDROIT.Compute();
PIDGAUCHE.Compute();
int outputDroit = (int)OutputDroit;
int outputGauche = (int)OutputGauche;
//utilisation de la sortie du PID pour asservir les moteurs
if (outputDroit >= 0) {
avancerMoteurDroit(outputDroit);
}
if (outputDroit < 0) {
reculerMoteurDroit(-outputDroit);
}
if (outputGauche >= 0) {
avancerMoteurGauche(outputGauche);
}
if (outputGauche < 0) {
reculerMoteurGauche(-outputGauche);
}
// à commenter, utilisé pour débug
Serial.println("output1");
Serial.println(outputDroit);
Serial.println("output2");
Serial.println(outputGauche);
Serial.println("'''''''''''''''''");
Serial.println("-------------------speed1");
Serial.println(InputDroit); // à commenter, utilisé pour débug
Serial.println("-------------------speed2");
Serial.println(speedGauche); // à commenter, utilisé pour débug
Serial.println("'''''''''''''''''");
}
Je n'arrive pas à voir la speed1 ou speed2 évolue, je vois seulement 100 ou -100 sur l'un ou l'autre des moteurs quand je tape -10 ou 10.
Merci de cette réponse rapide.
