Je vais détailler ici plus particulièrement l'algorithme utilisé sur Joyeux pour se diriger vers les ballons.
Il n'est pas très compliqué, mais présente quand même une difficulté :
En effet il y a un double asservissement imbriqué
La tourrelle doit chercher, puis rester sur la balise.
Et Joyeux lui-même doit se tourner dans la direction de la balise.
Oui, mais quand Joyeux se tourne, la tourrelle tourne avec !
Il faut donc la ramener un peu dans la direction inverse pour ne pas qu'elle perde la balise.
Résultat : des réglages un peu délicats (sachant qu'on n'a pas vraiment beaucoup de marges). Mais au final ça marche !
|
Comment la tourelle reste sur la balise ?
|
La tourelle n'a qu'une information pour se diriger. Oui il y a une balise en face, non il n'y a pas de balise en face.
Rien d'autre ! Et en particulier aucune information de direction ...
Du coup j'ai utilisé l'algorithme suivant pour traquer la tourrelle :
Admettons qu'on soit sur la balise. On initialise une valeur compteur à 0 et CompteurMax à 1.
On va alors bouger dans une direction (c'est important, on bouge tout le temps la tourrelle !)
Si on détecte on revient au début, en continuant à bouger dans la même direction.
Si on ne détecte plus on va alors incrémenter compteur.
On continue à bouger jusqu'à ce que soit on détecte (et on revient toujours au début dans ce cas),
soit compteur soit égal à compteurMax. A ce moment-là on double compteurMax, on remet compteur à 0 et on change de sens de rotation de la tourrelle.
On cherche encore la balise dans l'autre sens cette fois ...
Donc la tourrelle va faire des aller-retour de plus en plus longs pour essayer de trouver la balise qu'elle a perdue !
Si CompteurMax, qui double à chaque aller-retour, dépasse une certaine valeur, on considère la balise comme perdue,
et on passe à d'autres phases du programme pour essayer de la retrouver.
|
Et ça donne quoi sur les roues ?
|
Et bien tout simplement si la tourrelle part trop à droite ou trop à gauche les roues tournent.
Tout d'abord une roue d'un coté s'arrête, mais si la différence est encore plus grande elle part en marche arrière !
Du coup, il faut compenser sur la tourelle pour qu'elle suive le mouvement...
|
L'algo général?
|
Voici maintenant l'algo général.
Il y a eu depuis que je l'ai écrit (et oui, je l'ai écrit avant de coder !!) quelques différences par rapport au code, mais rien d'important !
Interruption :
Gère les PWM (avec Min et Max teste)
Teste l'obstacle devant -> retourne sa valeur dans une variable globale
Teste la balise -> retourne sa valeur dans une variable globale
Compteur++
NouvelleInterrution = 1
Main :
BaliseATrouver = Balise1
CompteurDeChoc = 0
CompteurDePerduBalise = 0
CompteurDeVaToutDroit = 0
Debut : etape 1
(les indications de sens tourelle/moteur sont en fait des fixations de la valeur de leur PWM)
-------------------------------------------------------
Les phases de mouvement forcees
-------------------------------------------------------
Etape 1 Rotation sur soi-meme
Compteur = 0
Faire
Tourelle devant
Moteur gauche avance
Moteur droit recule
Jusqu'a ce que :
Balise detectee -> va en 4
Compteur superieur a 200 -> Va en 2
Obstacle detecte -> va en 3
Etape 2 Avance tout droit
Compteur = 0
CompteurDeVaToutDroit++
si CompteurDeVaToutDroit > CompteurDeVaToutDroitMax
GOSUB ChangeBalise
Faire
Tourelle devant
Moteur gauche avance
Moteur droit avance
Jusqu'a ce que :
Balise detectee -> va en 4
Compteur superieur a 200 -> Va en 1
Obstacle detecte -> va en 3
Etape 3 Recule suite a obstacle
Compteur = 0
CompteurDeChoc++
si CompteurDeChoc > CompteurDeChocMax
GOSUB ChangeBalise
Faire
Tourelle devant
Moteur gauche arret
Moteur droit recule
Jusqu'a ce que :
Compteur superieur a 100
PWMtourelle = MaxPWMtourelle
sensDeRotationTourelle = -1
-> Va en 5
-------------------------------------------------------
L'asservissement
-------------------------------------------------------
Etape 4 Asservissement sur la balise
CompteurMax = 1
Compteur = 0
avance = 1
Faire
NouvelleInterrution =0
Faire (rien) // Attend qu'une phase d'interruption complete ait ete realisee
tantque NouvelleInterrution=0
PWMtourelle = PWMtourelle + sensDeRotationTourelle (-1 ou 1)
Si balise detectee -> compteur = 0, CompteurMax = 1, avance = 1
Si compteur > compteurMax, ou si en butee (> a MaxPWMtourelle ou < a MinPWMtourelle )
sensDeRotationTourelle = - sensDeRotationTourelle
compteur = 0
compteurMax = compteurMax * 2
Si compteurMax > 16 avance = 0
Si compteurMax > 128
si PWMtourelle > milieu
PWMtourelle = MaxPWMtourelle
sensDeRotationTourelle = -1
sinon
PWMtourelle = MinPWMtourelle
sensDeRotationTourelle = 1
Va en 5
Si PWMTourelle en butee
si PWMtourelle > milieu
PWMtourelle = MaxPWMtourelle -1
sinon
PWMtourelle = MinPWMtourelle +1
si avance = 1
si PWMtourelle < milieu + epsilon
Moteur droit avance
PWMtourelle++
sinon
si PWMtourelle > milieu + GrandEcart
Moteur droit recule
PWMtourelle--
sinon
Moteur droit arret
si PWMtourelle > milieu - epsilon
Moteur gauche avance
PWMtourelle--
sinon
si PWMtourelle < milieu - GrandEcart
PWMtourelle++
Moteur gauche recule
sinon
Moteur gauche arret
Sinon su avance = 0, va tout droit !
Jusqu'a ce que
Obstacle detecte
si balise detectee
GOSUB ChangeBalise
-> va en 3
-------------------------------------------------------
La recherche immobile, un tour de tourelle complet
-------------------------------------------------------
etape 5 Tour de tourelle immobile
Compteur = 0
CompteurDePerduBalise++
si CompteurDePerduBalise > CompteurDePerduBaliseMax
Gosub ChangeBalise
Faire (pour laisser le temps a la tourelle d'arriver en butee)
Moteur gauche arret
Moteur droit arret
Jusqu'a ce que :
Compteur superieur a 50, continue
Faire
PWMtourelle = PWMtourelle + sensDeRotationTourelle (-1 ou 1)
Moteur gauche arret
Moteur droit arret
Jusqu'a ce que :
Balise detectee -> va en 4
Obstacle detecte -> va en 3
Tourelle en butee -> Va en 1
-------------------------------------------------------
Des Sous-Fonctions
-------------------------------------------------------
ChangeBalise
CompteurDeChoc = 0
CompteurDePerduBalise = 0
CompteurDeVaToutDroit = 0
BaliseATrouver++
si BaliseATrouver > BaliseATrouverMax
BaliseATrouver = Balise1
|
Les autres parties du code : La gestion des servos
|
Les PWM des servos sur Joyeux ainsi que sur d'autres PMI qui ont le même code
sont gérés un peu différement de ce qu'on a vu les pages précédentes.
Le code est ici fait pour demander le même temps quelque soit les valeurs de ces PWM, soit 20ms,
pour simplifier ensuite les phases au timer.
Il y a eu pour cela une simplification : la tourrelle peut toujours avoir une valeur de PWM quelconque (8bits, limité par les butées),
mais les 2 PWM des moteurs gauches et droits ne peuvent plus prendre que 3 valeurs : Arret (pas de PWM,) avance (tourne à fond dans un sens) et recule (tourne à fond dans l'autre).
Cette partie du code sera expliquée plus en détail dans les pages d'autres PMI Fribottes.
|
Le démarrage au timer
|
Joyeux présente une particularité : l'utilisation de l'EEPROM du 16F84.
En effet, les numéros des balises à rechercher, ainsi que l'ensemble du parcours à effectuer était stoqué dans cette dernière
et lus à l'éxécution par le programme.
Le stockage dans l'EEPROM permet surtout une modification plus rapide : pas besoin de recompiler le programme pour changer de parcours.
Par contre sa capacité de stockage est limitée (mais suffisante dans le cas de Joyeux).
Une simple modification dans l'EEPROM à partir du programmateur permet le changement des codes des balises,
ou la modification d'un paramêtre du parcours au timer.
Ces derniers sont représentés de manière très simple :
Il y a 2 octets, un premier indique l'état des moteurs à appliquer (marche/arrêt et sens)
et le deuxième indique le temps pendant lequel il est appliqué, en muliple de 20ms.
Toutes ces données sont pré-remplies dans le programme par la commande d'assemblage "DE" qui permet
de remplir l'EEPROM. Et elles sont donc ensuite rapidement modifiables dans l'éditeur du programmateur
(même si il était quand même nécessaire d'enlever le PIC pour le mettre sur le programmateur !)
|
Le code final
|
Voici donc le code de Joyeux tel qu'il a été utilisé à la coupe, à quelques réglages près !
|
|
