1

Asservissement.net (part. 2)

Nous avons déjà vu la base de notre asservissement en .net

Voici maintenant la suite : comment intégrer ce PID dans le cadre de notre Robot, avec un asservissement en distance et en rotation.

Notre code de PID utilise des delegates pour passer les différentes valeurs et renvoyer celles de sortie. Il y a donc 6 fonctions à définir (3 par PID).

Les deux premières concernent la récupération des valeurs réelle obtenue en lisant les codeurs. Dans notre cas, nous utilisons des composants nommés LS7366 qui, de manière autonome, récupère les pas renvoyés par les codeurs. Il suffit ensuite de les interroger par SPI pour connaitre l’état des codeurs. Ce composant s’occupe de filtrer les pas dus à la vibration du robot et prend en compte correctement le sens de rotation des moteurs. Nous ferons sans doute un article à son sujet (car il le mérite).
Pour en revenir à nos deux fonctions, elles renvoient pour l’avance la moyenne du codeur droit et gauche pour l’avance et la différence entre les deux pour la rotation.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
public static int CodeurDroitPourObj

{
get { return codeurDroit - lastResetDroit; }
}

public static int CodeurGauchePourObj
{
get { return codeurGauche - lastResetGauche; }
}

private static double GetCurrentAvance()
{
// le signe - est lié au branchements des codeurs
codeurDroit = -LSCodeurDroit.ReadCounter();
codeurGauche = LSCodeurGauche.ReadCounter();

// on fait la moyenne
return (CodeurDroitPourObj + CodeurGauchePourObj) / 2;
}
private static double GetCurrentCodeursAngle()
{
return (CodeurGauchePourObj - CodeurDroitPourObj) / 2;
}

Le composant LS7366 ne doit pas être reseté durant l’exécution de l’application car on risquerait de perdre quelques pas lors de la manœuvre, nous avons donc mis en place  un système de reset software : d’où l’usage des accesseurs CodeurXXPourObj.

Je garde la seconde paire de getteur pour la fin et commence par le setteur final. Il s’agit simplement d’affecter la valeur obtenue vers les PWM des moteurs en tenant compte du sens.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
private static void SetCommandAvance(double val)
{
// on ne settera les PWM qu'au set de l'angle (une fois qu'on aura la valeur finale)
curvalDroit = curvalGauche = val;
}

private static void SetCommandAngle(double val)
{
// -= car lié aux branchements...
curvalGauche -= val;
curvalDroit += val;

// Set des PWM
SetPWMValue(pwmGaucheTrigo, pwmGaucheHoraire, curvalGauche);
SetPWMValue(pwmDroitTrigo, pwmDroitHoraire, curvalDroit);
}

Et donc le dernier couple de setteur : celui qui renvoie la valeur désirée.

La problématique ici est de savoir ce que l’on veut. Dans notre cas, nous avons considéré deux cas : soit nous tournons, soit nous avançons.

Dans le premier cas, la valeur désirée en avance est de 0, dans le second cas, c’est la rotation qui doit être à zéro.

Pour les deux autres valeurs, il faut retourner une pente d’accélération, puis stabiliser autour de la valeur maximum de la vitesse et enfin une pente de décélération une fois proche de la destination (en rotation ou en avance).

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
private static double GetCurrentObjAvance()
{

// incrémentation ou décrément la vitesse (de l'acceleration)
if (currentVitesse < vitesseMax && Math2.Abs(objAvance) > Math2.Abs(curObjAvance))
{
currentVitesse += acceleration;
}
else if (currentVitesse > 0 && Math2.Abs(objAvance) {
currentVitesse -= acceleration;
}

// on augmente l'avance de la vitesse
curObjAvance += Math2.Sign(objAvance) * currentVitesse;

return curObjAvance;
}

private static double GetCurrentObjectiveAngle()
{
// incrémentation ou décrément la vitesse de rotation
if (Math2.Abs(currentVitesseAngle) < vitesseMaxAngle &amp;&amp; Math2.Abs(ObjTeta * EntraxePas / 2) &gt; Math2.Abs(curObjAngle))
currentVitesseAngle += accelerationAngle;
else if (Math2.Abs(currentVitesseAngle) > 0 && Math2.Abs(ObjTeta * EntraxePas / 2) currentVitesseAngle -= accelerationAngle ;

// augmentation de l'angle désiré de la vitesse (convertion de l'angle en pas)
curObjAngle += Math2.Sign(ObjTeta) * currentVitesseAngle;

return curObjAngle;
}

Pour démarrer une rotation, il suffit donc de mettre curObjAngle à la valeur désirée (en radiant). De même pour avancer, il faut mettre curObjAvance a la valeur désirée (en pas).

Enfin, l’initialisation des PID :

1
2
3
4
PID.pidAvance = new PID(1, 0, 0, 4000, -4000, GetCurrentAvance, GetCurrentObjAvance, SetCommandAvance);

PID.pidAngle = new PID(1,0,0, 4000, -4000, GetCurrentCodeursAngle, GetCurrentObjectiveAngle, SetCommandAngle);
PID.Enable();

Une petite précision pour être complet, on ne traite jamais lors de cet asservissement, de rotation simultanée à une avance, qui pourrait permettre de belle courbe comme présenté sur les articles d’autres équipes.  Dans notre cas, chaque déplacement est décomposé en une rotation puis une avance. J’y vois plusieurs intérêts :

  • cela permet de mieux anticiper les obstacles que nous pourrions rencontrer (une courbe n’est pas évidente à simuler)
  • l’asservissement est grâce à cela, complètement indépendant du mécanisme de calcul de position
  • dans le cadre de la coupe, il ne me semble pas forcément nécessaire d’arriver au cm prêt au point désiré (du moment que l’on connait la position exacte du robot)
  • cela simplifie grandement le code et les temps de calcul

Comme la dernière fois, vous pouvez retrouver l’ensemble du code ici, y compris le calcul de position du robot et la définition des objectifs (fonctions GotoXY et GotoTeta)

  1. [...] dans un prochain article. Toute les sources sont de toute façon disponible ici.EDIT : La suite iciAsservissement.net (part. 2) | MLRobotic dit : 3 mars 2012 à 21 h 31 min[...] propos [...]