Objet
La carte d'asservissement possède, outre le processeur d'odométrie, le processeur d'asservissement.
C'est lui qui pilote les moteurs par découpage donc est capable de leur appliquer à chacun une puissance variable en marche avant et arrière.
L'asservissement qui calcule cette puissance (par moteur) utilise en entrée :
les informations parvenant de la carte d'attitude : angle et vitesse angulaire
les information parvenant de l'odométrie (par roue) : distance et vitesse de déplacement
les information parvenant de la carte de pilotage : consignes de vitesse et de volant
Voici donc une description des deux bouts de codes importants... Le programme complet est bien plus conséquent mais est-ce utile de tout montrer alors que vous voulez sûrement construire VOTRE robot et non une copie.
Commande des moteurs
Pour commander les moteurs, il faut générer un signal indiquant le sens de rotation et un signal périodique dont le rapport cyclique dépend de la puissance à appliquer.
Il faut noter ici qu'on compense par un seuil le rapport cyclique pour éviter que le moteur ne tourne pas dans les toutes petites puissances : zéro correspondra à un moteur arrêté mais epsilon correspondra à un rapport cyclique qui fait tourner le moteur. Voir ce site pour le détail.
Le contrôle des moteurs se fait en interruption :
L'interrupt timer 0 qui passe toutes les 5msec démarre alternativement chaque moteur toutes les 10 msec et lance les timers 1 ou 3, suivant la roue avec une temporisation égale à la puissance demandée.
Lorsque l'interrupt 1 ou 3 passe, on stoppe le moteur correspondant.
La fonction setPwm() permet de calculer la puiss ance à appliquer avec tous les contrôles de validité des informations et application du sueil décrit pécédemment.
/******************************************************
/ Interrupt timers 0 , 1 et 3
/ traite le pwm des roues gauche et droite
/ toggle 10msec pour traiter alternativement gauche et droite
/ (égalise mieux les pointes de courants)
/ utilise timer 1 et 3 pour arrêter les impulsions
/
/******************************************************/
#int_TIMER0
void TIMER0_isr() { // interruption toutes les 5msec.
set_timer0(VAL_TMR0); // restart timer
toggle=!toggle;
if (toggle){ // Toutes les 10msec : roue avant
toggleA=!toggleA; // toggle de lancement des fonctions de base
if(toggleA) GoBase=true; // toutes les 20 msec
output_low(D_ENABLE);
if (Torque_D>0){ // pair : roue droite
output_high(D_H1); //marche avant roue droite
output_low(D_H2);
set_timer1(65536 - Torque_D); // set timer 1
output_high(D_ENABLE); // enable roue droite
}
else if (Torque_D<0){
output_low(D_H1); //marche arrière roue droite
output_high(D_H2);
set_timer1(65536 +Torque_D); // set timer 1
output_high(D_ENABLE); // enable roue droite
}
} else { // Toutes les 10msec : roue gauche
output_low(G_ENABLE);
if (Torque_G>0){
output_high(G_H1); //marche avant roue gauche
output_low(G_H2);
set_timer3(65536 - Torque_G); // set timer 3
output_high(G_ENABLE); // enable roue gauche
}
else if (Torque_G<0){
output_low(G_H1); //marche arrière roue gauche
output_high(G_H2);
set_timer3(65536 +Torque_G); // set timer 3
output_high(G_ENABLE); // enable roue gauche
}
}
}
//
//interrupt timer 1 : arrêter l'impulsion roue droite
#int_TIMER1
void TIMER1_isr(){
output_low(D_ENABLE);
set_timer1(0);
}
//
//interrupt timer 3 : arrêter l'impulsion roue gauche
#int_TIMER3
void TIMER3_isr(){
output_low(G_ENABLE);
set_timer3(0);
}
//
//init du pwm
void Init_Pwm(){
Torque_D = 0; // init puissance moteur droit
Torque_G = 0; // init puissance moteur gauche
pwmtg=0;
pwmtd=0;
toggle=true;
toggleA=true;
}
//
// set pwm
void setPwm(){
if(pwmtd<0){
pwmtd -=THRESH; // annulation du threshold
if(pwmtd<-TQMAX) pwmtd=-TQMAX; // contrôle de non dépassement
}else if(pwmtd>0){
pwmtd +=THRESH;
if(pwmtd> TQMAX) pwmtd= TQMAX;
}
if(pwmtg<0){
pwmtg -=THRESH;
if(pwmtg<-TQMAX) pwmtg=-TQMAX;
}else if(pwmtg>0){
pwmtg +=THRESH;
if(pwmtg> TQMAX) pwmtg= TQMAX;
}
disable_interrupts(GLOBAL); // application des valeurs
Torque_D = pwmtd;
Torque_G = pwmtg;
enable_interrupts(GLOBAL);
}
Formule de balancement
Cette fonction répond par la puissance à appliquer sur chaque roue. Le code est court pour unr telle fonction. Bien sûr, il existe d'autres fonctions sui préparent les informations utiles...
Notez 4 points :
Quand le robot est à l'arrêt, on utilise un asservissement en position au lieu de l'asservissement en vitesse
L'erreur entre position souhaitée et position réelle est écrétée pour que le robot rejoigne tranquillement sa consigne... D'une manière générale, toutes les valeurs sont contrôlées et bornées.
Le calcul Trim est destiné à faire pencher le robot progressivement quand il "attaque" une montée... Pour éviter qu'il ne s'écrase vers l'avant dès la côte ou l'obstacle terminé, cette valeur reprend très vite une valeur nulle ou négative.. Il y a lieu de faire des essais de coefficients pour bien régler ce terme Trim
Tous les coefficients sont modifiables, en fonctionnement, à partir de la télécommande infra rouge... Ceci est très utile pour montrer rapidement l'effet de chacun d'eux.. (bien entendu, il existe une commande de retour aux paramètres usine !)
Enfin, pour avoir un fonctionnement satisfaisant pour toutes les tensions de la batterie (de complètement chargée à presque à plat), on applique au résultat un coefficient dépendant de l'écart tension-tension nominale.
/*********************************************************
/
/ Balancing : lancé par asserv tous les 20 msec
/
**********************************************************/
void balancing(){
/*- en entrée de la routine, on a :
* Angle= l'angle en 500ièmes de degrés
* Vangle= la vitesse angulaire en degrés par secondes
* Vitesse_D et Vitesse_G : vitesse mesurée en tics par 50msec
* Position_Ecart = l'écart en 2*tics entre la position du robot et la consigne de position
* Orientation_Ecart= écart en tics entre la direction du robot et la consigne de direction
* Vitesse_Consigne = vitesse de consigne entre -20 et +20 (approx en cm par seconde)
*
* - le but est de calculer les pwm des moteurs droit et gauche
*/
signed int32 Err_V;
float Vit;
// si la vitesse est nulle, alors asservissement en position
if (Vitesse_Consigne==0)
Vit =((float)(Vitesse_D)+(float)(Vitesse_G) - ((float)(Position_Ecart))/16.0);
else
// sinon, appliquer un facteur dépendant de l'écart vitesse réelle -vitesse de consigne
Vit =(float)(Vitesse_D)+(float)(Vitesse_G) - (float)(Vitesse_Consigne);
if ( Vit>30.0) Vit=30.0; // plafonner l'écart à 30cm*s pour les facteurs K4 et K3
else if (Vit<-30.0) Vit=-30.0; // (vit est en mm(tics)/100msec => vit=1 ==> 1cm/sec
Trim += K4B*Vit +K5B*(Vit-Vit_O); // ajuster le trim d'asservissement
if (Trim>2000.0) Trim=2000.0; // plafonner le trim à quelques degrés
else if (Trim<-2000.0) Trim=-2000.0;
Vit_O=Vit; // mémorisation de l'écart
pwmtb = K1B*(float)(Angle) // proportionnel à l'angle K1B
+K2B*(float)(Vangle) // + amortissement en fonction de la vitesse angulaire
+K3B*Vit // + vitesse
+Trim; // et le trim d'asservissement en position
// appliquer un coeff dépendant de la tension batterie
pwmtb = pwmtb*Err_Voltage;
Err_V = (signed int32)(Orientation_Ecart)*5; // et enfin, la consigne du volant à chaque roue
pwmtg=(signed int32)(pwmtb)+ Err_V;
pwmtd=(signed int32)(pwmtb)- Err_V;
setPwm(); // appliquer le résultat
}
Conclusion
Le programme comprend d'autres fonction (retour à l'équikibre après chute, alarme tension faible, dialogue I2C, interruption de surintensité .. ) mais ces deux fonctions constituent le coeur de l'asservissement.
