Project

General

Profile

Files » Asservissement_JC_ULTIME.ino

Justin Cano, 05/14/2016 09:30 AM

 

/*
Travail du Club Robot de l'école Centrale Marseille
****************************************************************************************
Programme d'asservissement de moteurs
Armand Sibille, le 22/12/2015

Définition des constantes:

Constantes Physiques*/
float const d=10;//distance entre les roues en cm
float const Vmax=12.5;//vitesse max en cm/s
float const conversion= 0.0893; //0.0486; //permet de transformer le nombre de ticks en vitesse par seconde

/* Définition des constantes pour la correction */
float const decalage=1;//le compteur G délivre moins de ticks que le droit pour une même vitesse
float const correctionRegulationMAv=251.0/200.0;//compense l'erreur de régulation en marche avant - Moteur Gauche trop rapide
float const correctionRegulationMAr=233.0/200.0;//compense l'erreur de régulation en marche arrière - Moteur gauche trop rapide
float const KiG=0.0;// correcteur intégrale du moteur gauche
float const KpG=1.5;// correcteur proportionnel du moteur gauche
float const KiD=0.0;
float const KpD=1.5;
int hacheurMin = 100;
/*
Méthode de calcul de la conversion
tick en 200ms -> tick/s -> tour/s -> cm/s
5 1/1938 6*Pi
*/

/*Définition des variables globales du moteur Gauche*/
int PWMG = 3; //pin du pwm du moteur gauche
int sensG=12;//pin du sens de rotation du moteur. Si sensG à LOW => marche avance. Si sensG à HIGH => marche arrière.
int entreeG = A2;// pin du capteur à effet haul du moteur
boolean etatG;
float consigneG=0.0;//en cm/s
float integraleG;

/*Définition des variables globales du moteur Droit*/
int PWMD=11;
int sensD=13;//Si sensD à LOW => marche avant. Si sensD à HIGH => marche arrière.
int entreeD=A3;// pin du capteur à effet haul du moteur
boolean etatD;
float consigneD=0.0;//en cm/s
float integraleD;

float vitesseTranslation=0.0;
float vitesseRotation=0.0;


// ******************* Asservissement ***********************
float Ki, CiG, CiD; // correction integrale
float Kp, Cp; // corretion proportionnelle
float V, Vs; // vitesse sortie
int commandeD, commandeG;
// ******************************************************************

#include <Wire.h>

void setup() {
Wire.begin(); //join i2c bus (address optional for master)
pinMode(PWMG, OUTPUT);
pinMode(sensG, OUTPUT);
pinMode(PWMD, OUTPUT);
pinMode(sensD, OUTPUT);
pinMode(entreeG, INPUT);
pinMode(entreeD, INPUT);
Serial.begin(9600);
integraleG=0;
integraleD=0;
//AdaptationConsigneRobot(7.0, 0);
//AdaptationConsigneRobot(0.0,0.0);
analogWrite(PWMG,0);
analogWrite(PWMD,0);
}


void loop() {

vitesseTranslation = 0.0;
vitesseRotation = 0.0;
/*
while(millis() < 180000){
delay(20);
}
*/
run();

}

boolean boost = true;


int ticks[3];
int commandeSortie[3];
float vitesse[3];


void testPleinePuissance () {
analogWrite(PWMG,250);
analogWrite(PWMD,250);
while( millis() < 10000){
AcquisitionTicks(ticks);
}
analogWrite(PWMD,0);
analogWrite(PWMG,0);
while (true);
}

void testPuissanceVariable () {
int pwmTest;
while( millis() < 10000){
AcquisitionTicks(ticks);
pwmTest= constrain(int(255*(10000-millis())/10000),0,255);
analogWrite(PWMG,pwmTest);
analogWrite(PWMD,pwmTest);
}
analogWrite(PWMD,0);
analogWrite(PWMG,0);
while (true);
}
void run() {

communication(vitesse);
conversionMoteur(vitesseTranslation, vitesseRotation);
AcquisitionTicks(ticks);
traitement(ticks, commandeSortie, vitesse);
boostMoteur(ticks,commandeSortie);
application(commandeSortie);
/* Adaptation à l'alimentation par générateur*/
}
void blbl () {
int ticks[3];
int commandeSortie[3];
float vitesse[3];
if(consigneG !=0.0 && consigneD != 0.0 && ticks[0]==0 && ticks[1]==0) {
analogWrite(PWMG,0);
analogWrite(PWMD, 255);//envoie de la commande au moteur droit
delay(100);
analogWrite(PWMG, 255);
} else{
//Envoi des commandes aux moteurs
analogWrite(PWMD, commandeSortie[1]);//envoie de la commande au moteur droit
analogWrite(PWMG, commandeSortie[0]);//envoie de la commande au moteur gauche
}
//Envoi de données pour correction du code
/*Serial.print(commandeSortie[0]);//Gauche
Serial.print(" ");
Serial.print(commandeSortie[1]);//Droit
Serial.print(" ");
Serial.print(consigneG);
Serial.print(" ");
Serial.print(consigneD);
Serial.print(" ");
Serial.print(vitesse[0]);//gauche
Serial.print(" ");
Serial.println(vitesse[1]);*/
//droit
Serial.print(commandeSortie[0]);
Serial.print(" ");
Serial.println(commandeSortie[1]);

//Gestion de la communication i2c
communication(vitesse);
}
// Permet le comptage des ticks effectués lors de la rotation du moteur
void AcquisitionTicks(int ticks[3]) {
unsigned long timeInit=millis();
boolean UpG;
boolean UpD;
int tickG=0;
int tickD=0;
float seuil = 1.3 ;
/*
Acquisition des ticks des deux moteurs
*/
while(millis()-timeInit < 200) {
//Détection des ticks pour le moteur de Gauche par détection de fronts descendants
if(analogRead(entreeG)*5.0/1024.0>seuil) {
UpG=true;
}
if(analogRead(entreeG)*5.0/1024.0<seuil && UpG) {
UpG=false;
tickG++;
}
//Détection des ticks pour le moteur de Droite par détection de fronts descendants
if(analogRead(entreeD)*5.0/1024.0>seuil) {
UpD=true;
}

if(analogRead(entreeD)*5.0/1024.0<seuil && UpD==true) {
UpD=false;
tickD++;

}
}
ticks[0]=tickG;
ticks[1]=tickD;
Serial.println( "t = |" + String(millis()) + "|tickG= |" + String(ticks[0]) + "|tickD= |" + String(ticks[1]) ) ;
/*
Fin de la détection
****************************
*/
}
//permet de faire une rectification de la commande du moteur à partir du nombre de ticks
void TraitementTicks(int ticks[3], int commandeSortie[3], float vitesse[3]) {
int tickG=ticks[0];
int tickD=ticks[1];
int commandeSortieD;
int commandeSortieG;
float erreurG;
float erreurD;
float proportionnelleD;
float proportionnelleG;
float vitesseG;
float vitesseD;
tickG=tickG*decalage;
if(consigneG != 0 && tickG == 0) {//permet de vite faire démarrer le moteur en cas de faible vitesse
integraleG=200;
commandeSortieG=255;
} else {//cas normal
vitesseG=tickG*conversion;
erreurG = (consigneG - vitesseG);//application du correcteur;
integraleG =integraleG+ KiG*erreurG;
proportionnelleG = KpG*erreurG;
commandeSortieG = int(integraleG + proportionnelleG);
commandeSortieG = constrain(commandeSortieG, 0, 255);
}
/*
La fonction commande s'occupe de la commande a envoyé au moteur droit pour atteindre la consigne
*/
if(consigneD != 0 && tickD == 0) {//permet de vite faire démarrer le moteur en cas de faible vitesse
integraleD=200;
commandeSortieD=255;
} else {//cas normal
vitesseD=tickD*conversion;
erreurD = (consigneD - vitesseD);//application du correcteur;
integraleD =integraleD+ KiD*erreurD;
proportionnelleD = KpD*erreurD;
commandeSortieD = int(integraleD + proportionnelleD);
commandeSortieD = constrain(commandeSortieD, 0, 255);
}
//mise à jour des tableaux
ticks[0]=tickG;
ticks[1]=tickD;
commandeSortie[0]=commandeSortieG;
commandeSortie[1]=commandeSortieD;
vitesse[0]=vitesseG;
vitesse[1]=vitesseD;
//Fin du traitement du nombre de ticks pour déduire la commande des moteurs.
}

void AdaptationConsigneMoteur(boolean AvG, float consignePourMG, boolean AvD, float consignePourMD) {
//Mise à jour des consignes de vitesse des moteurs
commandeSortie[0] = consignePourMG;
commandeSortie[1] = consignePourMD;
consigneG=consignePourMG;
consigneD=consignePourMD;
int commandeSortieG;
int commandeSortieD;
//Redéfinition des sens de rotation du moteur gauche
if( !AvG && etatG == HIGH) {
etatG=LOW;
//integraleG=50;
commandeSortieG=255;
} else if( AvG && etatG == LOW) {
etatG=HIGH;
//integraleG=50;
commandeSortieG=255;
}
//Redéfinition des sens de rotation du moteur droit
if( !AvD && etatD == HIGH) {
etatD=LOW;
//integraleD=50;
commandeSortieD=255;
} else if( AvD && etatD == LOW) {
etatD=HIGH;
//integraleD=50;
commandeSortieD=255;
}
//Correction de la consigne pour l'adapter à la réalité - f(consigne)=rotation différent pour les deux moteurs-
if(etatD==false){
consigneD=consigneD*correctionRegulationMAv;
} else {
consigneD=consigneD*correctionRegulationMAr;
}
// Correction des consignes pour ne pas saturer les moteurs et obtenir n'importe quoi
if(consigneG>Vmax && consigneG>consigneD) {
consigneD=consigneD*Vmax/consigneG;
consigneG=Vmax;
} else if(consigneD>Vmax) {
consigneG=consigneG*Vmax/consigneD;
consigneD=Vmax;
}
/*
Transmission des consignes aux moteurs
* En cas de changement de sens, on transmet le dirac afin de faire changer le sens des moteurs.
* En cas de maintient du sens des moteurs, la valeur envoyé aux moteurs ne change pas et l'asservissement va se charger d'atteindre la consigne de vitesse
*/
digitalWrite(sensG, etatG);//envoie de la commande au moteur
digitalWrite(sensD, etatD);//envoie de la commande au moteur
//analogWrite(PWMG, commandeSortieG);//envoie de la commande au moteur
//analogWrite(PWMD, commandeSortieD);//envoie de la commande au moteur
}
//méthode permettant de traiter lems informations envoyé par l'arduino nano
void AdaptationConsigneRobot(float vitesseTranslation, float vitesseRotation) {
//vitesse en cm/s et vitesseRotation en rad/s
float vg=vitesseTranslation-vitesseRotation*d/2;
float vd=vitesseTranslation+vitesseRotation*d/2;
boolean etmg=false;
boolean etmd=false;
//mise en forme pour utilisation d'AdaptationConsigneMoteur(boolean AvG, float consignePourMG, boolean AvD, float consignePourMD);
if(vg<0) {
etmg=true;
vg=-vg;
}
if(vd<0) {
etmd=true;
vd=-vd;
}
AdaptationConsigneMoteur(etmg,vg,etmd,vd);
}

//Intégration de la variation de position
void VariationPosition(float vitesseG, float vitesseD, float tab[3]) {
tab[0]=0.2*vitesseG;
tab[1]=0.2*vitesseD;
}


//méthode permettant de communiquer avec l'arduino nano
void communication(float vitesse[3]) {
float tabPosition[3];
int tabPositionInt[3];
float vitesseG;
float vitesseD;
vitesseG=vitesse[0];
vitesseD=vitesse[1];
VariationPosition(vitesseG, vitesseD, tabPosition);
tabPositionInt[0]=(int)(tabPosition[0]*10.0);
tabPositionInt[1]=(int)(tabPosition[1]*10.0); // !!!!!!!vérifier l'ajustement pour avoir une bonne précision, si besoin, changer de méthode
//Wire.beginTransmission(8); // transmit to device #8
//Wire.write(tabPositionInt[0]); // sends speed of left motor
//Wire.write(tabPositionInt[1]); // sends speed of right motor
//Wire.endTransmission(); // stop transmitting
Wire.requestFrom(8, 1); //request vitesseTranslation
Serial.println("blbl-JC");
while (Wire.available()) { // slave may send less than requested
int c = Wire.read(); // receive a byte as int
//Serial.print(c);
//Serial.print(" ");
vitesseTranslation=((float) c)/10.0-12.5;
}
Wire.requestFrom(8, 1); //request vitesseRotation
while (Wire.available()) { // slave may send less than requested
int c = Wire.read(); // receive a byte as int
//Serial.print(c);
//Serial.print(" ");
vitesseRotation=(((float) c)/100.0)-1.0;
}
//Serial.println("Et on repart");
//AdaptationConsigneRobot( vitesseTranslation, vitesseRotation);
/*
Serial.print(" Vitesse Translation : ");
Serial.print( String(vitesseTranslation));
Serial.print(" Vitesse Rotation : ");
Serial.println( String(vitesseRotation));
delay(1000);
*/
}

// Fonction traitement ticks
long TG; // ticks gauche
long TD; // ticks droit
float consigne;

int traitement(int ticks[3], int commandeSortie[3], float vitesse[3]){
consigneG=vitesse[0];
consigneD=vitesse[1];
// roue droite
Ki = KiD; Kp = KpD; consigne = consigneD; TD = ticks[1];
V =float(TD * conversion);
float erreur = consigne-V;
Serial.println("consigne D" + String(consigneD)+ "erreur = " + String(erreur));
Cp = Kp * erreur;
CiD += Ki * erreur;
Vs = Cp + CiD;
commandeD = int(Vs/Vmax * 255);
if(consigne<0.01) {
commandeD=0;
}
else{
commandeD = constrain(commandeD, hacheurMin, 255);
//vitesse[0]=V;
}
// roue gauche
Ki = KiG; Kp = KpG; consigne = consigneG; TG = ticks[0];
V =float(TG * conversion);
erreur = consigne-V;
Cp = Kp * erreur;
CiG += Ki * erreur;
Vs = Cp + CiG;
commandeG = int(Vs/Vmax * 255);
if(consigne<0.01) {
commandeG=0;
}
else{
commandeG = constrain(commandeG, hacheurMin, 255);
//vitesse[0]=V;
}
//vitesse[1]=V;
// ******************************************************************
commandeSortie[0] = commandeG;
commandeSortie[1] = commandeD;
}

// fonction remettre un coup de jus aux moteurs si n'avance pas
float CS = 0.1; // consigne seuil à choisir
void boostMoteur(int ticks[3], int commandeSortie[3]){
int commandeG, commandeD;
TG = ticks[0]; TD = ticks[1];
long t_arret;
if(vitesse[0] > CS && TG == 0 && boost){ commandeG = 255 ; commandeSortie[0] = commandeG;}
if(vitesse[1] > CS && TD == 0 && boost){ commandeD = 255; boost = false ; commandeSortie[1] = commandeD;}
else {boost =true;}


}

// fonction application

void application(int commandeSortie[3]){
analogWrite(PWMG, commandeSortie[0]);
analogWrite(PWMD, commandeSortie[1]);
digitalWrite(sensG, etatG);
digitalWrite(sensD, etatD);
}

void conversionMoteur(float vitesseTranslation, float vitesseRotation){
float vg=vitesseTranslation-vitesseRotation*d/2;
float vd=vitesseTranslation+vitesseRotation*d/2;
if(vg < 0.0){ etatG = HIGH; vg = -vg;}
else{ etatG = LOW; }
if(vd < 0.0){ etatD = HIGH; vd = -vd;}
else{ etatD = LOW; }
vitesse[0]=vg;
vitesse[1]=vd;
}
(8-8/8)