Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir
Posté 09 mai 2018 - 02:58
Et oui la Coupe de France de Robotique c'est aujourd'hui ... Oui oui il faut y aller dans quoi ... 10H environ ? Un peu plus ... Mais bon le temps d'y aller...
Bref le challenge :
Réaliser un petit robot homologable dans ce laps de temps !
Bon on va être honnête on ne va pas partir de rien x) ça serait un chouia trop long x) et on va prendre un sacré raccourci en partant du kit robot arduino 2WD !
( ça tombe bien je devais préparer du code pour ce kit et je devais faire un tuto sur comment faire un robot pour la Coupe de France de Robotique alors c'est parti ! )
Bref => On fait chauffer l'imprimante !
Bon après le kit c'est bien, ça facilite une partie de la méca... mais tout seul c'est loin d'être suffisant pour être homologué pour la coupe ! Il faut se déplacer certes, mais il faut aussi marquer des points, ne pas rentrer dans les autres robots et avoir un bouton d'arrêt d'urgence !
Challenge accepté !
Promis je ferai un blog sur le sujet et je posterais le code au propre plus tard mais là j'ai pas le temps ^^
au passage à l'occasion faudra que je me filme en train de souder ces broches histoire de montrer à quel point c'est facile et rapide =) surtout quand on utilise l'astuce des barrettes sécables femelles !
en prenant celui fournit sur le tuto pour contrôle PID d'un moteur CC équipé d'un encodeur en quadrature avec arduino et en ajoutant un deuxième moteur judicieusement, nous voilà maintenant partis pour du code !
En modifiant légèrement le code du tuto pour contrôler mes deux moteurs et en ajustant les paramètres du timer et des coefficients du PID voilà ce que ça donne comme code ! ( Au passage faudra que je dépoussière ce tuto à l'occasion! =) )
Code de test pour paramétrer rapidement au jugé le PID de deux moteurs CC en utilisant le moniteur série pour envoyer une vitesse de consigne.
// 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 5 // contrôle vitesse moteur 1; PWM
#define MOTEURDROITB 8 // controle direction moteur 1
// commande du moteur 2
#define ENCODEURGAUCHEA 3
#define ENCODEURGAUCHEB 7
#define MOTEURGAUCHEA 6 // contrôle vitesse moteur 2; PWM
#define MOTEURGAUCHEB 9 // controle direction moteur 2
double SetpointDroit, InputDroit, OutputDroit;
double SetpointGauche, InputGauche, OutputGauche;
double Kp = 0.8, Ki = 5, Kd = 0.1;
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 = 40; //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(MOTEURGAUCHEA, OUTPUT);
pinMode(MOTEURGAUCHEB, 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(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
}
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); // à commenter, utilisé pour débug
Serial.println("speed2");
Serial.println(speedGauche); // à commenter, utilisé pour débug
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); // à commenter, utilisé pour débug
Serial.println("speed2");
Serial.println(speedGauche); // à 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
}
void avancerMoteurGauche(uint8_t a) // En avant
{
analogWrite (MOTEURGAUCHEA, 255 - a); // Contrôle de vitesse en PWM, moteur 2
digitalWrite(MOTEURGAUCHEB, HIGH);
}
void reculerMoteurDroit (uint8_t a) // En arrière
{
analogWrite (MOTEURDROITA, 255 - a);
digitalWrite(MOTEURDROITB, HIGH);
}
void reculerMoteurGauche (uint8_t a) // En arrière
{
analogWrite (MOTEURGAUCHEA, a);
digitalWrite(MOTEURGAUCHEB, LOW);
}
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("'''''''''''''''''");
}
Plus d'explications pourront être données dans le blog ... Si vous avez des questions n'hésitez pas!
Je pense que je pourrais tuner encore un peu mieux le PID en utilisant le plotter série pour afficher la courbe de la vitesse... mais là j'ai pas le code sous la main ... Pas le temps pour ça tout de suite je pense ... On fera le tuto plus tard pour ça ! Voir à la coupe si j'ai fini tout le reste ! x)
Bon je peux pas encore faire rouler le robot, le châssis s'imprime toujours ... Et puis maintenant au-delà de la vitesse il faudrait "intégrer " => La position !
Comprendra qui pourra le jeu de mot pourri de 4H35 du matin ! x)
Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir
Posté 09 mai 2018 - 05:44
Les news : à peine le châssis imprimé voici que les roues sont montées et une première version de l'électronique, "volante " sur des fils a été montée sur le châssis, Pour le moment juste une batterie et un bouton pour alimenter le tout.... avec sa batterie Lipo et son alarme !
Petite photo sans retouches x) ( pas le temps) :
Bref juste de quoi faire les premiers tours de roue en mode show off devant Briel ! =)
La vidéo :
J'espère qu'elle est bien cadrée j'ai même pas pris le temps de la regarder ! x)
C'est pas une foudre de guerre mais c'est pas mal pour une vitesse à 33% ! =)
Va falloir continuer le code qui n'a pas tant que ça avancé ...
On voit apparaître les coordonnées du robot ... et des formules ça mérite un petit SnapShot même si c'est possible qu'une erreur soit glissée dedans !
Gardez en tête que c'est pas utilisable en l'état x) mais bon c'est pour montrer un peu comment ça bouge !
// Manipulation des ports
// PIND : RX TX 2 3 4 5 6 7
// https://www.arduino.cc/en/Reference/PortManipulation
// général
//IO
#define COULEUR 10
#define GO 11
bool couleur;
#define VERT 0
#define ORANGE 1
// asservissement en vitesse des moteurs
#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 5 // contrôle vitesse moteur 1; PWM
#define MOTEURDROITB 8 // controle direction moteur 1
// commande du moteur 2
#define ENCODEURGAUCHEA 3
#define ENCODEURGAUCHEB 7
#define MOTEURGAUCHEA 6 // contrôle vitesse moteur 2; PWM
#define MOTEURGAUCHEB 9 // controle direction moteur 2
double SetpointDroit, InputDroit, OutputDroit;
double SetpointGauche, InputGauche, OutputGauche;
double Kp = 0.8, Ki = 5, Kd = 0.1;
PID PIDDROIT(&InputDroit, &OutputDroit, &SetpointDroit, Kp, Ki, Kd, DIRECT);
PID PIDGAUCHE(&InputGauche, &OutputGauche, &SetpointGauche, Kp, Ki, Kd, DIRECT);
// volatile => pour toute variable qui sera utilise 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 */
volatile int8_t countGauche = 0; /* comptage de tick d'encoder qui sera incrémenté sur interruption " On change " sur l'interruption 1 */
volatile double speedDroit = 0; // vitesse du moteur
volatile double speedGauche = 0; // vitesse du moteur
// asservissement en position du robot
#define X0ORANGE 90
#define X0VERT (3000 - 90)
#define THETA0ORANGE 0
#define THETA0VERT PI
#define Y0 100
#define PPR 22
#define GEARBOXRATIO 21
#define ROUEPPR (PPR * GEARBOXRATIO)
#define DIAMETREROUE 65 // en mm
#define PERIMETREROUE ( PI * DIAMETREROUE )
#define ENTRAXE 160
volatile int32_t x, y; // variable en mm
volatile float theta; // variable radian
int32_t cibleX, cibleY;
double SetpointLineaire, InputLineaire, OutputLineaire;
double SetpointAngulaire, InputAngulaire, OutputAngulaire;
double KpL = 0.8, KiL = 5, KdL = 0.1;
double KpA = 0.8, KiA = 5, KdA = 0.1;
PID PIDLINEAIRE (&InputLineaire, &OutputLineaire, &SetpointLineaire, KpL, KiL, KdL, DIRECT);
PID PIDANGULAIRE(&InputAngulaire, &OutputAngulaire, &SetpointAngulaire, KpA, KiA, KdA, 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 = 40; //lecture de la consigne via le moniteur serie
void setup()
{
//initialisation moniteur serie
Serial.begin(9600); // facultatif uniquement pour feedback
// 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(MOTEURGAUCHEA, OUTPUT);
pinMode(MOTEURGAUCHEB, OUTPUT);
pinMode(COULEUR, INPUT);
pinMode(GO, INPUT);
// 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(80000); // set a timer of length 100000 microseconds = 0.1 sec - or 10Hz
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);
y = Y0;
if( digitalRead(COULEUR) ) { // On est orange
x= X0ORANGE;
theta = THETA0ORANGE;
}
else {
x= X0VERT;
theta = THETA0VERT;
}
}
void loop() {
//while(1){test();}
if (Serial.available()) {
time0 = timer;
valeur = Serial.parseInt(); //récupération des caractères sur le port série
}
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); // à commenter, utilisé pour débug
Serial.println("speed2");
Serial.println(speedGauche); // à commenter, utilisé pour débug
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()
{
// asservissement en vitesse
speedDroit = countDroit;
countDroit = 0;
speedGauche = countGauche;
countGauche = 0;
//asservissement en position
x += ((speedDroit + speedGauche) * PERIMETREROUE * cos(theta)) / ROUEPPR ; // variable en mm
y += ((speedDroit + speedGauche) * PERIMETREROUE * sin(theta)) / ROUEPPR ;
theta += atan2((speedDroit - speedGauche) * PERIMETREROUE / (ROUEPPR ), ENTRAXE); // Peu précis préférer utiliser un Imu si on a le temps ...
if( theta> 2* PI) theta -= 2 * PI;
else if (theta < 0) theta += 2 * PI;
}
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); // à commenter, utilisé pour débug
Serial.println("speed2");
Serial.println(speedGauche); // à 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
}
void avancerMoteurGauche(uint8_t a) // En avant
{
analogWrite (MOTEURGAUCHEA, 255 - a); // Contrôle de vitesse en PWM, moteur 2
digitalWrite(MOTEURGAUCHEB, HIGH);
}
void reculerMoteurDroit (uint8_t a) // En arrière
{
analogWrite (MOTEURDROITA, 255 - a);
digitalWrite(MOTEURDROITB, HIGH);
}
void reculerMoteurGauche (uint8_t a) // En arrière
{
analogWrite (MOTEURGAUCHEA, a);
digitalWrite(MOTEURGAUCHEB, LOW);
}
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("'''''''''''''''''");
}
Prochaine étape faire l'asservissement en position et le tester !
Même si c'est pas super on s'arrêtera à un premier résultat utilisable
Et oui pour être homologable il faut
+> Le bouton d'arrêt d'urgence et de l'évitement ! x)
Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir
Posté 09 mai 2018 - 08:44
Bah, être homologué en poussant des cubes ! ^^ Le truc le plus bateau...
ça avance ça avance le robot commence à avoir un soupçon d'intelligence pour aller au point demandé ...
Petite vidéo " trichée" où je lui demande juste d'aller à un point qui est devant lui en corrigeant son angle si il dévie ...
Pas encore eu le temps de régler quoi que ce soit ...
Je pense qu'il faudrait une IMU pour l'angle ...
En attendant la 3 ème pièce imprimée vient d'être sortie :
c'est le dessus du robot qui permet de tenir le bouton d'arrêt d'urgence ainsi que la petite carte relais déjà câblée pour pouvoir couper le robot ! =)
le schémas fait à la va vite sous paint qui va avec :
Je vous laisse deviner comment ça marche Sachant que le but c'est de couper les moteurs lorsqu'on appuie sur le bouton d'arrêt d'urgence =)
Je pense qu'il est difficile de faire plus simple et efficace...
pour info le diamètre que j'ai fais pour ce bouton c'est du 22mm mais on peut faire un petit peu plus gros
Le code qui a beaucoup de points à revoir et qui est actuellement sur le robot ... => attention c'est juste un SnapShot il est pas utilisable en l'état ! ça permet d'avoir un aperçu du code en mode brouillon à un instant t =)
// Manipulation des ports
// PIND : RX TX 2 3 4 5 6 7
// https://www.arduino.cc/en/Reference/PortManipulation
// général
//IO
#define COULEUR 10
#define GO 11
bool couleur;
#define VERT 0
#define ORANGE 1
// évitement :
#include <NewPing.h>
#define TRIGGER_PIN 13 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 12 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 100 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
// asservissement en vitesse des moteurs
#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 5 // contrôle vitesse moteur 1; PWM
#define MOTEURDROITB 8 // controle direction moteur 1
// commande du moteur 2
#define ENCODEURGAUCHEA 3
#define ENCODEURGAUCHEB 7
#define MOTEURGAUCHEA 6 // contrôle vitesse moteur 2; PWM
#define MOTEURGAUCHEB 9 // controle direction moteur 2
double SetpointDroit, InputDroit, OutputDroit;
double SetpointGauche, InputGauche, OutputGauche;
double Kp = 0.8, Ki = 5, Kd = 0.1;
PID PIDDROIT(&InputDroit, &OutputDroit, &SetpointDroit, Kp, Ki, Kd, DIRECT);
PID PIDGAUCHE(&InputGauche, &OutputGauche, &SetpointGauche, Kp, Ki, Kd, DIRECT);
// volatile => pour toute variable qui sera utilise 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 */
volatile int8_t countGauche = 0; /* comptage de tick d'encoder qui sera incrémenté sur interruption " On change " sur l'interruption 1 */
volatile double speedDroit = 0; // vitesse du moteur
volatile double speedGauche = 0; // vitesse du moteur
// asservissement en position du robot
#define X0ORANGE 90
#define X0VERT (3000 - 90)
#define THETA0ORANGE 0
#define THETA0VERT 0 // PI
#define Y0 0//100
#define PPR 22
#define GEARBOXRATIO 21
#define ROUEPPR (PPR * GEARBOXRATIO)
#define DIAMETREROUE 65 // en mm
#define PERIMETREROUE ( PI * DIAMETREROUE )
#define ENTRAXE 160
#define TOLERANCE 20 // 2cm de tolérance pour le positionnement => Mettre une tolerance pour éviter les oscillation autour de la positon d'arrivée ...
#define TOLERANCETHETA PI/10
volatile int32_t x, y; // variable en mm
volatile float theta; // variable radian
int32_t cibleX, cibleY;
double SetpointLineaire, InputLineaire, OutputLineaire;
double SetpointAngulaire, InputAngulaire, OutputAngulaire;
double KpL = 0.015, KiL = 0.000, KdL = 0.00;
double KpA = 2, KiA = 0, KdA = 0;
PID PIDLINEAIRE (&InputLineaire, &OutputLineaire, &SetpointLineaire, KpL, KiL, KdL, DIRECT);
PID PIDANGULAIRE(&InputAngulaire, &OutputAngulaire, &SetpointAngulaire, KpA, KiA, KdA, 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 = 40; //lecture de la consigne via le moniteur serie
void setup()
{
//initialisation moniteur serie
Serial.begin(9600); // facultatif uniquement pour feedback
// 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(MOTEURGAUCHEA, OUTPUT);
pinMode(MOTEURGAUCHEB, OUTPUT);
pinMode(COULEUR, INPUT);
pinMode(GO, INPUT);
// 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(80000); // set a timer of length 100000 microseconds = 0.1 sec - or 10Hz
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);
PIDLINEAIRE.SetMode(AUTOMATIC);
PIDLINEAIRE.SetOutputLimits(-60, 60);
PIDANGULAIRE.SetMode(AUTOMATIC);
PIDANGULAIRE.SetOutputLimits(-15, 15);
y = Y0;
if( digitalRead(COULEUR) ) { // On est orange
x= X0ORANGE;
theta = THETA0ORANGE;
}
else {
x= X0VERT;
theta = THETA0VERT;
}
/* while(!go) {
time0 = millis();
go = digitalRead(GO);
}*/
}
void loop() {
rotation(PI);
stopMotors();
while(1);
//while(1){test();} // test
if (Serial.available()) { // test
time0 = timer;
valeur = Serial.parseInt(); //récupération des caractères sur le port série
}
//goToPosition(1500, 0);
/* If obstacle
{
stopMotors();
}
*/
timer = millis();
if ( (timer - time0) > 90000) { //Timeout !
valeur = 0 ;
stopMotors();
time0 = timer;
}
Serial.println("speed1");
Serial.println(speedDroit); // à commenter, utilisé pour débug
Serial.println("speed2");
Serial.println(speedGauche); // à commenter, utilisé pour débug
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()
{
// asservissement en vitesse
speedDroit = countDroit;
countDroit = 0;
speedGauche = countGauche;
countGauche = 0;
//asservissement en position
x += ((speedDroit + speedGauche) * PERIMETREROUE * cos(theta)) / ROUEPPR ; // variable en mm
y += ((speedDroit + speedGauche) * PERIMETREROUE * sin(theta)) / ROUEPPR ;
theta += atan2((speedDroit - speedGauche) * PERIMETREROUE / (ROUEPPR ), ENTRAXE); // Peu précis préférer utiliser un Imu si on a le temps ...
if( theta> 2* PI) theta -= 2 * PI;
else if (theta < 0) theta += 2 * PI;
}
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); // à commenter, utilisé pour débug
Serial.println("speed2");
Serial.println(speedGauche); // à 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
}
void avancerMoteurGauche(uint8_t a) // En avant
{
analogWrite (MOTEURGAUCHEA, 255 - a); // Contrôle de vitesse en PWM, moteur 2
digitalWrite(MOTEURGAUCHEB, HIGH);
}
void reculerMoteurDroit (uint8_t a) // En arrière
{
analogWrite (MOTEURDROITA, 255 - a);
digitalWrite(MOTEURDROITB, HIGH);
}
void reculerMoteurGauche (uint8_t a) // En arrière
{
analogWrite (MOTEURGAUCHEA, a);
digitalWrite(MOTEURGAUCHEB, LOW);
}
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("'''''''''''''''''");
}
bool goToPosition(int32_t xCible, int32_t yCible) {
int32_t deltaX = xCible - x;
int32_t deltaY = yCible - y;
int8_t commandeMoteurDroit = 0;
int8_t commandeMoteurGauche = 0;
float deltaAngle;
SetpointLineaire = sqrt( deltaX * deltaX + deltaY * deltaY);
SetpointAngulaire = atan2(deltaY , deltaX);
deltaAngle = SetpointAngulaire - theta;
InputLineaire = 0;
InputAngulaire = theta;
PIDLINEAIRE.Compute();
PIDANGULAIRE.Compute();
Serial.println("setpoint Lineaire"); // à commenter, utilisé pour débug
Serial.println(SetpointLineaire); // à commenter, utilisé pour débug
Serial.println("setpoint Angulaire"); // à commenter, utilisé pour débug
Serial.println(SetpointAngulaire); // à commenter, utilisé pour débug
Serial.println("'''''''''''''''''"); // à commenter, utilisé pour débug
Serial.println("x");
Serial.println(x); // à commenter, utilisé pour débug
Serial.println("y");
Serial.println(y); // à commenter, utilisé pour débug
Serial.println("Angle");
Serial.println(theta); // à commenter, utilisé pour débug
Serial.println("'''''''''''''''''"); // à commenter, utilisé pour débug
Serial.println("deltax");
Serial.println(deltaX); // à commenter, utilisé pour débug
Serial.println("deltay");
Serial.println(deltaY); // à commenter, utilisé pour débug
Serial.println("deltaAngle");
Serial.println(deltaAngle); // à commenter, utilisé pour débug
Serial.println("'''''''''''''''''"); // à commenter, utilisé pour débug
if( abs(deltaAngle) < TOLERANCETHETA ) {
commandeMoteurDroit = (int)(OutputLineaire + OutputAngulaire);
commandeMoteurGauche = (int)(OutputLineaire - OutputAngulaire);
runMotorSpeed( commandeMoteurDroit, commandeMoteurGauche);
}
else {
commandeMoteurDroit = (int)( OutputAngulaire);
commandeMoteurGauche = (int)(- OutputAngulaire);
runMotorSpeed( commandeMoteurDroit, commandeMoteurGauche);
}
if( abs(deltaX) < TOLERANCE && abs(deltaY) < TOLERANCE)
return 1;
return 0;
}
void rotation(float angleConsigne){
while(1){
SetpointAngulaire = angleConsigne;
InputAngulaire = theta;
PIDANGULAIRE.Compute();
runMotorSpeed((int)( OutputAngulaire), (int)(- OutputAngulaire));
Serial.println("setpoint Angulaire"); // à commenter, utilisé pour débug
Serial.println(SetpointAngulaire); // à commenter, utilisé pour débug
Serial.println("Angle");
Serial.println(theta); // à commenter, utilisé pour débug
Serial.println("'''''''''''''''''"); // à commenter, utilisé pour débug
Serial.println("Angleconsigne");
Serial.println(angleConsigne); // à commenter, utilisé pour débug
Serial.println("'''''''''''''''''"); // à commenter, utilisé pour débug
}
}
Bon maintenant je commence un peu à préparer les sacs ...
C'était pas un bon plan de relever le défi ! x)
Mais oui ça va je suis large ! J'ai qu'une heure de route à faire ! ^^
Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir
Posté 09 mai 2018 - 12:29
Bon bon quand faut y aller il faut y aller !
Le robot est pas complètement fini , j'ai mis de côté le déplacement pour le moment afin de mettre les éléments nécessaire pour être homologué !
Petite photo vite fait :
On retrouve donc la batterie dans son sac ignifuge, un capteur à ultrason pour éviter la colision frontal ... Bon ok le robot n'a qu'un pauvre capteur et donc il ne fera que avancer ... mais bon on a fait avec le délais impartis...
Après vous remarquerez que le robot n'est bien entendu pas fini ^^ Il sera fini sur place il reste encore deux trois trucs à câbler dont la carte qui permet de faire démarrer le robot ( et qui indique aussi au robot de quel côté il est )
Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir
Posté 10 mai 2018 - 09:03
Bon finalement le robot a été adopté par l'équipe ESTIA SYSTEM qui a eu quelques soucis avec son robot et le robot a été réellement homologué en tant que robot secondaire hier avant minuit ! =) Il participera à son premier match demain ! Et visiblement son arme de prédilection c'est la paire de ciseau !
Du coup moi je ne touche plus au robot ! Et on espère que le robot principal de l'équipe sera rapidement de nouveau sur roue Je leur souhaite bon courage
Le code d'homologation :
// Manipulation des ports
// PIND : RX TX 2 3 4 5 6 7
// https://www.arduino.cc/en/Reference/PortManipulation
// général
//IO
#define COULEUR 10
#define GO 11
bool couleur;
#define VERT 0
#define ORANGE 1
// évitement :
#include <NewPing.h>
#define TRIGGER_PIN 13 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 12 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 100 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
// asservissement en vitesse des moteurs
#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 5 // contrôle vitesse moteur 1; PWM
#define MOTEURDROITB 8 // controle direction moteur 1
// commande du moteur 2
#define ENCODEURGAUCHEA 3
#define ENCODEURGAUCHEB 7
#define MOTEURGAUCHEA 6 // contrôle vitesse moteur 2; PWM
#define MOTEURGAUCHEB 9 // controle direction moteur 2
double SetpointDroit, InputDroit, OutputDroit;
double SetpointGauche, InputGauche, OutputGauche;
double Kp = 0.8, Ki = 5, Kd = 0.1;
PID PIDDROIT(&InputDroit, &OutputDroit, &SetpointDroit, Kp, Ki, Kd, DIRECT);
PID PIDGAUCHE(&InputGauche, &OutputGauche, &SetpointGauche, Kp, Ki, Kd, DIRECT);
// volatile => pour toute variable qui sera utilise 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 */
volatile int8_t countGauche = 0; /* comptage de tick d'encoder qui sera incrémenté sur interruption " On change " sur l'interruption 1 */
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 = 50; //lecture de la consigne via le moniteur serie
bool go = 0;
uint16_t obstacle = 0;
void setup()
{
//initialisation moniteur serie
Serial.begin(9600); // facultatif uniquement pour feedback
// 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(MOTEURGAUCHEA, OUTPUT);
pinMode(MOTEURGAUCHEB, OUTPUT);
pinMode(COULEUR, INPUT);
pinMode(GO, INPUT);
// 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(80000); // set a timer of length 100000 microseconds = 0.1 sec - or 10Hz
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);
while(!go) {
time0 = millis();
go = digitalRead(GO);
}
}
void loop() {
if (Serial.available()) { // test
time0 = timer;
valeur = Serial.parseInt(); //récupération des caractères sur le port série
}
obstacle = sonar.ping_cm();
if( obstacle==0 || obstacle > 10 ) { // le capteur retourne zéro si il n'y a pas d'obstacle ...
runMotorSpeed(valeur,valeur);
}
else {
valeur = 0 ;
stopMotors();
}
timer = millis();
if ( (timer - time0) > 90000) { //Timeout !
valeur = 0 ;
stopMotors();
time0 = timer;
}
Serial.println("speed1");
Serial.println(speedDroit); // à commenter, utilisé pour débug
Serial.println("speed2");
Serial.println(speedGauche); // à commenter, utilisé pour débug
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()
{
// asservissement en vitesse
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); // à commenter, utilisé pour débug
Serial.println("speed2");
Serial.println(speedGauche); // à 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
}
void avancerMoteurGauche(uint8_t a) // En avant
{
analogWrite (MOTEURGAUCHEA, 255 - a); // Contrôle de vitesse en PWM, moteur 2
digitalWrite(MOTEURGAUCHEB, HIGH);
}
void reculerMoteurDroit (uint8_t a) // En arrière
{
analogWrite (MOTEURDROITA, 255 - a);
digitalWrite(MOTEURDROITB, HIGH);
}
void reculerMoteurGauche (uint8_t a) // En arrière
{
analogWrite (MOTEURGAUCHEA, a);
digitalWrite(MOTEURGAUCHEB, LOW);
}
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("'''''''''''''''''");
}
Comme vous pourrez le constater ça a un peut taillé dans le gras ^^
Le robot attend son signal de départ puis avance tout droit à environ 40% de sa vitesse, et dégomme un réservoir avec sa paire de ciseaux et puis s'arrête et il s'arrête si on embête son capteur ultrason en chemin ! =)
C'est pas grande chose mais c'est suffisant pour être homologué ! =)
Les Karibous sont une des équipes sponsorisées pas Robot Maker, ( Même si en tout honnêteté on ne leur a pas fournis grand chose ... ) J'espère qu'on pourra faire un peu plus l'an prochain
Il en sont où dans la course ?
Pour tout te dire je ne sais même pas il faut que je check sur internet... De ce que je sais Estia System n'est pas allé très loin niveau des points... Une fois la décision prise par l'équipe que suite aux problèmes rencontrés ils souhaitaient utiliser le robot pour marquer des points je n'y ai plus touché =)
Les Karibous eux sont beaucoup mieux placés
Et puis comme je tenais tout seul mon petit stand ... Avec Briel qui faisait des pas de danse =), j'ai pas eu beaucoup de temps =)
D'ailleurs promis je poste des vidéos bientôt de BRIEL ! =) ( Mais ceux qui étaient présents ont pu voir le robot faire sa danse )
Je suis en train de tester l'asservissement PID sur mon robot et ca a l'air de fonctionner au premier abord .. le robot avance droit
Dans le code d'asservissement ci-dessous , je ne comprends pas bien le fonctionnement en cas de "output >0" en sortie de PID avec le passage de variable négative " -a" interprétée ensuite en "255-a " comme nouvelle valeur de PWM ?
void runMotorSpeed( int8_t commandeMoteurDroit, int8_t commandeMoteurGauche) {
................
//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);
}
}
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
}
void avancerMoteurGauche(uint8_t a) // En avant
{
analogWrite (MOTEURGAUCHEA, 255 - a); // Contrôle de vitesse en PWM, moteur 2
digitalWrite(MOTEURGAUCHEB, HIGH);
}
void reculerMoteurDroit (uint8_t a) // En arrière
{
analogWrite (MOTEURDROITA, 255 - a);
digitalWrite(MOTEURDROITB, HIGH);
}
void reculerMoteurGauche (uint8_t a) // En arrière
{
analogWrite (MOTEURGAUCHEA, a);
digitalWrite(MOTEURGAUCHEB, LOW);
}
Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir
Posté 27 octobre 2018 - 09:53
Cela est lié à l'usage du driver de moteur L9110.
Ce driver de moteur n'a que 2 broches par moteur pour piloter la rotation du moteur qu'on va noter PINA et PINB.
Pour tourner dans le sens1 on va mettre PINA à 1 et PINB à 0.
Pour tourner dans l'autre sens, le sens2 on va mettre PINA à 0 et PINB à 1.
Pour s'arrêter deux positions fonctionnes : PINA et B à la même valeur, soit 0 soit 1.
Dans notre cas on ne va gérer le PWM que sur 1 des pins choisi arbitrairement : Le PINA
En "PWM" on fait digitalWrite(PINA, pwm); avec pwm entre 0 et 255. avec 1 équivalent à pwm = 255. et 0 à pwm = 0
Pour aller à fond dans le sens1 il faut donc mettre digitalWrite(PINA, 255); avec PINB à 0
Pour aller à fond dans le sens2 il faut donc mettre digitalWrite(PINA, 0); avec PINB à 1
Hors /
Pour aller à fond dans le sens 1 le PID va retourner 255,
Pour aller à une vitesse nulle le PID va retourner 0
Pour aller à fond en sens inverse le PID va retourner -255 ...
Donc on va utiliser la valeur absolue de la vitesse retourner par le PID pour savoir à quelle vitesse on règle le système et le signe pour savoir dans quel sens on tourne.
Le fait que dans le sens 2 on est à l'arrêt avec 255 et à fond à 0 et donc on fait : (255 - vitesse) avec vitesse = 0 qand on veut s'arrêter et vitesse = 255 pour aller à fond ...
j'ai été suffisament clair ou pas ?
Dans tous les cas ce que je préconise c'est de faire les fonctions avancer / reculer pour chacun des moteurs, puis de faire une fonction qui va appeler ces 4 fonctions.
Si jamais tu changes de driver ou autre par la suite tu auras juste à changer ces fonctions bas niveau.
L'adaptation pour le driver moteur Arduino ( Moteur A = moteur Gauche et Moteur B = Moteur Droit )
Le shield driver deux moteurs Arduino n’a qu’une seule broche par moteur pour piloter la rotation du moteur , notées DIRA et DIRB.
La vitesse est gérée par la broche PWM de chaque moteur soit PWMA et PWMB
Pour avancer , on met DIRA à HIGH et DIRB à LOW (du fait que le moteur droit a le sens de rotation inversé par au moteur gauche ).
Pour reculer , on met DIRA à LOW et DIRB à HIGH
Pour s'arrêter , on met PWM à 0.
Pour aller à fond , on met PWM à 255
En "PWM" , pour le moteur A , on fait digitalWrite(DIRA, pwm); avec pwm entre 0 et 255.
Pour aller à fond en avant il faut donc mettre digitalWrite(PWMA, 255); avec DIRA à HIGH
Pour aller à fond en arrière il faut donc mettre digitalWrite(PWMA, 255); avec DIRA à LOW
pour le moteur B , on fait digitalWrite(DIRB , pwm); avec pwm entre 0 et 255.
Pour aller à fond en avant il faut donc mettre digitalWrite(PWMA, 255); avec DIRA à LOW
Pour aller à fond en arrière il faut donc mettre digitalWrite(PWMA, 255); avec DIRA à HIGH
Hors /
Pour aller à fond dans le sens avant le PID va retourner 255,
Pour aller à une vitesse nulle le PID va retourner 0
Pour aller à fond en sens arrière le PID va retourner -255 ...
Donc
on va utiliser la valeur absolue de la vitesse retourner par le PID pour savoir à quelle vitesse on règle le système et le signe pour savoir dans quel sens on tourne.
Dans les 2 sens avant et arrière , on est à l'arrêt avec pwm=0 , à fond à 255 .
L’adaptation de ton code au driver moteur Arduino donne ceci :
/////////////////////////////////////////////////////////
void runMotorSpeed( int commandeMoteurDroit, int commandeMoteurGauche) {
……..
//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);
}
………
void avancerMoteurDroit(int a) // En avant
{
analogWrite (PINMOTEURDROITPWMB, a); // Contrôle de vitesse en PWM, moteur 1
digitalWrite(PINMOTEURDROITDIRB, HIGH); // sens de marche avant avec HIGH
}
void avancerMoteurGauche(int a) // En avant
{
analogWrite (PINMOTEURGAUCHEPWMA, a); // Contrôle de vitesse en PWM, moteur 2
digitalWrite(PINMOTEURGAUCHEDIRA, HIGH);
}
void reculerMoteurDroit (int a) // En arrière
{
analogWrite (PINMOTEURDROITPWMB, a);
digitalWrite(PINMOTEURDROITDIRB, LOW); // sens de marche arrière avec LOW
}
void reculerMoteurGauche (int a) // En arrière
{
analogWrite (PINMOTEURGAUCHEPWMA, a);
digitalWrite(PINMOTEURGAUCHEDIRA, LOW);
}
J'aime ce sujet car il utilise le PID et j'ai un projet artistique qui devra faire tourner au moins 5 moteurs à vitesse constante et de manière égale dans un premier temps, puis dans un second temps les faire tourner à vitesse constante mais à des vitesses proportionnellement differentes (je rentre pas ici dans les details)
En essayant le programme, si je mets 10 dans le Serial monitor, le premier moteur accélère jusqu'a un maximum, puis le second accélère tandis que le premier ralentit, puis le second ralentit et le premier ré-accelere jusqu'a un maximum. Puis si je mets -10, le phénomène s'inverse. (je sais pas trop si c'est normal)
Dans mon projet, j'essaye de contrôler au moins pour l'instant deux moteurs afin qu'ils aillent exactement à la meme vitesse et j'aurais voulu m'inspirer de votre programme pour asservir des vitesses identiques puis differentes à chacun des moteurs et choisir leur sens, mais je sèche un peu.
Aussi j'ai voulu utilisé votre programme avec une Arduino Due, mais j'ai eu ce message:
ATTENTION : la bibliothèque TimerOne prétend être exécutable sur la (ou les) architecture(s) (avr) et peut être incompatible avec votre carte actuelle qui s'exécute sur (sam).
/Users/macbookpro/Documents/Arduino/PID_ROBOT_DEUX_ROUES/PID_ROBOT_DEUX_ROUES.ino: In function 'void setup()':
Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir
Posté 29 janvier 2019 - 10:13
Bonjour benjamin,
Pour utiliser le code avec une arduino due il faudra légèrement l'adapter en utilisant la librairie due timer
Par contre quand tu poses une questionje te recommande de copier coller le code que tu utilises.
Vu qu'il y a plusieurs versions du code dans ce fil ... ça permettra d'être sûr du code que tu utilises.
De plus on pourra voir les modifications que tu fais au fur et à mesure.