<< The Fribotte Homepage >>
Un club de passionnés en robotique participant à la coupe de France E=M6.
[Accueil] [Qui sommes-nous ?] [Robots] [Coupe e=m6] [BD Technique] [Forum] [Reportages] [Liens] [WiKiFri]

Fribotte

 

Description technique de Cnossos


Intro...

 

Cnossos est un robot construit assez rapidement, mais qui est relativement complexe.

La mécanique et l'électronique se résument finalement à... faire avancer un robot :-) ce qui reste assez simple (mais qui doit être bien fait).

Mais niveau programmation, c'est chaud ! Et oui, 1m/s, ça se mérite !

Avant de lire cette page, lisez d'abord les pages précédentes et le poster. En effet, elles ont été faites pendant la réalisation de Cnossos, et je vais considérer leur contenu comme acquis pour m'éviter du travail ;-)

 


Le programme du PIC N°2 (esclave)
PID sur PIC V3 en béta

Le 2ème PIC de Cnossos est en fait une version beta avancée du PID sur PIC V3.

Il permet :

- l'asservissement de position des deux moteurs (à ne pas confondre avec l'asservissement de position du robot)

- L'asservissement de vitesse des deux moteurs (qui est en fait un asservissement de position déguisé).

Je ne veux pas m'appesantir sur le sujet, car même si cette version marche bien, elle n'est pas propre, en particulier dans l'interface de communication.

Du coup je vais la reprendre et l'expliciter dans une page sur le PID sur PIC V3.

Je ne vous conseille pas de l'utiliser, mais je vous mets quand même le code à disposition pour voir ce qu'il en est !

Ce programme est indispensable. Il permettait un contrôle précis des réactions des moteurs. Ses pentes d'accélération et de décélération étaient absolument nécessaires pour éviter des dérapages des roues, préjudiciables à l'odométrie.

Je conseille à tout réalisateur de robot rapide d'avoir un système au moins équivalent. Ce n'est qu'en cascadant une série de boucles de rétroactions (celle du PID des moteurs étant la plus bas niveau) qu'on peut réellement contrôler les mouvements du robot au micro-poil.

 


Le programme du PIC N°1 (Maître)
L'intelligence du robot

 

Ce PIC maître a décidément beaucoup de choses à faire...

Il doit :

- Initialiser le PIC esclave

- Gérer les mouvements du robot jusqu'à ce qu'il sache où il se trouve dans le labyrinthe

- Calculer le plus court chemin vers le centre du labyrinthe où se fait le tir de balle, puis vers la sortie

- Faire naviguer le robot à 1m/s, en bouclant sur les calculs d'odométrie et sur le recalage effectué avec les distances aux murs.

Niveau E/S, il gère :

- Le bouton de départ

- Les 4 capteurs de distance SHARP, avec sortie analogique

- L'afficheur LCD

- La communication avec le PIC esclave.

En fait le pauvre 18F252 et ses 1.6Ko de RAM et 32Ko de FLASH est limite limite pour faire tout ça !

Mais bon, ça marche ;-)

 

Où trouver le programme ?

 

Il est téléchargeable ici, avec toutes les librairies et le projet C18/Mplab.

 

Pourquoi le programme est-il si moche ?

 

Je vous le dis tout de suite, il ne faut pas s'attendre à un programme digne des maîtres du C, très loin de là.

C'est pas que j'avais pas envie, en général je fais quand même beaucoup mieux que ça (même si c'est jamais extraordinaire)

Mais là, plusieurs raisons se sont liguées contre moi pour m'empêcher de faire un code propre.

- Tout d'abord la place disponible en FLASH. En effet, j'ai explosé les 32Ko ! Du coup quand le programme était bien avancé, il m'a fallu optimiser le code au fur et à mesure que je rajoutais des choses. Le tout rapidement, et sans faire de bugs. Donc bien sûr c'est limite. De plus, certaines fonctions non utilisées de librairies sont commentées (ou même enlevées).

- Ensuite la RAM disponible 1.6Ko c'était un poil juste. J'ai donc du faire quelques compromis, et par exemple réutiliser des variables globales là où ce n'était pas prévu (beuark).

- Au niveau des performances, il ne fallait pas que le PIC traîne. Du coup j'ai, sans autre forme de procès, mis la plupart des variables en variables globales ! (Gain dans les 10-20%). Pour s'y retrouver un peu (et pas mélanger les noms de variables), j'ai rajouté un préfixe sur les variables d'une même fonction.

- Problèmes de compilo ? Il m'est arrivé plusieurs choses bizarres avec le code ... Je n'avais pas tellement le temps de chercher, j'ai donc coupé au plus court. J'ai supposé que c'était des bugs de jeunesse du compilo, et j'ai été assez brutal : quasiment pas de variables locales (en fait utilisation minimale de la pile). Pas de paramètres qui portent le même nom dans une fonction et ses sous fonctions. Pas d'appel de fonction dans l'interruption. Il faut quand même dire que je poussais le C18 dans ses derniers retranchements ... (depuis une nouvelle version de C18 est sortie).

- Beaucoup de tests dans le main ...Un robot, c'est avant tout des tests jusqu'à ce qu'il marche. Du coup le main est bourré de code en commentaire qui permet de tester, de recalibrer, de valider... Bref, oubliez les commentaires ;)

- Plein de choses dans le même .c ? Evidemment, il est plus propre de séparer un code en plusieurs .c .h. Or ici, j'ai surtout tout mis dans le cnossos.c, à part les librairies (lcd, math ...). Du coup ça donne un machin de 3700 lignes tout de même. Oui mais... si j'avais découpé je risquais pas mal de problèmes. Tout d'abord, j'aurais du utiliser plus de paramètre dans les fonctions (= danger = plus lent) ou des extern (dont je me méfie aussi, surtout que j'ai bien trituré les banques RAM pour les variables). Et ça m'aurait dégradé (légèrement) les performances.

Allez zou ! tout dans le même .c ! ça roule ;)

Mais bon, ok c'est pas très beau, mais en tout cas ça marche. Et pour moi c'est le plus important ;)

J'arrivais en plus facilement à m'y retrouver. Par contre vous aurez plus de mal :-/

 

Les clefs de la réussite

 

Evidemment, c'est beaucoup plus facile de parler de ce qu'on a fait de bien dans un programme d'un robot quand ce dernier gagne un concours.

Mais ne nous y trompons pas, les choix faits ici ont certes permis de gagner, mais ça ne veut pas dire que ce soit les meilleurs, ni même qu'ils soient forcément très bons ! Et ce qui est sûr, c'est que ce n'est pas les seuls possibles. De toute façon la chance joue toujours un peu.

Ceci-dit, voici tout de même ce que j'estime important dans la réalisation du programme de Cnossos, et qui a certainement permis à ce dernier d'être efficace.

- Avant de se lancer dans un tel programme, il faut avoir une bonne idée d'où on va. Si vous regardez juste la taille de cette page qui décrit le programme, on se rend bien compte que c'est plutôt long ! Il faut avoir une certaine expérience pour ne pas se lancer dans des choses trop impossibles. J'étais ici beaucoup aidé par mes expériences sur les robots Coredump et Haku.

- Un tel programme se fait par étape. Cnossos a d'abord avancé droit. Puis il a avancé d'une case après l'autre. Puis il a trouvé le plus court chemin. Puis il a calculé l'endroit où il se trouvait. Et quand tout ça était bon, il s'est recalé avec les murs. Chaque étape doit non seulement être terminée avant de se lancer dans la suivante, mais elle doit en plus être correctement testée. Tout retour en arrière est dévastateur pour le moral !

- Nous sommes ici sur un robot réel, qui bouge. On n'est plus dans le virtuel où 1+1=2. Les capteurs donnent de vraies distances, avec du retard et du bruit. L'odométrie peut patiner. Le robot a de l'inertie. Il y a des retards dans les chaînes d'asservissement. Tout ceci, et bien d'autres choses, font que votre "système" robot est quelque chose de très complexe, surtout quand il commence à aller vite. Il faut, pour que le programme marche, qu'il prenne en compte un peu tout ça. Vous devez donc essayer de comprendre un peut tout ce qui se passe, et anticiper les réactions de votre robot.

- Il faut beaucoup de patiente et de travail pour que ça marche bien à la fin. On est souvent bloqué plusieurs jours sur des bêtises alors que l'heure tourne ! Il faut persévérer, et s'entêter. La solution est au bout. Cnossos a demandé aussi énormément de tests et de réglages pour arriver à aller vite. Là aussi, il ne faut pas lâcher, et faire tourner le robot encore et encore...

- Avoir un robot qui marche "parfois" est facile. Avoir un robot qui marche 90% du temps est beaucoup plus dur, et demande une attention de tous les instants, et là aussi beaucoup de persévérance. La fiabilisation du robot (qui concerne bien sûr tous ses aspects depuis la conception) est très difficile. Et ça ne veut malheureusement pas dire que ça marchera à la fin. Il faut toujours un peu de chance...

 

Description des .c/.h

 

En plus de cnossos.c, qui sera décrit plus loin, le projet comprend :

- lcd.c/.h qui est une librairie de gestion de LCD, mode parallèle, 4 bits.

- eepromvalue.h définit les valeurs qui vont être mises dans l'EEPROM du PIC esclave. En gros, il s'agit des coefficients du PID.

- math.h la librairie mathématique, réalisé par l'ANCR, qui définit les fonctions trigonométriques (en particulier) utilisées par le programme. Beaucoup de ses fonctions sont en commentaire. De plus, cette version a un bug sur le calcul de la racine carrée si x<1.

- plan.h définit la disposition du labyrinthe sous forme de tableau de bits, suivant les 4 directions possibles. Il y a aussi quelques informations intéressantes, comme la position de la sortie et du tir. C'est en grande partie généré à partir d'un fichier excel.

- sharp.h contient les tableaux de conversion valeur capteur -> distance en 1/2 cm. Ces valeurs ont étés obtenues après un long calibrage, et sont spécifiques à chaque capteur (et à leur emplacement dans le robot). Voici le fichier excel du calibrage.

 

Codage des murs du labyrinthe

 

La façon de stocker les murs du labyrinthe est assez optimisée, et pas très intuitive.

L'idée de base est de dire que 1 mur = 1 bit. Cette représentation est extrêmement pratique dans la recherche de la position du robot. En effet, en raisonnant sur des opérations logiques bit à bit, on teste 8 murs en même temps sur un octet.

Ensuite, les murs sont séparés en deux séries, les murs verticaux et les horizontaux.

La taille du labyrinthe, de 9*9 initialement, est portée à 16*16 pour que ça tombe juste informatiquement parlant. De plus, le 9*9 sera à peu près au milieu du 16*16, et tout le reste sera complété par des 0. Cette représentation a l'énorme avantage d'éviter des tas de tests de dépassement. Par exemple, la case d'arrivée, qui est à l'extérieur des 9*9, est une case comme une autre dans le 16*16.

Conséquence : dans le programme les numéros de cases sont représentés soit par un x,y avec x (colonne) et y (ligne) variant de 0 à 15, soit par un chiffre unique de la forme x+y*16, de 0 à 255 (255 signifiant généralement une erreur).

Nous avons dont deux tableaux. Un tableau de 16 mots de 16 bits (2 octets) avec les murs horizontaux, et un tableau de taille identique pour les murs verticaux.

Ces deux tableaux se retrouvent 4 fois, à chaque fois tournés de 90 degrés. Pour Cnossos, le labyrinthe avec ses 4 directions possibles est en fait 4 labyrinthes dont il ne connaît pas du tout la relation de rotation entre eux. Cnossos fait toujours son premier mouvement vers le haut. Ensuite, il choisit l'un des 4 labyrinthes correspondant à la bonne rotation.

Voici maintenant sur un schéma les correspondances entre la position des murs et les valeurs dans les tableaux, pour la rotation 0 (cad pas de rotation par rapport au plan officiel). N'oubliez pas présence d'un mur = bit à 1.

les murs horizontaux

les murs verticaux

Pour faire simple, un fichier excel est utilisé. Avec une représentation du labyrinthe, il génère directement les tableaux de constantes qui représentent les murs en C, dans les 4 rotations. Une petite manipulation manuelle est néanmoins nécessaire.

Voici le fichier excel en question.

On peut donc rapidement (si on ne se trompe pas) adapter Cnossos un nouveau labyrinthe (de même taille). Il faut changer le fichier excel, réintroduire les nouveaux tableaux dans plan.h et recompiler.

A noter tout de même que des murs ont été rajoutés dans le 16*16 par rapport au 9*9 (partie horizontale). Ces murs permettent en fait d'éviter que dans certain cas le programme de Cnossos ne calcule le plus court chemin vers la sortie... en sortant par l'autre sortie pour passer par l'extérieur ! Si, c'est possible !


Les systèmes de cordonnées de Cnossos

Au niveau des coordonnées de Cnossos, c'est relativement complexe.

Cnossos stocke dans les variables globales entières robot_casex et robot_casey ses cordonnées en terme de cases (par rapport au tableau de 16*16). Au départ il commence artificiellement en 8*8. Ensuite quand il connaît sa position, il change cette valeur.


Ces deux valeurs peuvent donc devenir aussi une seule valeur robot_casex + 16 * robot_casey entre 0 et 255. J'aurais pu tout harmoniser et n'utiliser que cette représentation qui est finalement assez pratique, mais j'ai laissé courir par manque de temps.

robot_direction indique la rotation du robot en terme de cases, de 0 (direction nord) à 3. Ca sert surtout au début quand Cnossos fait des mouvements case par case, avec des rotations de 90deg.

rotation_plan_general est aussi une rotation de 0 à 3, mais en réalité c'est une variable qui va être utilisée un peu partout pour dire lequel des 4 labyrinthes est utilisé ou plutôt donc quelle rotation du labyrinthe est prise en compte. Le choix se fait quand le robot trouve où il est.

Il y a ensuite x_robot et y_robot, qui sont tous des flottants, et représentent la position réelle de Cnossos en cm, à l'intérieur d'une case donnée (de 0 à 40cm). angle_robot est l'angle en radiant par rapport au nord, toujours en flottant.

Toutes les old_value_* gardent en mémoire les positions antérieures de Cnossos. Cela permet de se recaler non pas sur la dernière position du robot, mais sur une position en léger différé, pour tenir un peu compte du retard des capteurs SHARP.

Ce double système de coordonnés entier/float complique un peu et je ne suis pas intimement persuadé de sa grande utilité, mais il me permettait de simplifier ensuite mes calculs en utilisant l'un ou l'autre suivant les besoins. De plus la précision du float entre 0 et 40 reste maximale.


Description complète de cnossos.c
Le void main ()

Le main fait d'abord tout une phase d'initialisation (je n'utilise pas les initialisations des variables globales sous la forme int i = 0 parce qu'il faut un linker spécial qui va prendre plus de FLASH pour le programme) puis il règle les E/S (en particulier ouverture de la com SPI), il calcule le CRC (voir ci-dessous) et il affiche un petit coucou sur l'afficheur LCD.

Vous remarquerez qu'à ce niveau il y a deux modes : Soit Cnossos part en mode "rapide", soit il part en mode "lent". Ce qui différencie les deux modes, c'est comment le PIC a démarré. S'il a fait un reset à froid (cad le PIC était éteint et il a été rallumé) il part en mode "rapide". Sinon, si le pic a fait un reset à chaud (reset actionné sur la carte électronique), le mode est "lent". Tout ceci me permettait de lancer Cnossos sur un mode très lent (enfin, pas si lent que ça par rapport aux autres robots) au cas où son 1er tour en rapide se serait mal terminé. Il suffisait pour ça que j'actionne le bouton reset.

Le main lance ensuite la 1ère conversion analogique - numérique (qui s'entretient ensuite via les interruptions) et va ensuite bouger le servo qui commande le tir de la balle de ping-pong avec la fonction bougeServo. En fait ce mouvement permet de re-enclencher correctement le système de tir, en particulier si le robot a été arrêté avant la fin (sinon il re-enclencher automatiquement dès qu'il a fini son parcours).

Vient ensuite pogramAllEEProm qui va envoyer au PIC esclave les paramètres du PIC définis dans le eepromValue.h. Voir ci-dessous.

Le main envoie ensuite 2 commandes au PIC esclave via la fonction sendCommand( ). Les valeurs envoyées sont en fait les coefficients des pentes d'accélération et de décélération que le robot va ensuite utiliser partout dans ses mouvements, à travers la gestion du PID du PIC esclave. Si cette valeur était réglée plus petite, le robot patinait tout le temps !

Le setSpeed(0,0) arrête les deux moteurs. C'est au cas où le PIC a fait un reset avec un robot qui bouge toujours à 1m/s !

La fonction démarrage est ensuite là pour attendre que le bouton de départ soit activé, puis relâché ! C'est le relâchement qui fait bondir le robot. Il y a une petite détection d'anti-rebonds. A noter que pendant l'attente, les distances mesurées par le robot sur les 4 capteurs sont affichés, ce qui est une aide au placement du robot au milieu de sa case du labyrinthe.

Le main appelle ensuite getDistance, fonction qui va utiliser les valeurs obtenues en continue par les conversion A/N pour obtenir les 4 distances des murs par rapport au centre de Cnossos.

Tout de suite après vient ajouteCaseTableau qui, suivant les distances obtenues par les 4 capteurs, va remplir les tableaux de "murs" de proximité de Cnossos pour lui permettre de trouver ensuite où il se trouve. Voir ci-dessous pour les détails.

avance40cm() va maintenant dire au PIC esclave d'avancer le robot de 40cm exactement.

recale_wait à "2" fait que Cnossos va, au fur et à mesure de ce mouvement de 40cm, recalculer sa position X,Y,angle. Par contre il ne va pas faire de recalage aux infra-rouges de cette position (j'ai jugé qu'au départ, il fallait mieux faire confiance à l'odométrie).

main_previousCase = 16 va garder en mémoire le fait que le robot vient d'une case en +16 (+1 en y). Cela sert ensuite pour interdire par défaut les demi-tours.

On refait ensuite getDistance() et ajouteCaseTableau() avec les nouvelles coordonnées du robot. Nouveauté cette fois-ci, on lance recherchePosRobot, fonction compliquée qui cherche si la position du robot est connue ou non de façon sure, suivant les murs qu'il a pu voir. Ce test n'est pas fait avant le mouvement des 40cm, car le règlement demande qu'obligatoirement le robot bouge d'une case après le départ. Pas besoin de trouver où on est pour commencer éventuellement sur une rotation vers la sortie (ce qui était parfois possible).


robot_knows_pos va être mis à 1 par recherchePosRobot si Cnossos est sûr de sa position !

A ce stade là, soit il est sûr de sa position au bout du mouvement d'une case, soit il va boucler dans un while jusqu'à ce qu'il en soit sûr.

Voici ce que fait Cnossos dans ce while(robot_knows_pos!=1) :

1er test, if (robot_knows_pos!=2) -> en fait si robot_knows_pos = 2 cela veut dire que Cnossos a ses murs enregistrés depuis le début de son mouvement qui ne correspondent à rien dans les 4 labyrinthes qu'il possède en mémoire ! Il fait alors un reset de tout ce qu'il a enregistré, et recommence à chercher les murs... il n'y a donc pas de mouvement pour cette fois.

Sinon, il y a donc mouvement. Oui, mais lequel ? Ce n'est pas si simple... Voici la subtilité trouvée pour que Cnossos cherche toujours intelligemment sa prochaine case. Il fait en fait appel à calculPlusCourtChemin qui est, comme son nom l'indique, la fonction qui trouve le chemin le plus court vert une autre case. Oui mais à ce stade Cnossos ne sait pas où il est. Comment peut-il donc calculer un chemin ? En fait Cnossos utilise l'une des positions possibles dans le labyrinthe trouvé par recherchePosRobot (qui à ce stade donc en a trouvé plusieurs). Cette position définit donc un trajet vers la sortie qui est, pour autant que Cnossos puisse en juger en fonction des murs qu'il connaît actuellement, optimal.

En réalité, Cnossos ne va garder qu'un mouvement de ce chemin le plus court qu'il a calculé. Ca sera son mouvement suivant.

Mais ce n'est pas si simple. Pour éviter que Cnossos ne fasse des demi-tours (il pourrait en fait boucler indéfiniment sur des demi-tour !) la case d'où Cnossos vient est interdite pour le calcul du plus court demain (c'est main_actualCase + main_previousCase). Or il peut arriver que Cnossos soit dans un cul de sac, et que sa seule solution soit alors de faire demi-tour. A ce moment-là, calculPlusCourtChemin va renvoyer une erreur comme quoi il n'a pas trouvé de plus court chemin (buffer3[0] == 255).

Dans ce cas où il ne trouve rien, il fait un demi-tour sur lui-même avec tourne90deg(2). Sinon trouveRotationNextCase lui indique de combien il doit tourner pour aller à la case suivant du calcul de plus cour chemin, et faitRotation fait réellement cette rotation au robot.

La rotation étant faite, Cnossos avance de 40cm, et réactualise main_previousCase suivant son angle.

Il relance ensuite getDistance(), ajouteCaseTableau() et recherchePosRobot() pour voir si maintenant, avec les murs qu'il voit, il n'est pas sûr de l'endroit où il se trouve. Si il n'est pas sûr, il continue à boucler.


Pour la suite, on a donc Cnossos qui sait exactement où il se trouve dans le labyrinthe. Il sait aussi dans quelle rotation de labyrinthe il se trouve. Il affiche tout ça sur l'écran LCD avec afficheEmplacement().

Maintenant qu'il sait où il est, il faut foncer ! et tout d'abord foncer vers la case d'où il tire la balle (une seule case, je me suis simplifié la vie).

calculPlusCourtChemin est à nouveau appelé pour calculer le chemin le plus rapide vers la case de tir (stockée dans tir_x[rotation_plan_general] et tir_y[rotation_plan_general]).

Ce calcul fait, et si il a réussi, Cnossos va tout d'abord s'orienter correctement en réalisant une rotation pour se mettre dans le bon sens. En effet, l'expérience montre que l'asservissement de Cnossos doit toujours se faire sur les rotations quand il est déjà en mouvement. Il est calibré pour ça ! Il faut donc qu'au départ il soit correctement orienté pour partir tout droit et éviter une rotation à l'arrêt, ce que fait le main.en enchaînant trouveRotationNextCase et faitRotation.

Ensuite vient la partie intéressante où Cnossos va enchaîner case après case pour arriver à la case de tir.

Pour ça, le main dépile les cases les unes après les autres depuis le buffer "buffer3" qui a été rempli par calculPlusCourtChemin().

Il met la case à atteindre dans c_caseNext, puis il appelle la fonction allera() qui prend en paramètre le x,y de la case à atteindre et aussi un 3ème paramètre de vitesse qui peut être soit 0 (normal, 80 cm/s) soit 1 (précis - cad que l'asservissement va moins vite mais va aussi essayer d'atteindre le point plus précisément, c'est utilisé pour la dernière case) soit 2 (1m/s, utilisé si le robot à 3 cases tout droit devant lui). Cette fonction allera() va demander au robot d'aller au centre de la prochaine case. Elle est décrite plus loin.

Le main continue jusqu'à ce que toutes les cases aient été atteintes.

On est à ce moment là sur la case de tir.

Il faut donc ensuite tirer la balle !

Tout d'abord, le robot s'arrête avec un setSpeed(0,0);

A quoi sert la boucle for après ?

Et bien en fait il faut attendre un peu que la commande d'arrêt fasse son effet. Sinon, on enchaîne sur la correction angulaire alors que le robot est toujours en train de ralentir, ce qui peut apporter pas mal de problèmes !

Donc du coup on fait plusieurs fois une petit pause, un recalcul de la position du robot avec AskForPos() et computeRealPosRobot(). Et on fait ensuite un petit recalage(). Toutes ces fonctions seront décrites plus loin.


Ensuite vient toute une phase qui va permettre à Cnossos de se positionner correctement avant de lancer la balle.

Il y a tout d'abord un cas particulier. Si Cnossos vient de la gauche (avec le labyrinthe vu en rotation 0) il se recale en x. Pourquoi ? Parce que sinon l'arrivée avec l'asservissement donne des choses un peu bizarre (même en précis) et recaler en x à ca niveau permet de maximiser les chance de tir réussi. Par contre s'il vient d'en haut ou d'en bas il n'en a pas besoin, et de toute façon faire une rotation de 90deg pour faire ce recalage prendrait trop de temps.

Donc ici il appelle recaleAngulaireHorizontal() qui va donc faire tourner le robot pour qu'il se place bien avant d'avancer. Il calcule ensuite de combien il doit avancer suivant le rotation_plan_general et il n'a plus qu'a faire un avance40cm() qui cette fois-ci n'avance pas de 40cm comme son nom l'indique, mais de la valeur de la variable avanceFloat.

Le robot étant maintenant recalé en x si c'était nécessaire, il peut maintenant tourner sur lui-même pour se placer correctement perpendiculairement à la case centrale. Il utilise pour ça rotationAngle() avec un calcul qui dépend de la rotation du labyrinthe.

Ensuite, il recale cette fois-ci en y suivant le même principe, en appelant avance40cm().

Vient ensuite rotationAngle(-DELTA_ANGLE_TIR) qui bouge légèrement le robot sur la droite pour améliorer l'angle du tir.

bougeServo(20, 30) actionne le servo pour tirer la balle !

rotationAngle(DELTA_ANGLE_TIR) fait revenir le robot dans le même angle qu'initialement, juste après le tir de la balle. Normalement à ce stade là la balle n'est toujours pas retombée.

Il faut maintenant que le robot aille vers la sortie. On refait en fait exactement la même chose que pour aller vers la case de tir. Appel de calculPlusCourtChemin, puis on dépile les cases les unes après les autres en appelant allera().

Arrivée sur la dernière case, le robot s'arrête tout d'abord, puis se recale horizontalement, et avance franchement de 30cm.

Enfin le robot après s'être arrêté ré-enclenche le servomoteur pour le tir. Ayé, c'est fini ! Le robot finit en while infini, obligatoire sinon le main reboucle.

Voici pour le main, maintenant va suivre la description des principales fonctions.

 

calcul du CRC

Cette petite fonction calcule en fait un CRC simple qui va "signer" le contenu de la flash du PIC. A quoi ça sert ? Et bien ce CRC va être affiché par le robot au démarrage, et il va être obtenu aussi par simulation sous mplab. Si les deux chiffres sont identiques, c'est que le contenu de la flash du PIC correspond bien à ce qui a été compilé.

Mais alors vous me direz pourquoi faire ? Pendant le concours j'utilisais un portable un peu récalcitrant qui voulait bien programmer le PIC, mais pas le vérifier (en fait la lecture ne marchait pas par le port série). Du coup l'utilisation du CRC me permettait de vérifier que le PIC était bien programmé !

Interruption

L'interruption permettait sur Cnossos de faire les conversions analogiques - numériques en tache de fond, et ensuite d'avoir toujours une valeur à jour pour calculer les distances par les capteurs SHARP.

Donc l'interruption ici va successivement lancer les 4 conversions A/N et reboucler.

Un truc à retenir : il n'y aucun appel de fonctions dans cette interruption. Il faut le savoir : sous C18, faire des appels de fonction dans l'interruption est assez suicidaire. Enfin en tout cas dans la version que j'avais. En effet, les fonctions en interruption ou non peuvent mélanger leurs registres temporaires ce qui au final fait des choses pas terribles !

pogramAllEEProm

Cette fonction lance successivement programmingEEPromPic2 pour chaque paramètre à envoyer dans l'EEPROM du PIC esclave (valeur et position).

Il y a surtout les paramètres du PID.

En fait, la fonction programmingEEPromPic2 lit le paramètre dans ce PIC esclave, et, si il y a besoin de le changer, envois une instruction pour le modifier via le bus SPI.

Je préférais faire comme ça (le maître qui remet les paramètres comme il faut sur l'esclave) parce que j'avais des problèmes pour changer les valeurs "à la main" dans l'EEPROM du pic esclave. Et puis comme ça pour tout le réglage de ces paramètres du PID je ne touchais qu'au PIC maître.

sendCommand

sendCommand est une fonction assez bas niveau qui va envoyer une commande de 4 octets pour le PIC esclave via SPI.

Il reçoit aussi une réponde (soit la valeur OK, soit une valeur d'erreur).

A noter qu'il y a des pauses entre chaque envois. En fait, tout vient de la gestion du bus SPI. On ne peut pas savoir quand le PIC esclave a fini le traitement et a lu la réponse ! Du coup le pic maître fait une petite pause entre chaque octet, ce qui laisse le temps à l'esclave pour traiter.

setSpeed

Petite fonction là aussi qui va envoyer les commandes au PID du PIC esclave pour la vitesse des moteurs. Rien de sorcier à part qu'il faut envoyer la bonne commande en fonction des sens de rotation voulus.

avance40cm

Cette fonction, qui au départ était utilisée pour faire avancer le robot de 40cm exactement (la longueur d'une case) après calibration, est maintenant utilisée aussi pour bouger de X cm (stockée dans avanceFloat).

La commande de mouvement est envoyée au PIC esclave, et ensuite on appelle waitForStop qui va attendre la fin de cette commande (en poolant le pic esclave) et en réactualisant aussi si nécessaire la position x, y, angle du robot.

ajouteCaseTableau

On arrive sur des choses plus compliquées.

On a ici la distance des 4 capteurs dans les variables resultCapteur.

On appelle ajouteCaseCapteur() pour chaque rotation (on tourne petit à petit pour chaque capteur).

ajouteCaseCapteur() est plus compliquée.

Avec une bonne marge, on définit caseDist en 0, 1 ou 2 suivant la distance en case du mur qu'on voit (ou pas) devant le capteur en question.

Ensuite on va updater les tableaux en ram robot_mur_vert_value, robot_mur_vert_mask et robot_mur_horiz_value, robot_mur_horiz_mask. qui sont des tableaux de 16*16 bits correspondant au plan du labyrinthe. On va stoquer dedans ce que voit Cnossos au fur et à mesure de ses déplacements (jusqu'à ce qu'il sache où il est).

Les tableaux sont déjà découpés en horizontal et vertical, comme déjà vu plus haut. Mais il y a aussi le "mask" et le "value". En fait il y a 3 états possibles. Soit Cnossos ne sait pas qu'il y a un mur ou pas, soit il sait qu'il y a un mur, soit il sait qu'il n'y en a pas. Le "mask" est à 1 si Cnossos sait s'il y a un mur ou non. Puis la value est à 1 s'il y a un mur, à 0 sinon.

Donc en gros, on a ça comme possibilités :

Significations pour les murs
bit dans mask
bit dans value
Cnossos ne sait pas ce qu'il y a
0
peu importe, mais initialisé à 0.
Cnossos sait qu'il y a un mur
1
1
Cnossos sait qu'il n'y a pas de mur
1
0

 

Donc cette fonction remplit les tableaux de bits suivant si les capteurs ont vu ou pas des murs.

recherchePosRobot

C'est ici que Cnossos va essayer de trouver où il se trouve, en analysant les informations enregistrées dans ajouteCaseTableau (voir la fonction au-dessus).

1ère chose, le programme analyse les valeurs min et max où se trouve des bits à 1 dans "mask". Vu qu'il n'y a pas de mur ailleurs, il n'y a pas besoin de continuer l'analyse.

Vient ensuite des longues boucles imbriquées, qui vont trouver ou non des correspondances entre ce qui est enregistré en RAM et les 4 tableaux de labyrinthe (les 4 rotations).

La 1ère boucle parcourt les 4 labyrinthes. Les 2ème et 3ème bouclent sur toutes les valeurs de départ possible de Cnossos. A noter qu'il n'y a que 7*7 cases et pas 9*9, car d'après le règlement Cnossos ne peut pas démarrer sur une case en contact avec un mur relié à l'extérieur (en fait il y a un peu moins de 7*7 cases, mais se limiter au 7*7 simplifie).

Sur la dernière boucle, le programme va faire défiler toutes les positions possibles des tableaux en RAM.

Donc ici on teste bêtement toutes les solutions possibles en essayant de faire correspondre le tableau en RAM avec ce qu'on a sur les 4 rotations du labyrinthe connu.

Comment se fait cette correspondance ? On va utiliser en fait de façon pratique la représentation en RAM du labyrinthe en bit à bit.

r_decMask et r_decValue vont contenir les valeurs mask et value (ce qu'a mesuré le robot sur son environnement) pour la position à tester (avec des décalages de bits).

r_mur contient la valeur des bits du mur du labyrinthe.

Le test de "match" se fait alors de la façon suivante :

(r_mur & r_decMask) == r_decValue

Donc en gros on prend tous les bits de r_mur, on fait un ET bit à bit avec r_decMask, ce qui nous donne tous les murs que Cnossos aurait du voir. On compare ensuite avec r_decValue qui contient les murs qu'a vu Cnossos. Donc si les murs vus par Cnossos et les murs de la position du labyrinthe testé correspondent, c'est bon.

Plus précisément, on arrête dès qu'il y a au moins un mur d'écart ( (r_mur & r_decMask) != r_decValue ).

Si il n'y a pas de mur d'écart pour un une position donnée et une rotation du labyrinthe donnée, on peut alors dire que ce qu'a vu Cnossos correspond bien à cette position de départ. Il enregistre alors ce qu'il a trouvé dans r_foundRot, r_foundx, r_foundy, et il incrémente r_numberFound.

Le problème maintenant, c'est qu'il peut y avoir plusieurs endroits qui correspondent. A ce moment-là, r_numberFound va être incrémenté plus d'une fois.

Donc à la fin 3 cas se présentent :

Soit r_numberFound est à 1, ce qui veut dire qu'on a bien trouvé un et un seul endroit dans le labyrinthe. Cnossos sait où il est !

Soit r_numberFound est > à 1, ce qui veut dire qu'on a plusieurs endroit possibles. Cnossos ne sait pas encore où il est, il il devra encore avancer ...

Soit r_numberFound est à 0. C'est mauvais signe, rien ne correspond ! Du coup on y va à la hache : on efface tout ce qu'il y a dans la RAM et on retourne une erreur.

 

calculPlusCourtChemin

Voici une fonction intéressante, celle qui calcule le plus court chemin entre deux cases.

La méthode utilisée est un classique Dijkstra avec bucket un peu simplifié car tous les couts sur les arcs sont à 1. Il était certainement possible de faire encore plus simple.

On se sert ici des buffers1 2 et 3 (256 octets max chacun). Le buffer 1 contient le noeud parent (un noeud est ici une case du labyrinthe) le buffer2 contient le coût pour atteindre le noeud en question. Enfin le buffer3 contient les buckets.

A l'initialisation, on met la case actuelle dans le buffer3 (1ere case du bucket),on met son noeud parent à 255 et son coût à 0.

Ensuite la fonction boucle comme suit :

On dépile une à une les cases dans le bucket.

Si c'est la case d'arrivée, on a fini et on sort.

Sinon, pour chaque case, on regarde si on peut aller à la case suivante (présence de mur ou non) en commençant par celle qui a la même orientation que le robot (ceci permet en fait de faire que le robot aille de préférence tout droit au départ en cas d'égalité sur deux chemins).

S'il n'y a pas de mur, et si il n'y a pas déjà un chemin de coût moindre pour atteindre cette case, elle est ajoutée dans la pile des buckets, la case parente est notée comme étant la case d'origine, et le coût pour l'atteindre est noté comme le coût de la case parente plus un.

Et ainsi de suite.

Donc quand la case d'arrivée est sortie du bucket, la boucle finit et la fonction remet les cases de l'itinéraire les unes après les autres dans buffer3, en remontant de parents en parents.

Si toutes les cases ont été analysées sans résultat, la fonction retourne une erreur. Ca arrive normalement quand il y a une case interdite et qu'il faut obligatoirement passer dessus (un demi-tour généralement).

computeRealPosRobot

Petite fonction sympathique qui va calculer la position du robot en x, y, angle.

Pour cela elle connaît avance_gauche et avance_droite qui correspondent aux 2 valeurs cycliques retournés par le PIC esclave sur l'avance en nombre de pas des roues gauches et droites. A partir de ça, la fonction va tout d'abord calculer de combien le robot a réellement bougé en pas sur la gauche et la droite.

C'est les lignes :

p_delta_gauche = p_avance_gauche2 - p_last_avance_gauche2;
if (p_delta_gauche<-32768)
    p_delta_gauche = (long)65536 + p_delta_gauche;
if (p_delta_gauche>32767)
    p_delta_gauche = p_delta_gauche - (long)65536;

qui font ce calcul.

Ensuite on calcule l'avance à gauche et à droite en flottant avec p_dx et p_dy en multipliant par les coefficients FACTEUR_AVANCE_X et FACTEUR_AVANCE_Y. Ces coefficients sont obtenus par calibration. On prend le robot et on règle le nombre de pas pour qu'il avance exactement de 40cm. Quand c'est bon, on a plus qu'à diviser 40cm par le nombre de pas.

On calcule aussi le cosinus et le sinus de l'angle actuel du robot.

Ensuite deux cas de figure se présentent. Soit le robot a bougé du même nombre de pas à gauche et à droite, et à ce moment là il avance en X de - p_dx*a_si, en Y de p_dx*a_co et il ne bouge pas en angle, soit il a bougé d'un nombre de pas différent, et à ce moment là la formule est la suivante :

L'angle du robot bouge de : p_alpha = (p_dy -p_dx) * INVERSE_DISTANCE_ROUE;
La variable intermédiaire C vaut : p_C = (p_dx/p_alpha + DISTANCE_ROUE_DIV_2);
La variable intermédiaire DX vaut : p_DX = (cos(p_alpha)-1) * p_C;
La variable intermédiaire DY vaut : p_DY = sin (p_alpha) * p_C;
Le robot avance en X de : p_more_x = - p_DY * a_si + p_DX * a_co;
Le robot avance en Y de : p_more_y = p_DY * a_co + p_DX * a_si;

DISTANCE_ROUE_DIV_2 et son inverse INVERSE_DISTANCE_ROUE sont obtenues par calibration (ce n'est pas mesuré sur le robot!), après calibration des 40cm. On fait en fait tourner le robot 10 tours sur lui-même, et on essaie de le faire revenir bien droit. Quand c'est bon, on obtient par calcul cette fameuse distance.


Voir ici un petit schéma explicatif du calcul (attention, sur le schéma il faut en fait prendre d comme la distance entre A2 et B2, et pas A2/O2).

Pour information, ces formules ont été déjà utilisées sur Coredump (robot INT 1998). D'ailleurs j'ai bien repompé le code, et je l'ai juste un poil adapté au PIC.

On change ensuite les coordonnées du robot, on remet son angle entre -Pi et Pi.

On fait ensuite le changement de case. Si X ou Y sont <0 ou >40cm, la case du robot change, et on remet X ou Y comme il faut dans cette nouvelle case.

Enfin on calcule ce qu'on appelle le "point futur". C'est en fait non pas la position actuelle du robot, mais sa position qu'il aurait dans quelques itérations si il continuait avec ce même mouvement en X et en Y.

Pour cela on utilise un COEF_ANTICIPATION (ici réglé à 3). Donc le robot va ensuite calculer son asservissement sur ce point qui correspond à 3 fois le mouvement actuel. Ca permet de commencer à tourner plus tôt, et donc mieux tourner dans les virages.

 

allera

C'est ici que se fait l'asservissement du robot pour une case donnée.

Le robot doit sortir de la fonction uniquement après avoir atteint le milieu de la case demandée (approximativement).

Pour cela, il calcule d'abord la position du point à atteindre dans le repère du robot, après avoir demandé une réactualisation de la position du robot (AskForPos et computeRealPosRobot).

Ensuite, en utilisant de simples comparaisons, il calcule la vitesse à envoyer sur les deux roues.

Voici un schéma explicatif :

Suivant la position du point à atteindre par rapport au centre du robot, telle ou telle vitesse est appliquée.

Si on est dans le carré central, on considère le point comme atteint !

Les marges et les vitesses dépendent de la vitesse de l'asservissement choisi (lent ou rapide). Tout ça a été calibré après de longues journées de tests... Si le robot vire trop rapidement, il risque de toucher un mur à l'intérieur du virage. S'il vire trop doucement il risque de toucher à l'extérieur. S'il va trop vite, il risque de patiner. Evidemment, plus le robot va vite et plus c'est délicat de faire le réglage, et plus il faut de temps !

Tout cet algorithme d'asservissement, qui permet de lisser assez simplement les virages sans trop se prendre la tête, vient en fait directement lui aussi de Coredump.

Si ensuite la vitesse demandée est la plus rapide, on rajoute un petit plus sur la vitesse des deux moteurs.

Les vitesses sont envoyées par un setSpeed.

Reste plus qu'à faire un recalage (voir ci dessous) et à boucler jusqu'à atteindre le point.

Quand c'est bon, on sort.

A noter que toute la boucle se fait en 15ms à peu près.

AskForPos

AskForPos est une petite fonction qui va demander au PIC esclave de combien le robot a bougé (en fait la fonction retourne dans avance_droite et avance_gauche la valeur d'un tableau cyclique du PIC esclave). Elle est appelée périodiquement et assez rapidement. Tout de suite après, computeRealPosRobot est généralement appelé.

 

recalage


Voici enfin l'une des fonctions les plus compliquées (sinon la plus compliquée) du programme de Cnossos.

Cette fonction permet de corriger la position du robot suivant les information des 4 capteurs sharps, quelque soit la position du robot.

Vous l'avez compris, il s'agit surtout de calculs trigonometriques.

On trouve d'abord les distances sur les capteurs ( getDistance ).

On lance ensuite recalculeCosSinRecalage qui va initialiser certaines valeurs pour le calcul, en particulier des cos et des sin qu'on ne va calculer qu'une fois.

On arrive ensuite sur calculIntersection. Cette belle fonction va calculer ce qui, théoriquement, devrait être vu par les 4 capteurs suivant la position que le robot croit avoir dans le labyrinthe.

Pour simplifier, et pour ne pas trop forcer sur la précision des sharps qui décroît rapidement, on ne teste que les murs présents à moins d'une case du robot.

Je vous passe les calculs, c'est des intersections de droites.

testCase est utilisé ici pour retourner s'il y a un mur ou non.

Quand ceci est calculé, on lance calculMatchGeneral qui va calculer un "score" entre les valeurs théoriques calculées, et les valeurs pratiques réellement mesurées. Plus ce score est petit et plus on est proche.

Ce n'est pas trivial. La fonction calculMatchGeneral, qui calcule ce score va tout d'abord éliminer les valeurs aberrantes, et puis va donner un calcul de distance qui est exponentiel. Pourquoi exponentiel et pas linéaire ?

Prenons un exemple. Admettons que le robot se trouve entre deux murs distants de 38cm (au lieu de 40 en théorie, sans prendre en compte l'épaisseur). Les valeurs théoriques sont 20cm et 20cm à gauche et à droite.

Si le robot se trouve bien au milieu (et bien perpendiculaire), on a 19cm d'un coté et 19 de l'autre. S'il est décalé d'un cm sur la droite, on a 18cm d'un côté et 20 de l'autre. Et inversement.

Comparons maintenant les scores de distance que l'on obtiendrait :

Distances Score linéaire Score exponentiel
19-19
abs(20-19) + abs(20-19) = 2
2
20-18
&_abs(20-20) + abs(20-) = 2
4
18-20
abs(20-18) + abs(20-20) = 2
4

 

Contrairement au score linéaire, on a bien ici le 19-19 qui est inférieur aux 20-18. Et effectivement, en terme de position du robot, il vaut bien mieux qu'il soit au milieu !

Le calcul de cet "exponentiel" se fait avec une bête boucle.

Bon maintenant on somme les 4 scores des 4 capteurs. Que fait-on avec ?

Et bien on va tout bêtement considérer 9 décalages de la position théorique du robot (1.5cm de tout les côtés), et à chaque fois on va recalculer les distances de murs théoriques, et le score entre la théorie et ce qu'ont renvoyés les capteurs.

Evidemment dans tout ça, on va garder le décalage qui donne le meilleur score (le plus petit), qui est donc celui qui correspond le mieux à la position du robot ! (en tout cas, qui correspond le mieux aux informations renvoyées par les capteurs de distance).

Ce décalage, on va en appliquer une infime partie (DELTA_ADD_RECALAGE, ici 0,25 mm) à la position théorique du robot. C'est la succession de tous petits recalages qui va continuellement corriger la position du robot par rapport aux résultats des capteurs ! C'est le principe de base. On ne corrige que très peu, mais souvent. On s'évite ainsi des problèmes dus à la mauvaise précision des capteurs, ou d'éventuels problèmes ponctuels. Au pire même si un capteur dit un peu n'importe quoi pendant un coup, ça ne fera que 0,25mm d'erreur !

Ok, tout ça c'est bien, mais ça ne corrige que x ou y. Or, l'erreur la plus importante en terme de dérive est l'erreur en angle. Comment corriger alpha ?

Tout d'abord la même méthode que pour x et y n'est pas utilisable sur alpha. Trop peu précis, problèmes de symétries, etc.

La solution retenue est simple, et marche bien :

Si le robot est trop sur la droite, il va corriger et se décaler sur la gauche en x.

Mais en même temps, on va appliquer une toute petite correction angulaire vers la gauche. On part en fait du principe que si le robot est trop sur la droite, cela veut dire qu'il se dirige effectivement un peu trop sur la droite, et donc en plus de décaler en x ou y, il faut aussi décaler en alpha sur la gauche.

Ce décalage, de DELTA_ADD_RECALAGE_ANGLE, ne se fait que sous certaines contraintes angulaires du robot (il doit aller franchement dans une des 4 directions).

Ce recalage angulaire termine la fonction de recalage !

recaleAngulaireHorizontal

Demande une rotation par rotationAngle pour remettre le robot horizontal. L'angle demandé dépend du labyrinthe utilisé.

rotationAngle

rotationAngle va faire tourner Cnossos d'une certaine valeur angulaire. Pour cela, le programme calcule déjà la valeur en pas pour le PID par rapport au calibrage d'un tour, et envoie une commande de rotation suivant le sens.

L'appel à waitForStop() fait attendre ensuite la fin de la commande.

 

 

Conclusion

Laissez-moi vous dire que si vous avez lu tout ça, tout en regardant le code, vous êtes assez motivé pour vous lancer dans le programme d'un robot au moins aussi compliqué ;-)

J'espère que tout ceci va vous servir, et en tout cas vous donner une idée du travail que demande la réalisation d'un robot comme Cnossos.

Evidemment, vous êtes libre de reprendre tout ce qui vous interresse !

Amusez-vous bien sur vos robots ;)

Julien - pour les Fribottes.

 


Complétez cette page, posez vos questions et remarques ici : WiKiFri

Page http://fribotte.free.fr/bdtech/cnossos/robot_cnossos5.html modifiée le 13/09/2004.
Copyright fribotte@free.fr, libre de droit pour toute utilisation non commerciale.
Reproduction autorisée par simple mail