View
221
Download
0
Category
Preview:
Citation preview
Page 1 sur 110
Semestre 4
Parcours ROBOS
Année 2014
Rapport de projet de semestre S4
Coupe de robotique des IUT à Vierzon
Josué Ferreira, Alexandre Chhean, Fabien Lesueur, Florian Biazi
Date de réception au secrétariat pédagogique : ..../..../.... à ....h....
Impression : 30/03/14
Page 2 sur 110
Table des matières
Résumé .................................................................................................................................................... 4
Abstract ................................................................................................................................................... 4
Mots-clés ................................................................................................................................................. 4
Préface ..................................................................................................................................................... 5
Introduction ............................................................................................................................................. 6
1. Déplacement stratégique et précis du robot ..................................................................................... 8
1.1 Asservissement des moteurs du robot ........................................................................................... 8
1.2 Programmation de l’asservissement sur PSOC ........................................................................... 10
1.3 Calcul de la trajectoire avec des rayons de courbure................................................................... 12
2. Carte capteur : Détection d’obstacle.................................................................................................. 15
2 .2 Structure de la carte capteur ....................................................................................................... 15
2 .3 Principe de fonctionnement de la carte capteur .......................................................................... 16
2 .4 Choix des capteurs ...................................................................................................................... 16
2 .4 .1 Capteur de distance............................................................................................................. 17
2 .4 .2 Capteur de zone .................................................................................................................. 17
2 .5 Disposition des capteurs ............................................................................................................. 18
2 .7 Programme de détection d’obstacle ............................................................................................ 19
2 .7.1 Fonction conversion numérique en cm. ............................................................................... 19
2 .7 .2 Fonction de comparaison .................................................................................................... 19
2 .8 Conclusion .................................................................................................................................. 21
3. Repérage sur le terrain ....................................................................................................................... 21
3.1 Odométrie .................................................................................................................................... 21
3.2 Balise Infrarouge émission .......................................................................................................... 22
3.2.1 Table d’émission .................................................................................................................. 23
Étudiant auteur de cette partie : Florian Biazi ............................................................................. 24
3.2.2 Modulation ........................................................................................................................... 24
3.2.3 Conception de la carte émission ........................................................................................... 25
3.2.4 Carte de réception ................................................................................................................. 26
3.2.4 Conversion du code .............................................................................................................. 26
3.2.5 Réception par machine à état ................................................................................................ 27
3.2.6 Fonctions sous interruption .................................................................................................. 27
3.2.7 Boussole ............................................................................................................................... 28
3.2.8 Conception de la carte réception .......................................................................................... 29
3.3 Gestion des AX12 pour le laser et éclate-ballon. ........................................................................ 30
3.3.1 Principe de fonctionnement du LASER ............................................................................... 30
Page 3 sur 110
Étudiant auteur de cette partie : Fabien Lesueur.......................................................................... 31
3.3.2 Traitement de l'information .................................................................................................. 31
Conclusion ............................................................................................................................................. 41
Annexe .................................................................................................................................................. 42
PCB de la carte pour les moteurs. ..................................................................................................... 42
Schéma de connexions des codeurs incrémentaux ............................................................................ 43
PCB de la carte capteur ..................................................................................................................... 44
PCB carte émission de la balise infrarouge .............................................. Erreur ! Signet non défini.
PCB carte réception de la balise infrarouge ............................................. Erreur ! Signet non défini.
PCB carte AX12 ....................................................................................... Erreur ! Signet non défini.
Fiche des identifiants CAN ...................................................................... Erreur ! Signet non défini.
Programme C asservissement et de l’odométrie sur PSOC ...................... Erreur ! Signet non défini.
Programme C des fonctions de calcul de trajectoire ................................ Erreur ! Signet non défini.
Programme C de la carte capteur.............................................................. Erreur ! Signet non défini.
Programme C de la carte émission infrarouge ......................................... Erreur ! Signet non défini.
Programme C de la carte réception infrarouge ......................................... Erreur ! Signet non défini.
Programme C de la gestion des AX12 ..................................................... Erreur ! Signet non défini.
Page 4 sur 110
Résumé
La coupe de robotique des IUT est une compétition annuelle réunissant des équipes d’étudiants des
IUT du génie électrique de France. Son objectif est de permettre aux étudiants de mettre en application
les connaissances acquises durant leurs études en électronique et informatique, et le travail d’équipe
lors d’un projet.
Étudiant auteur de cette partie : Josué Ferreira
Abstract
The IUT robotic cup is a tournament which gathers student teams of electronic engineering French
University Departments. The tournament is made in order to make students apply what they have
learned about electronics and computer science during the two year university diploma and working
team.
Étudiant auteur de cette partie : Josué Ferreira
Mots-clés
IUT (Institut Universitaire de Technologie), coupe de Vierzon, asservissement, calcul de trajectoire,
capteurs, balise infrarouge, servomoteurs AX12, laser.
Étudiant auteur de cette partie : Josué Ferreira
Page 5 sur 110
Préface
La coupe de robotique des IUT (Institut Universitaire de Technologie) est un événement annuel qui
réunit des groupes d’étudiants venant de tous les IUT de France. L’objectif de cet événement est
d’approfondir les connaissances des étudiants apprises au cours de leur formation mais aussi d’aborder
quelques notions de base liées au domaine de la robotique. Dans le cadre de la compétition, chaque
équipe conçoit un robot autonome capable de traverser un terrain de 8m par 8m semé d’obstacles pour
se rendre le plus rapidement possible au coin opposé. Les rencontres sont réparties en 10 phases : les
huitièmes de finales, quarts de finales, demi-finales et une finale. Lors d’une manche, 4 équipes
s’affrontent ce qui ajoute aux obstacles fixes 3 obstacles mobiles : les robots adverses. Les obstacles
sur le terrain sont organisés de manières symétriques mais la configuration n’est pas connue a priori.
Ils mesurent 15cm de haut mais la largeur et la longueur peuvent varier. Le schéma ci-dessous tiré du
règlement présente des exemples de géométrie du terrain. Les dimensions du robot sont limitées à
40cm de longueur par 30cm de large et 30cm de hauteur. Les étudiants disposent de 2min avant une
rencontre pour régler le robot dans le quart de cercle de départ. Au top départ, le robot doit partir après
qu’un étudiant retire la prise jack. Pour s’orienter sur la piste, chaque équipe a la possibilité de poser
dans le coin opposé une balise d’émission d’ondes ou un dispositif réflexion pour laser. Les robots ont
90 secondes pour arriver au coin opposé et les collisions intentionnelles sont interdites. Dès son arrivé
dans la zone opposée, le robot doit éclater un ballon pour signaler la fin de course. Le ballon est fixé
sur un tube dont la hauteur est comprise entre 30 et 31cm. D’après le règlement, il n’est pas obligatoire
que le robot soit entièrement sur la zone blanche mais au moins « l’un quelconque de ces constituants
au dessus de la zone blanche ». Si un robot s’arrête sur la zone d’arrivée d’un autre robot, il est « retiré
et considéré comme n’étant jamais arrivé ».
Étudiant auteur de cette partie : Josué Ferreira
So
urc
e :
règ
lem
ent
du
co
nco
urs
de
rob
oti
qu
e d
es I
UT
.
Page 6 sur 110
Introduction
Afin de participer à la compétition, nous avons à notre disposition un châssis de robot conçu par
l’équipe enseignante robotique de l’IUT de Cachan (voir la photographie ci-dessous). La géométrie du
châssis est de forme circulaire pour éviter les chocs ou le coincement du robot lors d’une rotation sur
lui-même. Les dimensions ainsi que les équipements de base (moteurs et batteries) sont conformes au
règlement du concours. La stratégie déployée pour le concours est basée autour de la conception de 5
cartes microcontrôleurs réalisant chacune une des fonctions suivantes : la gestion des capteurs, le con-
trôle des moteurs, le cerveau du robot (carte stratégie), la balise infrarouge et le contrôle du laser. Les
cartes communiquent entre elles via un bus CAN comme représenté dans la figure 3. L’objectif est de
préprogrammer dans la carte stratégie 4 parcours prédéfinis représentés sur la figure 2. Ces parcours
permettront au robot de passer les phases préliminaires dont la géométrie des terrains reste très simple.
Ensuite pour les terrains plus complexes avec plus d’obstacles, une communication avec la carte cap-
teur précisant la position des obstacles permettra à la carte stratégie de recalculer en temps réel la tra-
jectoire. Enfin, pour compenser les erreurs de calculs de position et de trajectoire, une balise
d’émission d’ondes infrarouges à 36 kHz et un module de réflexion catadioptrique pour le laser seront
placés au coin opposé pour calibrer le robot dans la direction de l’arrivée.
Dans un premier temps, nous allons expliciter l’asservissement du robot et les fonctions de calcul de
trajectoire, puis nous analyserons le système de détection d’obstacle et enfin le repérage sur le terrain.
Étudiant auteur de cette partie : Josué Ferreira
Figure 1. Châssis du robot Figure 2. Schéma des parcours prédéfinis
Page 7 sur 110
Figure 3. Schéma du système des cartes électroniques
Page 8 sur 110
1. Déplacement stratégique et précis du robot
1.1 Asservissement des moteurs du robot
Pour déplacer un robot à une certaine position sur le terrain sans intervention d’un quelconque opéra-
teur, il est nécessaire d’asservir les moteurs en position. Un asservissement en position consiste à cal-
culer l’erreur en position entre la position actuelle du robot et une position future souhaitée. En fonc-
tion de l’importance de l’erreur, on applique une vitesse différente au robot pour diminuer l’erreur
jusqu’à qu’elle soit nulle. Il est donc indispensable de posséder sur le robot un capteur renseignant la
distance parcourue par le robot. Or l’équipement du robot standard fourni par les organisateurs de la
compétition contient des moteurs équipés avec des codeurs incrémentaux. Les codeurs incrémentaux
sont des capteurs de contrôle de déplacement angulaire fixés mécaniquement sur l’axe de rotation des
moteurs. Ils disposent de deux crénelures optiques l’un en retard par rapport à l’autre (cf. figure 4). En
passant dans un système optique, diode et photodiode, les dentures créent en sortie de la photodiode un
signal d’impulsions électriques. Les deux crénelures permettent de distinguer le sens de rotation des
roues puisque elles génèrent des signaux déphasés (cf. figure 5). Si l’on connait le nombre
d’impulsions en un tour de roue et le périmètre de la roue, on peut mesurer la distance parcourue par
chaque roue. Ainsi, nous allons asservir la distance parcourue par les roues droite et gauche du robot.
En fonction, des relations entre ces deux distances et des vitesses il est possible de faire non seulement
des lignes droites mais aussi des rayons de courbures. Ces notions seront explicitées plus en détails
dans la sous-partie suivante.
Crénelures
Figure 4. Schéma d’un codeur incrémental Figure 5. Sens de rotation des roues
L’asservissement adopté est présenté sur la figure 6. On noter a que le correcteur choisit est un cor-
recteur proportionnel-intégral. En effet, un correcteur classique proportionnel diminue l’erreur entre la
consigne de position de référence et la position mesurée mais sans jamais l’atteindre. La partie inté-
grale du correcteur permet dans tous les cas d’atteindre la position de consigne à la seule condition
d’un coefficient choisi correctement pour éviter l’instabilité du système. Or, afin de contrôler la vitesse
des moteurs, nous utilisons une génération d’un signal PWM1. En fonction du rapport cyclique, il est
1 En anglais : Pulse Width Modulation. En français : Modulation de Largeur d’Impulsion
Page 9 sur 110
possible de modifier la tension moyenne aux bornes des moteurs et par conséquent la vitesse de rota-
tion des moteurs.
Figure 6. Schéma du système bouclé de l’asservissement en position
Enfin, s’il on rentre dès le début la position future, on risque d’avoir une erreur tellement grande que
l’asservissement génère une vitesse maximale au niveau des moteurs. On risque alors de faire patiner
les roues, de déraper et de perdre l’odométrie que nous analyserons plus tard. Nous devons donc créer
un trapèze de vitesse (cf. figure 7). Dans un premier temps, on augmente progressivement la position
future de manière à avoir une variation linéaire de la vitesse (accélération constante) puis dans un deu-
xième temps on incrémente la position pour une vitesse constante et enfin on diminue les incréments
jusqu’à atteindre la position finale pour décélérer (cf. figure 8). La durée entre chaque incrément dé-
pend de l’horloge de l’automate. Avec une horloge fixe sur le microcontrôleur, on peut faire varier la
vitesse (cf. equation1). Avec le coefficient proportionnel il faudra faire en sorte que pour un incrément
maximum donné, le résultat fasse 100% de rapport cyclique. En fixant l’accélération de manière à ne
pas patiner, on voit que l’on a besoin de trois données pour chacun des moteurs : la position finale, la
vitesse maximale et la position avant de décélérer. Chacune de ces données sont transmises par la carte
stratégie qui calcule la trajectoire. A noter : même si chacune des roues possèdent son asservissement,
il existe une relation à respecter entre les vitesses des roues et les distances à parcourir pour
faire un rayon de courbure. Cette relation sera examinée plus en détail dans la prochaine sous-partie.
De plus, pour éviter de s’arrêter inutilement (donc perdre du temps) et la discontinuité des vitesses
entre les rayons de courbures, on décélère jusqu’à une vitesse minimale. On s’arrête uniquement à
l’arrivé du robot dans sa zone ou en cas d’urgence.
Equation 1 : v = dposition/dt avec dt une période de l’horloge
Étudiant auteur de cette partie : Josué Ferreira
Page 10 sur 110
Figure 7. Trapèze de vitesse des roues Figure 8. Évolution de la position
1.2 Programmation de l’asservissement sur PSOC
L’implémentation du programme de l’asservissement a été réalisé sur une carte microcontrôleur PSOC
conçue par M. Guinand, professeur à l’IUT de Cachan (cf. figure 11). Le PSOC se programme en lan-
gage C. L’avantage de ce microcontrôleur est qu’il possède un tas de fonctions préprogrammées pour
créer des horloges, des timers, des générateurs de signaux PWM, acquisition CAN, de codeurs, etc.
Dans une fenêtre d’interface graphique, on peut ainsi générer des blocs de fonctions préprogrammées
et choisir les pins de sorties et d’entrées de chacun des blocs (cf. figure 9). On simplifie ainsi la pro-
grammation puisque les initialisations du microcontrôleur et la plupart des blocs possèdent toutes les
fonctions nécessaires. Par exemple, pour l’acquisition des codeurs, on dispose directement du nombre
d’impulsions avec la fonction QuadDec_getcounter() et on peut remettre à zéro avec
QuadDec_setcounter(0). Pour d’autres, les blocs génèrent des fichiers .c et .h avec des prototypes de
fonctions vides à remplir selon les besoins. Il s’agit typiquement des fonctions d’acquisition de bus
CAN et autres2 dans lesquelles on traite les données en réception ou en transmission.
La figure 9 présente tous les blocs fonctionnels utilisés pour notre asservissement soit : deux blocs
traitement des codeurs pour les codeurs droit et gauche, un bloc générateur PWM, un bloc acquisition
CAN et une interruption ISR_TIMER avec une horloge de 100kHz.
Figure 9. Blocs fonctionnels pour l’asservissement sous PSOC Creator
2 SPI, bus série RS232, I2C
Page 11 sur 110
Après l’initialisation de tous les blocs dans le main.c principal à l’aide des fonctions d’initialisation
des blocs, on peut appeler dans un while(1) les fonctions d’asservissement de chacune des roues que
nous avons crées. De plus, pour contrôler la vitesse, l’automate doit avoir une horloge adaptée. En
effet, si on exécute en continu le programme, on ne maitrise pas le temps entre chacune des incrémen-
tations de la position et on arrivera à la consigne finale avant même que le robot ait le temps de ré-
pondre. Si l’horloge est trop longue, le robot ne répondra pas assez vite en cas d’obstacles.
L’interruption ISR_TIMER permet donc de créer une horloge pour l’automate. L’interruption incré-
mente une variable timer toutes les 10µs. Nous avons choisi de prendre une horloge de 100Hz. Donc
on attend que timer=1000 pour exécuter les asservissements. A cette fréquence, la vitesse minimum
est de 15 mm/s et le robot corrige sa trajectoire toutes les 10ms.
Notre machine d’état repose sur le schéma suivant :
Figure 10. Machine d’état de l’asservissement
Dans l’état d’attente, on maintient le robot à la dernière consigne reçue par la carte stratégie ou aux
coordonnées d’origines. Cela ne veut pas dire que le robot ne fait rien puisque si quelque chose vient
pousser le robot, le robot doit revenir à l’ancienne consigne ou bloquer les roues. Le codage de
l’asservissement qui est commun aux trois états est le suivant :
cons_pos_Gauche += vitesse_Gauche; //Augmentation de la consigne de position
Codeur_Gauche = QuadDec_Gauche_GetCounter(); //Lecture de la valeur du codeur
erreur_Gauche = cons_pos_Gauche - Codeur_Gauche; //Calcul de l'erreur
somme_Gauche += erreur_Gauche; //Calcul de la somme pour la correction intégrale
alpha_Gauche = kp_Gauche*erreur_Gauche+ki_Gauche*somme_Gauche ; //rapport cyclique
if (alpha_Gauche<-1.0) //Si alpha est plus petit que -1,
alpha_Gauche=-1.0; //On le limite à -1
else if (alpha_Gauche>1.0) //Si alpha est plus grand que 1,
alpha_Gauche = 1.0; //On le limite à 1
if (alpha_Gauche>0) //Si alpha est positif,
{
PWM_1_WriteCompare1(alpha_Gauche*1199); //On l'applique à la PWM,
DIR_Gauche_Write(0); //Et on va vers l'avant
}
else //Si alpha est négatif,
{
PWM_1_WriteCompare1(-alpha_Gauche*1199); //On l'applique à la MLI,
DIR_Gauche_Write(1); //Et on va vers l'arrière
}
Attente
Décélération Vitesse
constante
Accélération
Nouvelle
consigne
Vmax Position
décélération
Position
finale
Page 12 sur 110
La fonction QuadDec_Gauche_GetCounter() permet de récupérer le nombre d’impulsions du codeur
gauche. PWM_1_WriteCompare1(alpha_Gauche*1199) permet de générer un signal PWM. Le signal
est à 100% lorsque la variable en entrée vaut 1199 (cette variable est modifiable dans le bloc fonction-
nel de la PWM). L’incrément vitesse_gauche dépend de l’état dans lequel l’asservissement se trouve.
Dans la phase d’accélération, l’incrément augmente toutes les 10ms. Dans la phase vitesse constante,
l’incrément reste égal à la vitesse max. Enfin, dans la phase de décélération, l’incrément diminue jus-
qu’à atteindre la vitesse minimum ou la consigne finale. Entre les rayons de courbures, on n’arrête pas
le robot se déplace à vitesse minimum quand il repart sur un nouveau rayon de courbure. Le code
complet de l’asservissement est disponible en annexe.
La carte PSOC (cf. figure 11) dispose de hacheurs 4 quadrants pour pouvoir alimenter les moteurs à
partir d’un signal PWM provenant du PSOC. On dispose d’un connecteur pour le bus CAN pour rece-
voir les données de consignes et vitesse maximum de la carte stratégie. Enfin, elle est équipée de deux
connecteurs spécifiques pour les codeurs. Le schéma de la connexion des codeurs et le PCB de la carte
sont disponibles en annexe.
Figure 11. Carte Moteurs avec PSOC.
Étudiant auteur de cette partie : Josué Ferreira
1.3 Calcul de la trajectoire avec des rayons de courbure
L’objectif qui nous a été fixé pour la compétition est de permettre au robot de se déplacer avec des
trajectoires en rayon de courbure. Ce type de trajectoire permettra au robot d’éviter les obstacles avec
plus d’aisance et plus rapidement qu’un comportement à trajectoires rectilignes. Pour se faire on se
place dans un repère cartésien avec en plus une indication de direction du robot (angle dans le sens
trigonométrique). L’odométrie implémentée sur la carte PSOC, qui sera détaillée plus tard, permet de
connaitre à chaque instant la position du robot en coordonnées cartésiennes et la direction du robot. Si
on définit un point en coordonnées cartésiennes où le robot doit se rendre en fonction des obstacles
qu’il voit3, il faut calculer pour chacune des roues les trois données nécessaires pour l’asservissement :
3 Cette fonction n’a pas encore été réalisée.
Page 13 sur 110
la distance à parcourir, la vitesse maximale et la distance avant décélération. Pour cela, on dispose
d’un modèle mathématique qui énonce que pour deux points distincts du plan, on peut trouver un
cercle passant par les deux points. Or il ne faut pas n’importe quel cercle. Le rayon du cercle doit être
perpendiculaire au vecteur unitaire de direction du robot (cf. figure 12) car le vecteur vitesse doit être
tangent à la trajectoire. On remarquera que l’on ne peut calculer des trajectoires uniquement pour des
points situés au dessus de la droite orthogonale au vecteur direction du robot ou des vecteurs non coli-
néaires. Avec les données précédentes fixées, on en déduit les équations suivantes (il s’agit ici d’un
cas particulier mais il est toujours possible de revenir à cette configuration par symétrie):
d = arctan (y1/x1)
d = y1 x sin (d) ou x1 x cos(d)
= 2 x (0 - d)
Rayon= d / (2 x sinus (/2))
Distance de l’arc de cercle = x rayon
Distance rayon extérieur = x (rayon + entraxe/2)
Distance rayon intérieur = x (rayon - entraxe/2)
Figure 12. Schéma géométrique du calcul du rayon de courbure
La programmation du calcul de trajectoire sera implémentée sur la carte stratégie X12. Le microcon-
trôleur de la carte est bien adapté pour le traitement des données. De plus, la carte stratégie reçoit
toutes les informations nécessaires (obstacles, balise laser et infrarouge, position) pour recalculer la
trajectoire. On remarque que l’on a besoin de calculer un sinus et une tangente. Or, les performances
du microcontrôleur sur la X12 diminuent drastiquement lorsque l’on utilise des variables flottantes et
la bibliothèque mathématiques. Nous avons donc opté pour un tableau de sinus et de tangente enregis-
tré dans la mémoire flash du microcontrôleur. L’index du tableau correspond à l’angle en degré et on
Page 14 sur 110
prend uniquement de 0° à 90° puisque les fonctions sont périodiques et symétriques. Vu que les fonc-
tions varient entre 0 et 1 pour sinus et que la tangente varie entre 0 et 1 pour un angle de 0° à 45°, on
multiplie le tous par 103 pour rentrer dans un unsigned short.
Dans un premier temps, on doit calculer les coordonnées polaires du vecteur déplacement qui relie la
position actuelle et la position future souhaitée. Pour ce faire, on calcule le quotient de la différence
des ordonnées et des abscisses du point d’arrivé par rapport à la position du robot multiplié par 103. En
fonction du quadrant dans lequel se place le point d’arrivé par rapport à la position du robot, on peut
calculer des symétries avec le premier quadrant car le tableau de tangente est compris entre 0° et 90°.
On balaie ensuite le tableau de tangente et on cherche la valeur la plus proche, l’index correspond
alors à l’angle. Donc on peut calculer l’angle du vecteur dans le sens trigonométrique de 0° à 359°. De
la même manière, on utilise des symétries avec les quadrants pour calculer la norme du vecteur avec le
tableau de sinus (ex : sin (250)= sin(250-180) = sin(70), sin(170)= sin(180-170)= sin(10)). Pour éviter
de tendre vers l’infini : quand l’angle est inférieur à 45°, on calcule avec le cosinus soit
d=x.cos(d)=x.sin(90-d) et quand l’angle est supérieur à 45°, on calcule avec le sinus d=y.sin(d).
Le code étant trop long, il est disponible en annexe.
Ensuite, pour calculer les distances de chaque roue et les vitesses maximales, on utilise les expressions
vues plus haut. Comme on donne des coordonnées de position en avant de la direction du robot (1er et
4e quadrant), l’angle de l’arc de cercle est compris entre 0° et 90° donc pas de symétrie à calculer. Les
calculs étant simples, la figure 13 illustre les différentes symétries pour revenir au cas initial. Dans le
cas où les deux vecteurs sont colinéaires, on fait une ligne droite. Le code est disponible en annexe.
Figure 13. Calcul de l’angle de l’arc de cercle en fonction des vecteurs
= 2 x (360 – (d - 0))
= 2 x (360 – (0 - d))
Page 15 sur 110
Pour conclure cette partie, nous avons à disposition le programme d’asservissement des moteurs sur la
carte PSOC ainsi que le calcul de trajectoire permettant de calculer les trois données nécessaires sur la
carte X12. Les deux cartes communiquent en bus CAN. La carte PSOC communique les coordonnées
(cf. odométrie) et son arrivé à la consigne et la carte X12 communique les données nécessaires au
rayon de courbure. Pour les épreuves d’homologation des trajectoires sont prédéfinis sur le robot. A
noter : la fonction d’évitement d’obstacles n’a pas encore été réalisée. Il faudra créer une fonction qui
calcule un parcours de coordonnées afin d’éviter un obstacle sur un parcours prédéfini en fonction des
obstacles voisins. Ce qui nous amène à la détection d’obstacles.
Étudiant auteur de cette partie : Josué Ferreira
2. Carte capteur : Détection d’obstacle
2 .1 Introduction
Comme pour les Hommes et les animaux, les robots ont besoin de capteur pour se mouvoir dans un
environnement totalement aléatoire. Capteur infrarouge, ultrason, thermique, sensorielle … une boite
d’équipement multiple pour les robots, mais il ne suffit de brancher le capteur, il faut encore traiter
l’information qu’il renvoie. Et après l’avoir rendu plus apte et compréhensible nous transmettons
l’information au cerveau qui va réagir ont fonction du contenu de l’information.
Pour la coupe de robotique de Vierzon nous avons mis en place une carte capteur capable de fournir en
temps réelle différentes distances d’obstacle et de renvoyer d’autre information par bus CAN à la stra-
tégie.
Étudiant auteur de cette partie : Alexandre Chhean
2 .2 Structure de la carte capteur
La carte capteur peut être divisé en cinq grandes parties, la partie microcontrôleur, la partie CAN, la
partie capteur, la partie laser et la partie affichage. La partie microcontrôleur est la partie du traitement
de l’information avec le microcontrôleur µB12. La partie CAN est la partie de communication avec les
autres cartes par le bus CAN. On y retrouvera le transceiver CAN et le connecteur micro match (con-
necteur robot, le connecteur rouge). La partie capteur est la partie réception de l’information des diffé-
rents capteurs courts, longs, zone, contact et connecteur jack. La partie laser est la partie réception de
l’information du laser et de son alimentation +12V. La partie affichage est la partie qui nous permet de
voir si un seuil a été atteint par les capteurs sur des LEDs.
Page 16 sur 110
Étudiant auteur de cette partie : Alexandre Chhean
2 .3 Principe de fonctionnement de la carte capteur
La carte capteur a différente fonction la première étant d’envoyer la distance en cm d’un obstacle qui a
été détecté par le robot. Mais aussi de prendre en compte les capteurs de contacts à l’arrière du robot,
de savoir quand la course commence en retirant le jack du robot, de savoir s’il aperçoit quelque chose
avec ses lasers, des affichages LEDs pour vérifier l’état des capteurs de distances et de zone et enfin de
détecter l’arrivé du robot sur la zone blanche avec les capteurs de zone.
Étudiant auteur de cette partie : Alexandre Chhean
2 .4 Choix des capteurs
Le choix des différents capteurs est important car une fois sélectionné et installé sur le robot, il est
difficile de changer en plein milieu du projet car vos fonctions qui caractérisent votre capteur de dis-
tance pour la conversion des valeurs numériques en cm et totalement différentes d’un capteur à un
autre. De plus pour l’installation mécanique nous nous retrouvons dans le même problème de diver-
gence dans les trous et les connectiques. Il est donc important de faire une étude pour le choix de nos
capteurs.
Page 17 sur 110
Source : Extrait datasheet du GP2D12
Figure 17 : Caractéristique du GP2D12
Figure 16 : Schéma de fonctionnement GP2D12
2 .4 .1 Capteur de distance
Dans notre étude de choix des capteurs de distances nous avons opté pour deux capteurs. Un capteur
permettant la réception de courte distance et un capteur pour les longues distances.
Capteur de distance court GP2D12 :
C’est un capteur infrarouge qui émet un rayon infrarouge qui celui va être réfléchi par l’objet et ren-
voyer sur une photoréceptrice. L’angle entre le rayon démission et de réception permet de déterminer
la distance du l’objet.
Comme nous pouvons le voir sur la caractéristique du GP2D12, nous pouvons distinguer des distances
allant de 10 cm à 40 cm. Au-delà de 40cm il est difficile de faire la différence de distance.
Le seul problème est la non linéarité de la caractéristique et il faudra donc faire une fonction qui la
linéarise cette courbe par petit morceau.
Enfin le capteur peut être alimenté de +4.5V è 5.5V et nécessite de mettre une capacité de liaison entre
le VCC et le GND.
Capteur de distance long GP2YA02
Le capteur infrarouge GP2YA02 se présente comme le GP2D12 sauf il a une caractéristique diffé-
rente. Nous pouvons distinguer des valeurs de distances de ~15cm à 1m20. Le seul défaut est qu’il est
aveugle entre 0cm à 15 cm. Il est donc important de prévoir une solution.
2 .4 .2 Capteur de zone
Pour la détection de la zone blanche, nous avons préféré l’utilisation simple de CNY70. Facile à
mettre en place, on peut l’alimenter en +5V. Le principe de fonctionnement du CNY70 est d’envoyer
un rayon infrarouge sur la surface qui va le renvoyer sur le capteur et en fonction de la couleur de la
surface, la valeur analogique sera différente. On pourra régler la sensibilité du capteur en modifiant les
résistances.
Source : Mon-club-elec.fr
Page 18 sur 110
Figure 18 : Schéma de câblage du CNY70
Étudiant auteur de cette partie : Alexandre Chhean
2 .5 Disposition des capteurs
La disposition des capteurs sur la plateforme du robot et aussi très important car il faut que l’on est un
champ de vision optimal pour un nombre de capteur réduit. Le but n’est pas de faire un robot avec des
capteurs placers tout autour de lui, mais de mettre les capteurs à des dispositions intelligente et straté-
gique.
Figure 19 : Disposition des capteurs
Nous avons disposé les capteurs de distances courtes et longues de façon à avoir deux zones. La zone
danger qui est constitué de capteur court permet d’avertir le robot qu’un obstacle ou un robot ennemi
est proche et qu’il faut réagir très vite. La zone obstacle est une zone qui 50cm du centre du robot peut
distinguer un obstacle de largeur 40cm, ce qui est la taille d’un obstacle. Et on aura donc un angle de
40° entre les deux capteurs longs. Les capteurs de zone sont placés de façon à voir la zone blanche la
plus vite possible, c’est donc pour cela qu’ils ont été disposé le plus près de l’avant du robot. Enfin les
capteurs de contact sont placés à l’arrière du robot pour le recalage du robot lors du départ.
Page 19 sur 110
Étudiant auteur de cette partie : Alexandre Chhean
2 .7 Programme de détection d’obstacle
Notre programme peut être divisé en deux parties, les comparaisons des valeurs reçus et la partie con-
version des valeurs numérique ne cm.
2 .7.1 Fonction conversion numérique en cm.
Avant d’écrire notre fonction nous avons caractérisé le capteur court et long pour avoir la caractéris-
tique réelle pour après la linéariser par petit morceaux.
Figure 20 Fonction de conversion numérique des capteurs
2 .7 .2 Fonction de comparaison
Cette fonction est le corps du programme de la carte capteur. Elle est centré autour un switch case qui
en fonction de l’état des capteurs on peut distinguer si nous sommes dans une zone de danger ou bien
dans la réception de distance longue. La première étape est de savoir dans quel état nous sommes et
donc de faire un état wait.
Page 20 sur 110
Figure 21 : Etat Wait fonction comparaison
Les conditions && detection_court_ […] nous permettent de le pas saturer en renvoyant l’information
sur le bus CAN s’il y a détection dans la zone danger. Pour la zone danger, si la condition est vraie
nous envoyons la distance du capteur court en question.
Figure 22 : Etat Zone danger
La partie détection longue va envoyer la distance des deux capteurs longs si un objet atteinte le seuil
de distance. De plus si nous sommes en dessous du seuil la fonction renverra si la distance des cap-
teurs longs à varier de plus ou moins 10cm. C’est dans le but de ne pas surcharger le bus CAN.
Page 21 sur 110
Figure 23 : Etat détection long
Pour ce qui est des CNY70, des détecteurs de contact, du laser. Nous avons la même structure de
comparaison avec un if et si c’est vraie l’envoie de d’information par le bus CAN en utilisant la fonc-
tion CAN_send_message.
Étudiant auteur de cette partie : Alexandre Chhean
2 .8 Conclusion
La carte capteur fonctionnement et l’envoie des données sur le bus CAN aussi. Ce qu’il faut améliorer
c’est encore la réception des valeurs du convertisseur analogique numérique en interruption et non pas
scrutation ce qui optimiserait fortement la précision des calculs de distance.
Étudiant auteur de cette partie : Alexandre Chhean
3. Repérage sur le terrain
3.1 Odométrie
Le principe de l’odométrie est de découper en petite portion le chemin parcouru par le robot. En dispo-
sant des informations de distances par les codeurs et en prenant des petites variations à petits inter-
valles de temps (ordre 10ms), un modèle mathématique permet de calculer les petites variations sur x,
y et l’angle teta du robot. Voici les expressions mathématiques :
= (dist_d – dist_g) / entraxe Rayon= (entraxe/2) x (dist_d + dist_g)/ (dist_d – dist_g)
x = cos() x rayon
y = sin() x rayon
Le programme est implémenté sur la carte PSOC qui dispose de l’état des codeurs et donc de la dis-
tance parcourue par les deux roues. Elle communique par CAN les positions du robot.
Étudiant auteur de cette partie : Josué Ferreira
Page 22 sur 110
3.2 Balise Infrarouge émission
Le règlement de la coupe de Vierzon autorise une équipe à placer une balise laser ou infrarouge. Pour
cela on va développer une balise pouvant aider notre robot à atteindre la zone d’arrivée. Comme il y a
quatre équipes sur le terrain, il est fort probable que d’autres balises soient placées dans les coins de la
piste. Pour cette raison, notre balise devra envoyer un code pour permettre à la partie réception placée
sur le robot de reconnaitre notre balise. De cette manière si le robot ne sait plus vers quelle direction
aller, il peut utiliser la balise pour se réorienter vers la bonne direction. Pour cela il faut concevoir une
carte d’émission et une carte réception.
Dans un premier temps on fixe les paramètres d’émission. Il y a quatre paramètres, le premier donne la
vitesse d’émission en bit/seconde, le deuxième le type de codage, le troisième le code à envoyer et le
dernier la durée de la phase de non-émission.
Le code doit tenir sur cinq bits, donc la valeur maximale est 31 ou 0x1F.
La vitesse d’envoi choisie est de 1200 baud. On est limité, d’une part, par le récepteur qui exige un
minimum de 10 périodes de porteuse avant de changer d’état ce qui limite à 3600 baud et, d’autre part,
par les capacités d’exécution du microprocesseur (surtout pour la partie réception). Après plusieurs
essais il s’est avéré que 2400 baud était encore trop rapide et que le récepteur ne voyait pas certains
bits. Par conséquent, et de manière à reprendre les vitesses de transmission standards d’une liaison
série, on s’est arrêté sur 1200 baud.
La constante CODAGE peut prendre les valeurs 0, 1 et 2 ce qui respectivement correspond à NRZ,
Manchester et Miller. Le Non Return to Zero est le plus simple car les bits sont envoyés tel quel. Le
Manchester code de la manière suivante : 0 pour un front montant et 1 pour un front descendant. Le
codage Miller quant à lui procède comme suit : 1 = changement de niveau logique au demi temps bit
et 0 = pas de changement. De plus s’il y a deux 0 de suite on change de niveau logique au début du bit.
Page 23 sur 110
Après plusieurs tests de distance avec les différents codes, on est arrivé à aux conclusions suivantes :
Nom Complexité Portée
NRZ Simple Courte Manchester Compliqué Longue Miller Très compliqué Longue
Étudiant auteur de cette partie : Florian Biazi
3.2.1 Table d’émission
C’est donc le choix du codage Manchester qui a été retenu. Après initialisation du microprocesseur, on
créé un tableau transformant le code à envoyer en trame codée. La trame est composée de quatre par-
ties :
- Deux bits de Start à 1
- Code sur 5 bits
- Un bit de stop à 1
- Pas d’émission IR (envoi d’un 0)
Pour différencier les bits codés des autres bits non codés, ces derniers sont remplacés par 10 et 11 au
lieu de 0 et 1. Exemple avec le code 0x14.
Trame[] de l’indice 0 à l’indice n
11 11 1 0 1 0 0 11 10 10 10 …
Start Code Stop Pas d’émission
A 1200 baud les 8 bits (Start, code et stop) sont envoyés en 6,7 ms du poids fort au poids faible, s’en
suit une phase de non-émission de durée fixée à 13ms. Cette durée dépend de la constante
TAILLE_TRAME. La valeur minimale est 8 si on souhaite envoyer la trame sans pause, ce qui est
déconseillé.
Figure 24 Exemple de trame avant modulation
Page 24 sur 110
Étudiant auteur de cette partie : Florian Biazi
3.2.2 Modulation
A deux fois la vitesse de transmission, une interruption timer appelle la fonction codage(). Cette fonc-
tion traduit la trame en bits codés selon la constante CODAGE. Si l’interruption se fait à deux fois la
vitesse de transmission c’est par ce qu’il est impossible d’envoyer directement un front montant ou
descendant. L’astuce est d’envoyer un 0 suivit d’un 1 pour le front montant et inversement pour le
descendant. La fonction fait aussi la différence entre bit codé et non codé. C’est pour ça que l’on a
remplacé précédemment les bits de Start par 11. De cette manière le programme sait qu’il s’agit d’un
vrai 1 et non d’un front descendant pour le cas du codage Manchester.
La génération de la porteuse quant à elle est assez basique. A chaque interruption la variable porteuse
s’inverse. La fréquence d’interruption doit donc être deux fois supérieure à la porteuse soit 72 kHz.
Dans cette interruption on fait aussi la fonction de modulation. Grâce à une simple structure en if else
on obtient la modulation suivante :
BIT SORTIE
0 porteuse 1 0
De cette manière on anticipe l’inversion de la donnée causée par le récepteur que l’on verra dans le
chapitre sur la carte de réception. De plus dans le cas où la sortie doit être active on donne la valeur 7
au port A. Dans la partie réalisation de la carte on verra que trois transistors sont en sortie des trois
premiers bits du port. Comme 7 vaut 111 en binaire on contrôle donc les trois transistors en même
temps. Ci-dessous le code de l’interruption timer et les relevés d’oscilloscope.
Figure 25. Relevés de la modulation et du code
Page 25 sur 110
Étudiant auteur de cette partie : Florian Biazi
3.2.3 Conception de la carte émission
L’objectif de la carte d’émission est d’envoyer un code sur l’ensemble de la piste. Celle-ci faisant 8
mètres de côté il faut donc être capable d’émettre au minimum jusqu’à environ 11,30 mètres. Pour cela
on dispose en arc de cercle six diodes infrarouges TSAL 6200 de chez Vishay. Elles émettent à une
longueur d’onde de 940nm et ont un demi-angle de 17°. Par conséquent 3 diodes auraient pu suffire
mais en cas de panne, mieux vaut en avoir en plus pour assurer le bon fonctionnement de la balise. Les
diodes sont contrôlées par trois transistors MOSFET eux même reliés aux trois premiers bits du port A
du microcontrôleur Freescale. La carte µB12 a été la solution retenue car elle est bien connue depuis le
semestre 1 et les bibliothèques déjà fournies nous facilitent le travail.
Pour l’utilisation des diodes, la documentation recommande un courant de 100mA passant dans la
diode. De plus la chute de tension à ses bornes est de 1.3V. En choisissant de placer deux diodes en
série avant chaque transistor on obtient le calcul de résistance suivant :
Comme
cette résistance n’est pas présente dans la série E24 on peut choisir soit 22 ou 24 Ω
Pour vérifier le bon fonctionnement de la carte une LED verte a été placée mais celle-ci n’est pas in-
dispensable puisqu’il y en a une aussi sur la carte microcontrôleur. Le PCB est disponible en annexe.
Détails importants :
- les diodes sont inclinées à l’horizontal et orientés vers la piste,
- il faut toujours veiller à ce que le transformateur soit réglé sur 5V, comme il n’y a ni 7805 ni
traco sur la carte, une alimentation 12V pourrait endommager irréversiblement la carte4.
Pour les années suivantes nous avons pensé à quelques améliorations que nous n’avons pas eu le
temps d’ajouter ou de modifier.
Au début du projet l’idée était d’envoyer trois codes différents sur chaque groupe de deux diodes pour
pouvoir situer le robot dans un secteur de la piste. Cependant on s’est aperçu que les codes se superpo-
saient et l’idée a été abandonnée. Par conséquent il serait possible d’agir sur les trois transistors avec
un seul port du microcontrôleur puisque le code envoyé est le même sur toutes les diodes.
La deuxième amélioration possible serait de pouvoir choisir dynamiquement le code à transmettre
plutôt que de changer une constante et de recompiler le programme. L’idée est de relier un potentio-
mètre au convertisseur analogique/numérique. Une interface homme/machine serait à ajouter pour
savoir quel code a été choisi (écran ou LEDs).
Étudiant auteur de cette partie : Florian Biazi
4 Le 7805 et le traco sont des régulateurs de tension qui convertissent une tension plus élevée en 5V stable et
continu
Page 26 sur 110
3.2.4 Carte de réception
Figure 26. Programme de la carte de réception
Pour cette partie il y a désormais neuf paramètres définis par des constantes. Ces paramètres sont : le
débit binaire, l’ID de réception, l’ID d’envoi de donnée, le code pour demander l’angle sur la boussole
mais il ne sera pas utilisé, la taille en octet du message CAN à envoyer, la taille en bit du code à rece-
voir, la durée sur laquelle on écoute un capteur, la durée après laquelle on efface la liste des capteurs et
enfin le code à recevoir. Ces paramètres seront expliqués plus loin.
Étudiant auteur de cette partie : Florian Biazi
3.2.4 Conversion du code
De la même manière que pour la carte d’émission, la première chose que le programme fait est de
créer un tableau des valeurs codées que l’on doit recevoir. Vu que le codage Manchester a été choisi
pour la partie émission c’est ce codage qui est utilisé par défaut. La fonction remplissage_tableau()
rempli le tableau data[] avec le CODE_SECRET transformé en Manchester. Comme il s’agit encore
une fois de stocker des fronts, data[] a une taille de 2*TAILLE_TAB. Exemple de tableau de réception
créé quand CODE_SECRET vaut 0x14.
data[] 0 1 0 1 1 0 0 1 0 1
En binaire 1 0 1 0 0
En hexadécimal 0x14
Page 27 sur 110
Étudiant auteur de cette partie : Florian Biazi
3.2.5 Réception par machine à état
Avant tout on sélectionne un capteur à écouter. La carte possède en tout trois capteurs identifiés de la
manière suivante : 0, 1 et 2 correspondent dans l’ordre au capteur gauche, central puis droit. La pre-
mière solution était d’écouter tous les capteurs en même temps mais les performances plutôt moyennes
du microcontrôleur Freescale ne permet pas de tout traiter en même temps. C’est pour ça que l’on crée
une sorte de multiplexeur 3 vers 1.
Figure 27. Trame reçue par un récepteur
Dans le premier état, on compare le bit lu sur le capteur choisi et la valeur contenue dans data[0]. Si
c’est le cas, cela veut dire que le bit lu correspond à ce que l’on attend et donc qu’on tient potentielle-
ment le début du code émis pas notre balise. Dans ce cas à la prochaine interruption on comparera le
bit en entrée avec la valeur data[1] en ainsi de suite. Si l’information lue ne correspond pas à ce que
l’on attend, et ce quel que soit l’état actuel, on revient dans l’état « j’attends que l’entrée corresponde à
data[0] ».
A ce moment-là on regarde s’il n’est pas temps d’écouter le capteur suivant. En effet le programme en
fait de telle manière que l’on ne peut pas mettre de côté un capteur qui reçoit le début de notre code.
Pour se faire on n’autorise le changement de capteur que si on se trouve dans l’état 0 (état de départ).
Si on arrive au dernier état c’est que l’on a reçu le code que l’on attendait et qu’il s’agit bien de notre
balise. Dans ce cas on enregistre le capteur écouté temporairement. La durée pendant laquelle le cap-
teur est conservé dépend de la constante DUREE_DE_VIE.
Étudiant auteur de cette partie : Florian Biazi
3.2.6 Fonctions sous interruption
Enregistrement des capteurs et transmission par bus CAN
A chaque fois qu’un capteur déclare avoir reçu le code provenant de notre balise, celui-ci est enregistré
dans un tableau. Après une certaine durée fixée par DUREE_DE_VIE, on affiche sur trois LEDs les
capteurs enregistrés. Ceci permet de mettre en place une interface simple et intuitive avec l’utilisateur
Page 28 sur 110
et également voir rapidement sans sonde ni oscilloscope l’état des capteurs. Aussi, on place dans la
variable hold un nombre correspondant aux capteurs gardés en mémoire.
Aucun capteur hold = 0
Capteur gauche hold = 1
Capteur central hold = 2
Capteur droit hold = 4
Les valeurs peuvent s’additionner ainsi si on capte en face et à gauche hold prendra la valeur 3. Après
ça on peut effacer le tableau contenant les capteurs gardés en mémoire. De cette manière on garantit la
fraicheur des données. Ci-dessous une version simplifiée du code exécuté.
Si (time_out < DUREE_DE_VIE)
time_out = time_out +1 ;
sinon
{
time_out = 0 ;
donner_une_valeur_a_hold() ;
afficher_sur_les_LED() ;
effacer_la_liste_des_capteurs() ;
}
Cependant l’affichage sur les LEDs n’est là que pour faciliter l’utilisation de la carte de réception.
L’objectif principal est d’envoyer la valeur de hold à la partie stratégique du robot s’il le demande.
Ainsi en cas de réception CAN, on regarde si l’ID correspond à ID_RECEPT qui a été fixé par
l’ensemble de l’équipe à 0x80. Si c’est le cas cela signifie que la carte stratégie a besoin de savoir vers
quel côté s’orienter. Dans le cas contraire il s’agit d’un message qui ne nous intéresse pas et celui-ci
est donc ignoré.
L’envoi de 1 octet de donnée se fait sur l’identifiant ID_ENVOI (0x81, fixé également). C’est là
l’intérêt d’avoir en mémoire un code correspondant aux capteurs recevant notre code. Au moment
précis où la demande est faite, la carte renvoie par bus CAN le contenu de hold. Dans le cas contraire
il aurait fallu observer les trois capteurs ce qui prend des dizaines de millisecondes et donc trop long
pour prendre une décision stratégique. Avec la première méthode, la réponse se fait immédiatement,
permettant une prise de décision rapide.
Étudiant auteur de cette partie : Florian Biazi
3.2.7 Boussole
Bien que non utilisé le programme de la carte de réception peut recevoir les données d’une boussole et
les transmettre par bus CAN. Le CMPS09 peut être utilisé en mode liaison série en mettant la patte
nommée « Mode » à la masse. En envoyant 0x12 (ou la constante ASK_ANGLE) sur la liaison série le
compas nous renvoie une valeur d’angle sur un octet non signé. Pour plus de précision on peut en-
voyer 0x13 pour recevoir une valeur sur 2 octets entre 0 et 3600 (donc 1/10 de degré par unité). Cette
information peut être ensuite lue par interruption sur la liaison série avec le code ci-dessous.
interrupt void it_serie(void)
{
SCI0SR1 = SCI0SR1;
val_compas = SCI0DRL;
}
Page 29 sur 110
Étudiant auteur de cette partie : Florian Biazi
3.2.8 Conception de la carte réception
Comme expliqué dans la partie précédente notre carte de réception a besoin de trois récepteurs, trois
LEDs et d’une partie gérant la transmission CAN. L’alimentation se fait intégralement par le connec-
teur robot rouge, relié au réseau CAN. Le courant utilisé par la carte est compris entre 40mA et 80mA
si toutes les LEDs sont allumées. On est loin du courant maximum pouvant passer par la nappe de fils
qui est de 1 ampère même si, bien sûr, la carte de réception est loin d’être la seule à s’alimenter de
cette façon. Le PCB est disponible en annexe.
Le récepteur choisi est le TSOP 31236, il est parfaitement adapté car il fait pour recevoir un signal sur
une porteuse de 36kHz et à une longueur d’onde de 940nm qui est exactement celle émise par les
diodes. La documentation recommande de placer un filtre avant le capteur mais après plusieurs essais
il fonctionne correctement sans ce filtre. Les TSOP sont placés à 90° pour correctement séparer les
zones de détection. Cependant un cache non réfléchissant sera à ajouter pour être sûr de bien délimiter
les zones de détection.
Comme la carte se trouve sur le dessus du robot, la partie communication CAN se situe en dessous de
la carte. De cette façon on économise de la place car la carte doit être plus longue que large pour pou-
voir être placé sur l’emplacement supérieur du robot. La partie communication CAN contient, un con-
necteur robot, une résistance de 120 Ω avec un cavalier si la carte est en terminaison et un transciever
CAN MCP2551.
De la même minière que pour la carte d’émission il serait intéressant de pouvoir choisir en temps réel
le code à recevoir.
Egalement cette année, pour la partie microcontrôleur, deux projets ont pour objectif d’implanter un
autre µC sur la carte µB12 tout en conservant le même placement des ports, des entrées analogiques
etc. Il pourrait être intéressant pour la prochaine coupe de remplacer le microcontrôleur Freescale par
un autre plus performant qui serait capable de surveiller tous les capteurs en même temps. On pourrait
par la même occasion placer plus de capteurs pour couvrir aussi l’arrière du robot et ce sans perdre du
temps à scruter chaque capteur indépendamment.
Étudiant auteur de cette partie : Florian Biazi
Page 30 sur 110
3.3 Gestion des AX12 pour le laser et éclate-ballon.
Lors d'un match, nous avons la possibilité de placer certaines balises dans les coins opposés à notre
départ, balises vers lesquelles nous devons nous rendre. Nous disposons déjà d'une balise infrarouge
mais pour plus de sécurité nous utiliserons aussi un repérage laser. Pour terminer un match, notre robot
doit s'arrêter dans la zone blanche du coin opposé à son départ ( au moins une partie du robot ). Dans
le règlement, chaque robot doit être porteur d'un ballon de baudruche qui doit être éclaté par le robot
pour valider l'arrivée dans la zone blanche. Ces deux objectifs seront réalisés à l'aide d'AX-12 qui sont
des servomoteurs de chez dynamixel. Leur usage sera détaillé plus tard dans cette partie du rapport.
Étudiant auteur de cette partie : Fabien Lesueur
3.3.1 Principe de fonctionnement du LASER
Le repérage laser se base sur un système optique. En effet, le robot dispose d'un laser et dans le coin
opposé est placée une balise de catadioptre. Le principe de fonctionnement est simple : le laser placé
sur le haut du robot envoie son faisceau lumineux unidirectionnel vers l'avant. Il est donc placé de
manière à envoyer son faisceau perpendiculairement à l'axe des roues. Ainsi, si le faisceau touche la
balise, le système catadioptrique va le renvoyer avec le même angle que l'angle d'incidence. De cette
manière, le signal lumineux va revenir heurter le récepteur du laser.
Figure 28. Schéma illustrant le principe du repérage
Notre balise catadioptrique est composée d'un ensemble de catadioptres. Un système catoptrique est
formé de la façon suivante :
Figure 29. Schéma illustrant le principe d’un système catoptrique
récepteur
émetteur
Page 31 sur 110
Trois miroirs sont placés perpendiculairement les uns par rapport aux autres dans différents plans de
l'espace, formant ainsi ce que l'on appelle un trièdre. Lorsqu'un faisceau lumineux arrive sur un des
miroirs avec un certain angle d'incidence, il est réfléchi sur le second mirroir puis sur le troisième. Par
compensation des angles, le faisceau sortant du système, après la réflexion sur le troisième miroir, a un
angle de sortie similaire à celui de l'angle incident. Néanmoins, un léger décalage est présent au niveau
de la position. Si l'on place une lentille avant le trièdre de miroirs, on réduit cet écart et on forme ainsi
un système catadioptrique.
Figure 30. Catadioptre classique
Étudiant auteur de cette partie : Fabien Lesueur
3.3.2 Traitement de l'information
Dans notre robot nous disposons de plusieurs cartes qui dialoguent par bus CAN. Il y a donc un mi-
crocontrôleur par carte. La carte capteur dispose d'un uB12 qui est un microcontrôleur que nous avons
eu l'habitude d'utiliser lors de notre formation à l'IUT de Cachan. Ce micro dispose d'entrées numé-
riques et analogiques qui disposent elles-mêmes de résistances internes de tirage. Nous avons la possi-
bilité d'activer ou non ces résistances de tirage.
Le laser s'alimente en 12V et la récupération de l'information nécessite une résistance de tirage. Nous
avons donc choisi d'envoyer les trois pins de connexion du laser (alimentation, masse, et data) direc-
tement sur la carte capteur. Ainsi, nous pouvons connecter la sortie du laser sur une entrée du µB12 en
activant la résistance de tirage de cette entrée, elle-même reliée à l'alimentation du micro qui est de
5V. Nous obtenons donc, sans aucun montage, un signal TTL traité directement par la carte capteur.
Figure 31. Sortie du laser
Page 32 sur 110
Figure 32. Utilisation des AX12
Le ballon à éclater pour valider la fin de la course sera situé sur le haut du robot. Nous allons le faire
éclater grâce à un petit moteur sur lequel nous avons fixé un disque magnétique venant d'une dis-
quette. Nous devons donc contrôler la position de ce disque, puis activer le moteur lors de l'arrêt du
robot dans la zone d'arrivée. De plus nous devons faire osciller le laser pour un meilleur repérage de la
balise. Pour ce faire, nous avons à notre disposition des AX-12 de dynamixel. Ce sont des servomo-
teurs asservis en position qui communiquent en liaison série halfduplex. On cable les AX-12 les uns
après les autres, en cablant le premier à la carte microcontrôleur.
Figure 33. Communication entre les AX12
Page 33 sur 110
Nous utilisons donc deux servomoteurs AX-12. Pour les diriger il est nécessaire d'utiliser un micro-
contrôleur. Nous choisissons d'utiliser un mbed LPC1768 que nous implanterons sur une carte dont le
routage sera fait par un collègue de robotique. En effet, son projet est entrecroisé avec le nôtre. Nous
choisissons l'utilisation d'un mbed car il s'agit avant tout d'une communauté, tout le monde peut ainsi
poster des bibliothèques ou différents types de codes. L'avantage étant qu'une bibliothèque pour les
AX-12 nous est déjà fourni sur mbed. De plus, le mbed dispose d'une bibliothèque principale qui per-
met l'utilisation simple et directe de ces nombreux pins de connexion.
Figure 34. Architecture d’un Mbed
Les liaisons série seront utilisées pour les ports des AX-12. Le CAN, nécessaire pour la communica-
tion de notre robot, est aussi présent. La carte qui accueillera le mbed dispose donc:
• de 3 connecteurs AX-12
• d'une alimentation 12V
• d'un connecteur micro-match ( CAN + alim 5V )
Le PCB de la carte est en annexe. Les AX-12 disposent d'une plage d'alimentation allant de 7V à 10V.
Néanmoins, ils fonctionnent très bien en 12V. Etant donné que nous disposons sur le robot d'une batte-
rie de 12V, nous simplifions la carte, en envoyant directement l'alimentation 12V aux pins d'alimenta-
tion des AX-12. Ceux-ci possèdent seulement 3 pins sur leurs connecteurs qui sont les suivantes:
Figure 35. Schéma présentant la connectique des AX12
Page 34 sur 110
La plus grosse partie de ce projet consiste en réalité à corriger et à compléter la bibliothèque AX-12
fournie sur la communauté mbed. En effet, les servomoteurs disposent d'un microcontrôleur dont les
registres de mémoire sont les suivants:
Figure 36. Registres mémoires du Mbed
Certains registres ne sont accessibles qu'en lecture, d'autres peuvent être lus et écrits. Chaque adresse
correspond à une donnée de la taille d'un octet. La communication des AX-12 fonctionne par l'envoi
de trames sur une seule ligne de transfert. Etant donné que les AX-12 peuvent se cascader, le principe
d'identification est appliqué de la manière la plus simple, chaque AX-12 dispose d'un ID. Cet ID peut
être redéfini dans le registre lui correspondant à l'adresse 0x03. Un ID de broadcast est disponible afin
de communiquer avec l'ensemble des AX-12 connectés sur une même ligne. Il s'agit de l'ID 0xFE.
Page 35 sur 110
Lors de l'envoi d'une trame par la carte microcontrôleur, l'AX-12 destinataire renvoie une trame de
retour vers cette même carte microcontrôleur. Les trames se construisent de la manière suivante :
Figure 37. Exemple d’une trame de communication
Les deux premiers octets sont des octets de Header qui sont toujours de 0xFF. Le suivant correspond
à l'ID, l'octet Length correspond à la "taille" de la trame:
Length = Nombre de paramètres + 2
Puis arrive l'octet d'instruction:
Figure 38. Tableau récapitulatif des instructions
Page 36 sur 110
Nous utiliserons principalement les fonctions READ_DATA et WRITE_DATA. Ensuite viennent les
paramètres. Comme indiqué sur le tableau ci-dessus, lorsque que l'on choisit d'écrire dans un registre,
le nombre de paramètres est de deux ou plus:
paramètre 1 adresse du registre où écrire
paramètre 2 premier octet de données à être écrit
paramètre 3 deuxième octet de données à être écrit
paramètre N Nième octet de données à être écrit
Lorsque l'on choisit de lire dans un registre, les paramètres sont au nombre fixe de deux:
paramètre 1 adresse du registre où écrire
paramètre 2 nombre d'octets à lire
Le dernier octet de la trame d'envoi appelé Checksum correspond à une validation de la trame totale:
Checksum = NOT ( ID + Length + Instruction + Parameter 1 + ... + Parameter N )
La trame de retour reprend les octets précedents à l'exception d'un octet spécifique au retour qui est
l'octet Error.
Figure 39. Explication de l’octet d’erreur
Page 37 sur 110
Cet octet peut nous permettre de déboguer facilement les AX-12 si des erreurs les concernant se pro-
duisent. La trame de retour à une écriture ne possède pas de paramètres. Par contre la trame de retour à
une lecture possède un ou plusieurs paramètres qui correspondent tous à la valeur, ou aux valeurs, des
données lues dans les registres. La bibliothèque fournie par la communauté mbed dispose d'une fonc-
tion d'écriture et d'une fonction de lecture concernant les AX-12. Ces fonctions devraient nous per-
mettre d'écrire ou de lire un registre mémoire. Malheureusement, lors des premiers essais de lecture de
registre avec une fonction fournie, la valeur retournée n'était pas du tout cohérent et fixe. La fonction
de lecture n'était donc pas valide. La partie concernant la création de la trame d'envoi dans le cas des
fonctions READ et WRITE, était correctement codée. Ce sont les parties de communication qui po-
saient un problème. Dans le cas de la fonction READ, une fois la trame envoyée, la bonne réception de
la trame de retour est essentielle car c'est cette trame qui va contenir les valeurs que l'on souhaite récu-
pérer. La réception d'origine remplissait un tableau de char dans une structure for à l'aide de la fonc-
tion getchar (getc();)
Les fonctions getchar et putchar permettent la récupération ou le placement d'un octet de données sur
la ligne de communication qui est bufférisée (les octets restent stockés sur la ligne tant qu'ils ne sont
pas récupérés). La ligne étant bufférisée lors d'une coupure de connexion, par exemple, certains octets
peuvent rester bloqués dans la ligne. Et donc lors de la réception de la trame de retour, le programme
n'étant pas synchronisé sur le début de celle-ci, les premiers octets lus vont être ceux de la trame pré-
cédente, restés dans le buffer. Donc la trame ne sera pas interprétée correctement. Pour résoudre ce
premier problème, la solution a été de réaliser une machine d'état pour la réception. Nous pouvons
ainsi nous synchroniser sur les octets de Header de la trame de retour, mais aussi contrôler chaque
octet que nous connaissons déjà de la trame de retour, comme l'ID et le Legth.
Figure 40.
Diagramme d'Etat de la réception des trames de retours
Page 38 sur 110
La machine d'états s'applique aussi bien à la fonction READ et WRITE, bien que la trame de retour
pour une écriture ne soit pas d'une grande utilité.
Néanmoins, les erreurs les plus importantes et les plus fréquentes étaient les erreurs de blocage dans le
programme. Il fallait corriger ce problème et faire en sorte que, même si l'AX-12 ne reçoit pas une
trame envoyée par le contrôleur, ou s'il y a une erreur de communication, le programme continu et les
trames suivantes s'envoient. En effet pour notre robot, si le laser oscille et qu'il rate une oscillation
mais continue par la suite son mouvement, ce n'est pas dérangeant. Par contre, s’il s'arrête totalement
d'osciller, il n'est plus d'aucune utilité.
L'envoi d'origine de la trame se faisait de la même façon que la réception d'origine de la trame de re-
tour (sauf qu'ici il s'agira d'un putchar):
Le problème de blocage était dû aux fonctions putchar et getchar. En effet, lorsque l'on fait appel à ces
fonctions, si la ligne n'est pas libre cela va poser problème et le programme va rester bloqué dans ces
fonctions. Afin d'être sûr de ne pas rester bloqué, on va vérifier avant chaque appel de putchar et get-
char si la ligne physique est libre ou prête à être lue. C'est le rôle exact des fonctions writeable et rea-
dable. La structure ne peut donc pas rester un for mais doit être présentée sous forme d'un while:
Bien entendu, toujours dans l'optique de ne pas rester bloqué dans le programme, chaque while de la
partie transmission comme de la partie réception, bénéficie d'une sortie en timeout.
La bibliothèque ne fournit pas beaucoup de fonctions concernant les registres mémoire des AX-12. La
dernière tâche était de compléter la bibliothèque avec des fonctions permettant d'écrire et de lire la
plupart des registres mémoire disponibles.
Page 39 sur 110
Fonctions déjà fournies Fonctions créées sous mbed
Figure 41. Schéma présentant le travail accompli
Une fonction de sécurité a aussi été créée. Elle compare la valeur du registre de position avec la con-
signe qu'on lui donne. Ainsi, tant que la valeur du registre ne sera pas très proche de la consigne, on
relance la trame avec la même consigne. Toujours pour ne pas rester bloqué, cette fonction dispose
d'un timeout.
Le programme principal qui sera implanté sur le robot n'est pas très compliqué à coder. Il suffira de
déclarer les AX-12 présents sur le robot sous forme de lignes de code, puis d'utiliser les fonctions d'in-
terruption CAN du mbed pour communiquer avec l'ensemble du robot. Sous interruption CAN, un
switch va déterminer l'état futur de la machine d'état de la main.
L'identifiant CAN de l'AX-12 Laser est le 0x80
L'identifiant CAN de l'AX-12 Moteur est le 0x30
Page 40 sur 110
Le diagramme d''état de la main est le suivant :
Figure 41. Diagramme d’état de la main robotisée
Étudiant auteur de cette partie : Fabien Lesueur
Page 41 sur 110
Conclusion
Au terme de la partie officielle de ce projet, nous pouvons dire que la plus grande partie du projet a été
traité. La liste ci-dessous résume le travail effectué :
l’asservissement des moteurs
le calcul de trajectoire
la carte capteur
les cartes pour la balise infrarouge
la correction de la bibliothèque des AX12
Il reste cependant à vérifier que les cartes arrivent à communiquées entre elles en BUS CAN, que le
programme de la carte capteur fonctionne et que les cartes finales de la balise infrarouge fonctionnent.
Enfin, le programme de calcul d’un parcours en temps réel sur la carte X12 pour éviter les obstacles
est en cours de conception. Toutes ces étapes doivent être validées avant la compétition qui se déroule-
ra entre le 23 et le 25 mai à Vierzon.
Étudiant auteur de cette partie : Josué Ferreira
Page 42 sur 110
Annexe
PCB de la carte pour les moteurs
+12V GND
Alimentation batterie
Borne
Moins Borne Plus
Borne
Moins
Moteur Droit
Moteur Gauche
Borne Plus Codeur Gauche :
- GND
- CHAB
- CHA
- VCC
- CHB
Codeur Droit :
- GND
- CHAB
- CHA
- VCC
- CHB
Page 43 sur 110
Schéma de connexions des codeurs incrémentaux
Page 44 sur 110
PCB de la carte capteur
Page 45 sur 110
Page 46 sur 110
PCB de la carte d’émission infrarouge
Page 47 sur 110
Page 48 sur 110
PCB de la carte de réception infrarouge
Page 49 sur 110
Page 50 sur 110
PCB de la carte AX12
Page 51 sur 110
Fiche des identifiants CAN
Identifiants Descritption
0x40 4 octets commande moteur droit en position en impulsions
4e octet : sens 0x00 avant et 0x01 arrière Transmission LSB → MSB
0x41 4 octets commande moteur gauche en position en impulsions
4e octet : sens 0x00 avant et 0x01 arrière Transmission LSB → MSB
0x42 Commande d'arrêt du robot : 1octet 0x00
0x43 8 octets commande moteur en vitesse en impulsions toutes les 10ms
4 octets droit → 4 octets gauche Transmission LSB → MSB
0x45 Consigne en position atteinte par le robot : 1 octet 0x01
0x50 8 octets position avant décélération en impulsions
4 octets droit → 4 octets gauche Transmission LSB → MSB
0x51 Autorise la décélération 1 octet 0x01 ou non 0x00
0x52 8 octets coordonnées en impulsions sur x, y et teta en degrée (PSOC → X12)
3 octets droit → 3 octets gauche → 2 octets teta Transmission LSB → MSB
0x53 Initialisation des coordonnées à l'origine 8 octets en impulsions (X12 → PSOC)
3 octets droit → 3 octets gauche → 2 octets teta Transmission LSB → MSB
0x60 à 0x66 Dans l'ordre capteurs long gauche, long droite, court centre, court gauche, court droite,
court extrème gauche, court extrème droite. 1 octet : distance en cm
0x80 Requête d'information de l'état de la balise infrarouge.
1 octet : 0xFF
0x81 Capteurs de la balise recevant le signal : 0x00 rien, 0x01 gauche, 0x02 centre, 0x04 droit
Les variables peuvent s'additionner.
0x90 Activation de l'AX12 du laser : 1 octet: 0xFF
0x91 Activation de l'AX12 de l'éclate-ballon : 1 octet: 0xFF
Page 52 sur 110
Programme C asservissement et odométrie sur PSOC
/* ========================================
*
* Programme MOTORISATION et ODOMETRIE
* Robots CRAC 2013-2014
* Corrigé pour VIERZON par Josué Ferreira
* Professeur: Yves Guinand
*
* ========================================
*/
#include <device.h>
#include <math.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>
#define MM_TO_IMP 61.211343965
#define PI 3,14159265
//Déclaration des variables
char tab_acc[11][10]= {0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 1, 0, 0, 0, 1, 0, 0,
0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
1, 0, 1, 1, 0, 1, 0, 1, 1, 0,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 2, 1, 1, 1, 1, 1,
1, 1, 1, 2, 1, 1, 1, 2, 1, 1,
1, 2, 0, 1, 2, 0, 1, 2, 0, 1,
1, 2, 1, 2, 1, 2, 1, 2, 1, 2,
2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
120,120,120,120,120,120,120,120,120,120};
uint16
tab_sin[91]={0,175,349,523,698,872,1045,1219,1392,1564,1736,1908,2079,2250,2419,2588,2756,2924
,
3090,3256,3420,3584,3746,3907,4067,4226,4384,4540,4695,4848,5000,5150,5299,5446,5592,
5736,5878,6018,6157,6293,6428,6561,6691,6820,6947,7071,7193,7314,7431,7547,7660,7771,
7880,7986,8090,8192,8290,8387,8480,8572,8660,8746,8829,8910,8988,9063,9135,9205,9272,
9336,9397,9455,9511,9563,9613,9659,9703,9744,9781,9816,9848,9877,9903,9925,9945,9962,
9976,9986,9994,9998,10000};
int32 Codeur_Gauche_Can=0;
int32 Codeur_Gauche=0, anc_codeur_Gauche=0;
int32 Codeur_Gauche_1=0;
int32 valeur_Gauche=0;
int etat_Gauche=-1;
float cons_avtdec_Gauche=0, kp_Gauche=0, alpha_Gauche=0, erreur_Gauche=0,
vmin_Gauche=0, somme_Gauche=0, ki_Gauche=0;
int32 cons_finale_Gauche_Can=0;
int32 cons_finale_Gauche=0;
int32 cons_finale_Gauche_1=0;
char CodeurTxt_Gauche[150];
int32 cons_pos_Gauche=0;
int32 acc_Gauche=0, dec_Gauche=0, vitesse_Gauche=0, vmax_Gauche=0;
int32 acc_Gauche_Can=0,
vmax_Gauche_Can=0,pos_acc_G=0,pos_vit_G=0,pos_acc_G_Can=0,pos_vit_G_Can=0;
int32 Codeur_Droite_Can=0;
int32 Codeur_Droite=0, anc_codeur_Droite=0;
int32 Codeur_Droite_1=0;
int32 valeur_Droite=0;
int etat_Droite=-1;
float cons_avtdec_Droite=0, kp_Droite=0, alpha_Droite=0, erreur_Droite=0,
vmin_Droite=0, somme_Droite=0, ki_Droite=0;
int32 cons_finale_Droite_Can=0;
int32 cons_finale_Droite=0,cons_position_Droite_ant=0;
int32 cons_finale_Droite_1=0;
char CodeurTxt_Droite[150];
int32 cons_pos_Droite=0;
Page 53 sur 110
int32 acc_Droite=0, dec_Droite=0, vitesse_Droite=0, vmax_Droite=0;
int32 acc_Droite_Can=0,
vmax_Droite_Can=0,pos_acc_D=0,pos_vit_D=0,pos_acc_D_Can=0,pos_vit_D_Can=0;
volatile uint16 timer=0;
int16 teta_rob=0;
uint32 x=0,y=0;
uint8 i=0;
unsigned char ack_dec=0;
int bit_sent=0;
int32 t_message;
double coeff_rayon=0; //coeff_rayon =
(double)cons_finale_Gauche/((double)cons_finale_Droite);
//Prototypes des fonctions appellées dans le programme
void motorisation_Gauche(void);
void motorisation_Droite(void);
void odometrie (void);
void main(void)
{
// Initialisation, Start, Enable, ..., des blocs
can_Init();
can_Start();
Clock_1_Start();
Clock_2_Start();
Clock_3_Start();
QuadDec_Gauche_Start();
QuadDec_Droite_Start();
PWM_1_Enable();
PWM_1_Start();
UART_1_Enable();
UART_1_Start();
ISR_TIMER_Start();
CyGlobalIntEnable;
QuadDec_Gauche_SetCounter(0);
QuadDec_Droite_SetCounter(0);
PWM_1_WriteCompare1(0);
PWM_1_WriteCompare2(0);
BRAKE_Gauche_Write(0);
DIR_Gauche_Write(0);
BRAKE_Droite_Write(0);
DIR_Droite_Write(0);
while(1)
{
//Place du code
//Appel des fonctions utilisées
// Environ 10000 IMP/tr
motorisation_Gauche(); //Fabrication du trapèze correspondant au déplacement an-
gulaire voulu sur la roue gauche
motorisation_Droite(); //Fabrication du trapèze correspondant au déplacement an-
gulaire voulu sur la roue droite
odometrie();
can_SendMsgcan_t_coor_cart();
while(timer<1000); //Si la boucle n'est pas trop longue,
timer=0; //Mettre la valeur du timer Ã
0.
Page 54 sur 110
}
}
//La fonction permettant de fabriquer le trapèze gauche sera commenté mais pas celui pour la
droite. En effet, ces deux fonctions sont identiques.
//Ces deux fonctions sont basées sur une machine à états
void motorisation_Gauche(void) //Fontion de fabrication du trapèze gauche
{
char i=0;
if (etat_Gauche == (-1))
{
if(cons_finale_Gauche_Can != cons_finale_Gauche && vmax_Gauche_Can !=0 &&
pos_vit_G_Can !=0)
{
cons_finale_Gauche =cons_finale_Gauche_Can;
//cons_finale_Gauche_Can = 0;
//acc_Gauche = acc_Gauche_Can; //Pas pour l'accélération
//dec_Gauche = -acc_Gauche; //Pas pour la déccélération
//pos_acc_G=pos_acc_G_Can;
pos_vit_G=pos_vit_G_Can;
vmax_Gauche=vmax_Gauche_Can;
vmin_Gauche=30;
vmax_Gauche_Can=0;
pos_vit_G_Can=0;
etat_Gauche = 0;
}
else
{
kp_Gauche=0.01;
ki_Gauche=0.0000000001;
anc_codeur_Gauche= Codeur_Gauche;
Codeur_Gauche = QuadDec_Gauche_GetCounter(); //Lecture de la valeur du co-
deur
erreur_Gauche = cons_finale_Gauche - Codeur_Gauche; //Calcul de l'erreur
somme_Gauche += erreur_Gauche;
alpha_Gauche = kp_Gauche * erreur_Gauche + ki_Gauche * somme_Gauche;
if (alpha_Gauche<-1.0) //Si alpha est plus petit que -1,
{
alpha_Gauche = -1.0; //On le limite à -1
}
else if (alpha_Gauche>1.0) //Si alpha est plus grand que 1,
{
alpha_Gauche = 1.0; //On le plafonne à 1
}
if (alpha_Gauche>0) //Si alpha est positif,
{
PWM_1_WriteCompare1(alpha_Gauche*1199); //On l'applique à la MLI,
DIR_Gauche_Write(0); //Et on va
vers l'avant
}
else //Si alpha est négatif,
{
PWM_1_WriteCompare1(-alpha_Gauche*1199); //On l'applique à la MLI,
DIR_Gauche_Write(1); //Et on va
vers l'arrière
}
}
}
if (etat_Gauche == 0) //Première partie de la machine à état
{
//can_SendMsgcan_t_codeurs();
BRAKE_Gauche_Write(0);
QuadDec_Gauche_SetCounter(0);
kp_Gauche = 0.001634; //Coefficient proportionnel
Page 55 sur 110
cons_pos_Gauche = 0;
anc_codeur_Gauche=0;
//vitesse_Gauche = 0;
etat_Gauche = 1; //Passage dans la seconde partie de la machine à état
somme_Gauche=0;
ki_Gauche=0.0000000001; //Coefficient intégral
}
else if (etat_Gauche == 1) //Seconde partie de la machine à état
{
//can_SendMsgcan_t_codeurs();
acc_Gauche= tab_acc[9][i];
i++;
if(i==10) i=0;
vitesse_Gauche += acc_Gauche; //Phase d'acélération, augmentation de la vi-
tesse
if (cons_finale_Gauche >= 0) //Si le déplacement demandé est positif
{
cons_pos_Gauche += vitesse_Gauche; //La position augmente
}
else //Si le déplacement demandé est négatif
{
cons_pos_Gauche -= vitesse_Gauche; //La position diminue
}
anc_codeur_Gauche= Codeur_Gauche;
Codeur_Gauche = QuadDec_Gauche_GetCounter(); //Lecture dela valeur du codeir
erreur_Gauche = cons_pos_Gauche - Codeur_Gauche; //Calcul de l'erreur
alpha_Gauche = kp_Gauche * erreur_Gauche; //Calcul du futur rapport cyclique
if (alpha_Gauche<-1.0) //Si alpha est plus petit que -1,
{
alpha_Gauche = -1.0; //On le limite à -1
}
else if (alpha_Gauche>1.0) //Si alpha est plus grand que 1,
{
alpha_Gauche = 1.0; //On le plafonne à 1
}
if (alpha_Gauche>0) //Si alpha est positif,
{
PWM_1_WriteCompare1(alpha_Gauche*1199); //On l'applique à la MLI,
DIR_Gauche_Write(0); //Et on va
vers l'avant
}
else //Si alpha est négatif,
{
PWM_1_WriteCompare1(-alpha_Gauche*1199); //On l'applique à la MLI,
DIR_Gauche_Write(1); //Et on va
vers l'arrière
}
if ((fabs(vitesse_Gauche))>=vmax_Gauche) //Si la vitesse maximale est atteinte,
{
if (cons_finale_Gauche >= 0) //Et
si la consigne de position est positive,
{
vitesse_Gauche = vmax_Gauche;
//On maintient la vitesse
}
else //Et
si la consigne de position est négative,
{
vitesse_Gauche = -vmax_Gauche;
//On maintient la vitesse
}
etat_Gauche=2; //Passage dans la troisième partie de la machine à état
}
}
Page 56 sur 110
else if (etat_Gauche == 2) //Troisième partie de la machine à état
{
//can_SendMsgcan_t_codeurs();
cons_pos_Gauche += vitesse_Gauche; //Augmentation de la consigne de position
anc_codeur_Gauche= Codeur_Gauche;
Codeur_Gauche = QuadDec_Gauche_GetCounter(); //Lecture de la valeur du codeur
erreur_Gauche = cons_pos_Gauche - Codeur_Gauche; //Calcul de l'erreur
alpha_Gauche = kp_Gauche * erreur_Gauche; //Calcul du futur rapport cyclique
if (alpha_Gauche<-1.0) //Si alpha est plus petit que -1,
{
alpha_Gauche=-1.0; //On le limite à -1
}
else if (alpha_Gauche>1.0) //Si alpha est plus grand que 1,
{
alpha_Gauche = 1.0; //On le plafonne à 1
}
if (alpha_Gauche>0) //Si alpha est positif,
{
PWM_1_WriteCompare1(alpha_Gauche*1199); //On l'applique à la MLI,
DIR_Gauche_Write(0); //Et on va
vers l'avant
}
else //Si alpha est négatif,
{
PWM_1_WriteCompare1(-alpha_Gauche*1199); //On l'applique à la MLI,
DIR_Gauche_Write(1); //Et on va
vers l'arrière
}
if (fabs(cons_pos_Gauche) >= fabs(pos_vit_G)) //Si la position précédent la
décélération est atteinte
{
etat_Gauche=3; //Passage dans la quatrième partie de la machine à état
}
}
else if (etat_Gauche == 3) //Quatrième partie de la machine à état
{
//can_SendMsgcan_t_codeurs();
dec_Gauche= tab_acc[2][i];
i++;
if(i==10) i=0;
if (cons_finale_Gauche >= 0) //Si le déplacement demandé est positif
{
vitesse_Gauche -= dec_Gauche; //On diminu la vitesse (en valeur absolue)
}
else //Si le déplacement demandé est négatif
{
vitesse_Gauche += dec_Gauche; //On diminue la vitesse (en valeur absolue)
}
if ((fabs(vitesse_Gauche))<= vmin_Gauche) //Si la valeur absolue de la vitesse
atteinte est négative,
{
if(cons_finale_Gauche >= 0) //Et que la consigne est positive,
{
vitesse_Gauche = vmin_Gauche; //On limite la vitesse à(Vmin)
déclarée au début
}
else //Et que la consigne est négative,
{
vitesse_Gauche = -vmin_Gauche; //On limite la vitesse à-(Vmin)
déclarée au début
}
}
cons_pos_Gauche += vitesse_Gauche; //Augmentation de la position
Page 57 sur 110
if ((fabs(cons_pos_Gauche)) >= (fabs(cons_finale_Gauche))) //Si on est arrivé Ã
la consigne de position,
{
cons_pos_Gauche = cons_finale_Gauche;
etat_Gauche = 4; //Passage dans la cinquième partie de la machine à état
}
anc_codeur_Gauche= Codeur_Gauche;
Codeur_Gauche = QuadDec_Gauche_GetCounter(); //Lecture de la valeur du codeur
erreur_Gauche = cons_pos_Gauche - Codeur_Gauche; //Calcul de l'erreur
somme_Gauche += erreur_Gauche; //Calcul de la somme pour l'asservissement
intégral
alpha_Gauche = kp_Gauche * erreur_Gauche + ki_Gauche * somme_Gauche; //Calcul
du rapport cyclique
if (alpha_Gauche<-1.0) //Si alpha est plus petit que -1,
{
alpha_Gauche=-1.0; //On le limite à -1
}
else if (alpha_Gauche>1.0) //Si alpha est plus grand que 1,
{
alpha_Gauche = 1.0; //On le limite à 1
}
if (alpha_Gauche>0) //Si alpha est positif,
{
PWM_1_WriteCompare1(alpha_Gauche*1199); //On l'applique à la MLI,
DIR_Gauche_Write(0); //Et on va
vers l'avant
}
else //Si alpha est négatif,
{
PWM_1_WriteCompare1(-alpha_Gauche*1199); //On l'applique à la MLI,
DIR_Gauche_Write(1); //Et on va
vers l'arrière
}
}
else if (etat_Gauche == 4) //Cinquième partie de la machine à état
{
if (ack_dec==0xFF)
{
//BRAKE_Gauche_Write(1);
PWM_1_WriteCompare1(0); //On met la PWM (MLI) Ã 0
vitesse_Gauche=0;
}
DIR_Gauche_Write(0); //On met la direction à 0
//cons_finale_Gauche = 0;
//cons_finale_Gauche_Can = 0;
anc_codeur_Gauche= Codeur_Gauche;
Codeur_Gauche = QuadDec_Gauche_GetCounter(); //Lecture de la valeur du codeur
if ((etat_Droite == 4)||(etat_Droite == (-1)))
{
can_SendMsgcan_t_consigne_finie();
can_SendMsgcan_t_codeurs();
Codeur_Gauche_1 += Codeur_Gauche;
//Codeur_Gauche = 0;
//anc_codeur_Gauche=0;
//QuadDec_Gauche_SetCounter(0);
etat_Gauche = (-1);
}
else
{
//can_SendMsgcan_t_codeurs();
etat_Gauche = (4);
}
}
Codeur_Gauche_Can = Codeur_Gauche_1 + Codeur_Gauche;
//can_SendMsgcan_t_codeurs();
Page 58 sur 110
}
void motorisation_Droite(void)
{
if (etat_Droite == (-1))
{
if(cons_finale_Droite_Can != cons_finale_Droite && vmax_Droite_Can !=0 &&
pos_vit_D_Can !=0)
{
cons_finale_Droite =cons_finale_Droite_Can;
//cons_finale_Droite_Can = 0;
//acc_Droite = acc_Droite_Can; //Pas pour l'accélération
//dec_Droite = -acc_Droite; //Pas pour la déccélération
//pos_acc_D=pos_acc_D_Can;
pos_vit_D=pos_vit_D_Can;
vmax_Droite=vmax_Droite_Can;
vmin_Droite=30;
vmax_Droite_Can=0;
pos_vit_D_Can=0;
etat_Droite = 0;
}
else
{
kp_Droite=0.01;
ki_Droite=0.0000000001;
anc_codeur_Droite= Codeur_Droite;
Codeur_Droite = QuadDec_Droite_GetCounter(); //Lecture de la valeur du co-
deur
erreur_Droite = cons_finale_Droite - Codeur_Droite; //Calcul de l'erreur
somme_Droite += erreur_Droite;
alpha_Droite = kp_Droite * erreur_Droite + ki_Droite * somme_Droite;
if (alpha_Droite<-1.0) //Si alpha est plus petit que -1,
{
alpha_Droite = -1.0; //On le limite à -1
}
else if (alpha_Droite>1.0) //Si alpha est plus grand que 1,
{
alpha_Droite = 1.0; //On le plafonne à 1
}
if (alpha_Droite>0) //Si alpha est positif,
{
PWM_1_WriteCompare2(alpha_Droite*1199); //On l'applique à la MLI,
DIR_Droite_Write(0); //Et on va
vers l'avant
}
else //Si alpha est négatif,
{
PWM_1_WriteCompare2(-alpha_Droite*1199); //On l'applique à la MLI,
DIR_Droite_Write(1); //Et on va
vers l'arrière
}
}
}
else if (etat_Droite == 0)
{
//can_SendMsgcan_t_codeurs();
BRAKE_Droite_Write(0);
QuadDec_Droite_SetCounter(0);
kp_Droite = 0.001634;
cons_pos_Droite = 0;
anc_codeur_Droite = 0;
//vitesse_Droite = 0;
etat_Droite = 1;
somme_Droite=0;
Page 59 sur 110
ki_Droite=0.000000001;
coeff_rayon = (double)cons_finale_Droite/((double)cons_finale_Gauche);
}
else if (etat_Droite == 1)
{
//can_SendMsgcan_t_codeurs();
vitesse_Droite = coeff_rayon*vitesse_Gauche;
if (cons_finale_Droite >= 0)
{
cons_pos_Droite += vitesse_Droite;
}
else
{
cons_pos_Droite -= vitesse_Droite;
}
anc_codeur_Droite= Codeur_Droite;
Codeur_Droite = QuadDec_Droite_GetCounter();
erreur_Droite = cons_pos_Droite - Codeur_Droite;
alpha_Droite = kp_Droite * erreur_Droite;
if (alpha_Droite<-1.0)
{
alpha_Droite = -1.0;
}
else if (alpha_Droite>1.0)
{
alpha_Droite = 1.0;
}
if (alpha_Droite>0)
{
PWM_1_WriteCompare2(alpha_Droite*1199);
DIR_Droite_Write(0);
}
else
{
PWM_1_WriteCompare2(-alpha_Droite*1199);
DIR_Droite_Write(1);
}
if (etat_Gauche == 2)
{
if (cons_finale_Droite >= 0)
{
vitesse_Droite = vmax_Droite;
}
else
{
vitesse_Droite = -vmax_Droite;
}
etat_Droite=2;
}
}
else if (etat_Droite == 2)
{
//can_SendMsgcan_t_codeurs();
cons_pos_Droite += vitesse_Droite;
anc_codeur_Droite= Codeur_Droite;
Codeur_Droite = QuadDec_Droite_GetCounter();
erreur_Droite = cons_pos_Droite - Codeur_Droite;
alpha_Droite = kp_Droite * erreur_Droite;
if (alpha_Droite<-1.0)
{
Page 60 sur 110
alpha_Droite=-1.0;
}
else if (alpha_Droite>1.0)
{
alpha_Droite = 1.0;
}
if (alpha_Droite>0)
{
PWM_1_WriteCompare2(alpha_Droite*1199);
DIR_Droite_Write(0);
}
else
{
PWM_1_WriteCompare2(-alpha_Droite*1199);
DIR_Droite_Write(1);
}
if (fabs(cons_pos_Droite) >= fabs(pos_vit_D))
{
etat_Droite=3;
}
}
else if (etat_Droite == 3)
{
//can_SendMsgcan_t_codeurs();
vitesse_Droite= coeff_rayon*vitesse_Gauche;
if ((fabs(vitesse_Droite))<=0)
{
if(cons_finale_Droite >= 0)
{
vitesse_Droite = vmin_Droite;
}
else
{
vitesse_Droite = -vmin_Droite;
}
}
cons_pos_Droite += vitesse_Droite;
if ((fabs(cons_pos_Droite)) >= (fabs(cons_finale_Droite)))
{
cons_pos_Droite = cons_finale_Droite;
etat_Droite = 4;
}
anc_codeur_Droite= Codeur_Droite;
Codeur_Droite = QuadDec_Droite_GetCounter();
erreur_Droite = cons_pos_Droite - Codeur_Droite;
somme_Droite += erreur_Droite;
alpha_Droite = kp_Droite * erreur_Droite + ki_Droite * somme_Droite;
if (alpha_Droite<-1.0)
{
alpha_Droite=-1.0;
}
else if (alpha_Droite>1.0)
{
alpha_Droite = 1.0;
}
if (alpha_Droite>0)
{
PWM_1_WriteCompare2(alpha_Droite*1199);
DIR_Droite_Write(0);
}
else
Page 61 sur 110
{
PWM_1_WriteCompare2(-alpha_Droite*1199);
DIR_Droite_Write(1);
}
}
else if (etat_Droite == 4)
{
if (ack_dec==0xFF)
{
//BRAKE_Droite_Write(1);
PWM_1_WriteCompare2(0);
vitesse_Droite=0;
}
DIR_Droite_Write(0);
//cons_finale_Droite = 0;
//cons_finale_Droite_Can = 0;
anc_codeur_Droite= Codeur_Droite;
Codeur_Droite = QuadDec_Droite_GetCounter();
if ((etat_Gauche == 4)||(etat_Gauche == (-1)))
{
// can_SendMsgcan_t_consigne_finie();
//can_SendMsgcan_t_codeurs();
Codeur_Droite_1 += Codeur_Droite;
//Codeur_Droite = 0;
//anc_codeur_Droite=0;
//QuadDec_Droite_SetCounter(0);
etat_Droite = (-1);
}
else
{
//can_SendMsgcan_t_codeurs();
etat_Droite = (4);
}
}
Codeur_Droite_Can = Codeur_Droite_1 + Codeur_Droite;
//can_SendMsgcan_t_codeurs();
}
void odometrie (void)
{
teta_rob= teta_rob + (((Codeur_Droite - anc_codeur_Droite) - (Codeur_Gauche -
anc_codeur_Gauche))*180/(14691*PI));
if(teta_rob>360)
{
while(teta_rob>(2*PI)) teta_rob= teta_rob - 360;
}
else if (teta_rob<0)
{
while(teta_rob<0) teta_rob= teta_rob + 360;
}
if(teta_rob<=90)
{
x= x + (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -
anc_codeur_Gauche))*tab_sin[90-teta_rob])/20000;
y= y + (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -
anc_codeur_Gauche))*tab_sin[teta_rob])/20000;
}
else if(teta_rob<=180)
{
x= x - (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -
anc_codeur_Gauche))*tab_sin[teta_rob-90])/20000;
y= y + (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -
anc_codeur_Gauche))*tab_sin[180-teta_rob])/20000;
}
else if(teta_rob<=270)
{
x= x - (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -
anc_codeur_Gauche))*tab_sin[270-teta_rob])/20000;
Page 62 sur 110
y= y - (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -
anc_codeur_Gauche))*tab_sin[teta_rob-180])/20000;
}
else if(teta_rob<360)
{
x= x + (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -
anc_codeur_Gauche))*tab_sin[360-teta_rob])/20000;
y= y - (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -
anc_codeur_Gauche))*tab_sin[teta_rob-270])/20000;
}
}
/* [] END OF FILE */
Page 63 sur 110
Programme C des fonctions de calcul et de trajectoire
/*************************************************************/
/* Bibliothèque VIERZON 2014 */
/* */
/* Auteur : Josué Ferreira */
/* Mise à jour : 25/03/2014 */
/* */
/*************************************************************/
/*********** Déclaration des vitesses et accélération max
**********/
#define VROB 30606 // 500mm/sec *61.211
#define AROB 12242 // 200mm/sec² *61.211
/****** Déclaration des variables pour les trajectoires fixes
******/
UINT8 msg_ext_t1_p1[5]={0x83,0x40,0x01,0x00,0x00};
//virage r=1.9m et 40°
UINT8 msg_int_t1_p1[5]={0x72,0x18,0x01,0x00,0x00};
UINT8 msg_vit_t1g_p1[8]={0x1E,0x01,0x00,0x00, 0x46,0x01,0x00,0x00};
//à gauche vit_g => vit_d
UINT8 msg_pos_vit_t1g_p1[8]={0x4D,0x02,0x00,0x00,
0xA2,0x02,0x00,0x00};
UINT8 msg_vit_t1d_p1[8]={0x46,0x01,0x00,0x00, 0x1E,0x01,0x00,0x00};
//à doite
UINT8 msg_pos_vit_t1d_p1[8]={0xA2,0x02,0x00,0x00,
0x4D,0x02,0x00,0x00};
UINT8 msg_int_t1_p2[5]={0x5E,0x58,0x03,0x00,0x00};
//virage r=6.2 et 90°
UINT8 msg_ext_t1_p2[5]={0x83,0xB2,0x03,0x00,0x00};
UINT8 msg_vit_t1g_p2[8]={0x82,0x02,0x00,0x00, 0x45,0x02,0x00,0x00};
UINT8 msg_pos_vit_t1g_p2[8]={0x51,0xB7,0x02,0x00,
0x16,0x75,0x02,0x00};
UINT8 msg_vit_t1d_p2[8]={0x23,0x01,0x00,0x00, 0x40,0x01,0x00,0x00};
UINT8 msg_pos_vit_t1d_p2[8]={0xCF,0x14,0x03,0x00,
0x52,0x61,0x03,0x00};
/********** Déclaration des tableaux de sinus et tangente
**********/
const UINT16
tab_sin[91]={0,175,349,523,698,872,1045,1219,1392,1564,1736,1908,207
9,2250,2419,2588,2756,2924,
3090,3256,3420,3584,3746,3907,4067,4226,4384,4540,4695,4848,5000,515
0,5299,5446,5592,
5736,5878,6018,6157,6293,6428,6561,6691,6820,6947,7071,7193,7314,743
1,7547,7660,7771,
7880,7986,8090,8192,8290,8387,8480,8572,8660,8746,8829,8910,8988,906
3,9135,9205,9272,
Page 64 sur 110
9336,9397,9455,9511,9563,9613,9659,9703,9744,9781,9816,9848,9877,990
3,9925,9945,9962,
9976,9986,9994,9998,10000};
const UINT16
tab_tan[90]={0,17,35,52,70,87,105,123,141,158,176,194,213,231,249,26
8,287,306,325,344,364,384,
404,424,445,466,488,510,532,554,577,601,625,649,675,700,727,754,781,
810,839,869,
900,933,966,1000,1036,1072,1111,1150,1192,1235,1280,1327,1376,1428,1
483,1540,1600,
1664,1732,1804,1881,1963,2050,2145,2246,2356,2475,2605,2747,2904,307
8,3271,3487,
3732,4011,4331,4705,5145,5671,6314,7115,8144,9514,11430,14301,19081,
28636,57290};
/*************** Variables ***********************/
UINT8 message_stop[4]={0x00,0x00,0x00,0x00};
UINT8 message_stp=0x00;
UINT8 message_dec=0xFF;
UINT8 message_nondec=0x00;
char etat=0;
UINT8 flag_parc=0x00, flag_bp=0x00;
UINT32 distance_G=0,distance_D=0;
UINT32 x=0,y=0,x_aff,y_aff;
UINT16 teta=0;
UINT8 coord_init[8]={0x4B,0x51,0x00, 0x4B,0x51,0x00, 0x2D,0x00};
// coordonnées après calibrage
UINT8 cpt_long_g=0, cpt_long_d=0, cpt_court_f=0, cpt_court_g=0,
cpt_court_d=0, cpt_court_extd=0, cpt_court_extg=0;
UINT8 etat_balise=0;
/********** Déclaration des prototypes des fonctions *********/
void parcours (void);
void fct_rcpt(void);
void calcul_rayon_courbe (UINT32 xo, UINT32 yo, UINT32 x1, UINT32
y1, UINT32 teta_rob, UINT32* dist_rob, UINT32* dist_g, UINT32*
dist_d, UINT32* vit_g, UINT32* vit_d, UINT32* pos_av_dec_g, UINT32*
pos_av_dec_d);
void placement (void);
void envoie_can (UINT8 msg_gauche[5],UINT8 msg_droite[5],UINT8
msg_vitgd[8],UINT8 msg_pos_vitgd[8],UINT8 ack_dec);
char calcul_vecteur (UINT32 xo, UINT32 yo, UINT32 x1, UINT32 y1,
UINT16* teta, UINT32* distance);
Page 65 sur 110
void calcul_vitesse_roues (UINT32 dist_rob, UINT32 dist_ext, UINT32
dist_int, UINT32* vit_ext, UINT32* vit_int, UINT32* pos_av_dec_ext,
UINT32* pos_av_dec_int);
void determination_trajectoire (void);
/*************************************************************/
/* Nom de fonction : parcours */
/* Description : Selection de la trajectoire fixe selon */
/* le bouton poussoir. */
/* Envoie les données des rayons de courbure */
/* dans l'ordre */
/*************************************************************/
void parcours (void)
{
switch(etat)
{
case 0: if(BP1==0)
{
en-
voie_can(msg_int_t1_p1,msg_ext_t1_p1,msg_vit_t1g_p1,msg_pos_vit_t1g_
p1,message_nondec); // Parcours 1
flag_parc=0x0F;
while(BP1==0);
}
if(BP2==0)
{
en-
voie_can(msg_ext_t1_p1,msg_int_t1_p1,msg_vit_t1d_p1,msg_pos_vit_t1d_
p1,message_nondec); //Parcours 2
flag_parc=0xF0;
while(BP1==0);
}
case 1: switch (flag_parc)
{
case 0x0F:
envoie_can(msg_ext_t1_p2,msg_int_t1_p2,msg_vit_t1g_p2,msg_pos_vit_t1
g_p2,message_dec); //Parcours 1 suite
PORTA=PORTA|0xF0;
etat=2;
CAN_send_message(0x80, 1, &message_dec);
break;
case 0xF0: en-
voie_can(msg_int_t1_p2,msg_ext_t1_p2,msg_vit_t1d_p2,msg_pos_vit_t1d_
p2,message_dec); //Parcours 2 suite
PORTA=PORTA|0xF0;
etat=2;
break;
}
}
if(BP0==0)
Page 66 sur 110
{
while(BP2==0);
// arrêt du robot si BP0
CAN_send_message(0x42,1,&message_stp);
PIT_wait(50*MS);
}
}
/*************************************************************/
/* Nom de fonction : placement */
/* Description : permet de calibrer le robot dans la zone */
/* blanche à 33 cm en x et y */
/* */
/*************************************************************/
void placement (void)
{
UINT8 msg_recul[5]={0x60,0xA7,0x00,0x00,0x01};
UINT8 msg_avance[5]={0xD2,0x2F,0x00,0x00,0x00};
UINT8 msg_tourg[5]={0x12,0x2D,0x00,0x00,0x00};
UINT8 msg_tourd[5]={0x12,0x2D,0x00,0x00,0x01};
UINT8 msg_tourg2[5]={0x89,0x16,0x00,0x00,0x01};
UINT8 msg_tourd2[5]={0x89,0x16,0x00,0x00,0x00};
UINT8 msg_vit_recul_avance[8]={0x64,0x00,0x00,0x00,
0x64,0x00,0x00,0x00};
UINT8 msg_pos_vit_recul[8]={0x60,0xA7,0x00,0x00,
0x60,0xA7,0x00,0x00};
UINT8 msg_pos_vit_avance[8]={0xDD,0x23,0x00,0x00,
0xDD,0x23,0x00,0x00};
UINT8 msg_pos_vit_tour[8]={0x12,0x2D,0x00,0x00,
0x12,0x2D,0x00,0x00};
UINT8 msg_pos_vit_tour2[8]={0x89,0x16,0x00,0x00,
0x89,0x16,0x00,0x00};
UINT32 ant_dist_g=1,ant_dist_d=1;
en-
voie_can(msg_recul,msg_recul,msg_vit_recul_avance,msg_pos_vit_recul,
message_dec);
while(etat==0 && flag_bp==0x00)
{
fct_rcpt(); // attente de fin de consigne
}
etat=0;
flag_bp=0x00;
CAN_send_message(0x42,1,&message_stp);
PIT_wait(50*MS);
etat=0;
en-
voie_can(msg_avance,msg_avance,msg_vit_recul_avance,msg_pos_vit_avan
ce,message_dec);
Page 67 sur 110
while(etat==0)
{
fct_rcpt();
}
etat=0;
en-
voie_can(msg_tourg,msg_tourd,msg_vit_recul_avance,msg_pos_vit_tour,m
essage_dec);
while(etat==0)
{
fct_rcpt();
}
etat=0;
en-
voie_can(msg_recul,msg_recul,msg_vit_recul_avance,msg_pos_vit_recul,
message_dec);
while(etat==0 && flag_bp==0x00)
{
fct_rcpt();
}
etat=0;
flag_bp=0x00;
CAN_send_message(0x42,1,&message_stp);
PIT_wait(50*MS);
etat=0;
en-
voie_can(msg_avance,msg_avance,msg_vit_recul_avance,msg_pos_vit_avan
ce,message_dec);
while(etat==0)
{
fct_rcpt();
}
etat=0;
CAN_send_message(0x42,1,&message_stp);
PIT_wait(50*MS);
etat=0;
en-
voie_can(msg_tourg2,msg_tourd2,msg_vit_recul_avance,msg_pos_vit_tour
2,message_dec);
Page 68 sur 110
while(etat==0)
{
fct_rcpt();
}
etat=0;
CAN_send_message(0x42,1,&message_stp);
PIT_wait(50*MS);
etat=0;
CAN_send_message(0x53,8,coord_init); // envoie coor-
données initiales xo, yo et tetao
PIT_wait(500*MS);
fct_rcpt();
IHM_clear();
IHM_gotoxy(0,0);
IHM_printf("%lu",x_aff);
PIT_wait(MS);
IHM_gotoxy(1,0);
IHM_printf("%lu",y_aff);
PIT_wait(MS);
}
/*************************************************************/
/* Nom de fonction : envoie_can */
/* Description : envoie les données à la carte moteur */
/* distance droite et gauche */
/* vitesse max droite et gauche */
/* distance avant décélération droite gauche */
/* */
/*************************************************************/
void envoie_can (UINT8 msg_gauche[5],UINT8 msg_droite[5],UINT8
msg_vitgd[8],UINT8 msg_pos_vitgd[8],UINT8 ack_dec)
{
CAN_send_message(0x40, 5, msg_droite);
CAN_send_message(0x41, 5, msg_gauche);
CAN_send_message(0x43, 8, msg_vitgd);
CAN_send_message(0x50, 8, msg_pos_vitgd);
CAN_send_message(0x51, 1, &ack_dec);
PIT_wait(50*MS);
}
/*************************************************************/
/* Nom de fonction : calul_rayon_courbe */
/* Description : calcul le rayon de courbure en fonction */
/* des coordonnées actuelles du robot */
/* (xo,yo,tetao) et des coordonnées de la position */
/* future (x1,y1) */
/* */
/* Fournie les distances, vitesses max et */
/* position avant décélération gauche et droite */
/* */
Page 69 sur 110
/* Variables : UINT 32 xo, yo, teta_rob, x1, y1, */
/* UINT 32* dist_rob, dist_gauche, dist_droite */
/* UINT 32* vitesse_max_gauche et droite */
/* UINT 32* position_avant_decélération g et d */
/*************************************************************/
void calcul_rayon_courbe (UINT32 xo, UINT32 yo, UINT32 x1, UINT32
y1, UINT32 teta_rob, UINT32* dist_rob, UINT32* dist_g, UINT32*
dist_d, UINT32* vit_g, UINT32* vit_d, UINT32* pos_av_dec_g, UINT32*
pos_av_dec_d)
{
UINT32 dist_rayon=0, corde=0;
UINT16 teta_deplace=0, teta_rayon=0, tmp;
calcul_vecteur(xo,yo,x1,y1,&teta_deplace,&corde);
if (teta_rob>teta_deplace)
{
tmp= teta_rob - teta_deplace;
if(tmp<=90)
teta_rayon= 2*tmp;
else
teta_rayon= 2*(360-tmp);
dist_rayon= 10000UL*(corde/(2UL*tab_sin[teta_rayon/2])); //
70/99 = 1/sqrt(2)
*dist_rob= (teta_rayon*10UL*dist_rayon)/573UL;
// 10/573 = PI/180
*dist_g= (teta_rayon*10UL*(dist_rayon+7345UL))/573UL;
*dist_d= (teta_rayon*10UL*(dist_rayon-7345UL))/573UL;
calcul_vitesse_roues
(*dist_rob,*dist_g,*dist_d,vit_g,vit_d,pos_av_dec_g,pos_av_dec_d);
}
else if(teta_rob<teta_deplace)
{
tmp= teta_deplace - teta_rob;
if(tmp<=90)
teta_rayon= 2*tmp;
else
teta_rayon= 2*(360-tmp);
dist_rayon= 10000UL*(corde/(2UL*tab_sin[teta_rayon/2]));
*dist_rob= (teta_rayon*10UL*dist_rayon)/573UL;
*dist_d= (teta_rayon*10UL*(dist_rayon+7345UL))/573UL;
*dist_g= (teta_rayon*10UL*(dist_rayon-7345UL))/573UL;
calcul_vitesse_roues
(*dist_rob,*dist_d,*dist_g,vit_d,vit_g,pos_av_dec_d,pos_av_dec_g);
}
else
{
*dist_rob= corde;
*dist_d= corde;
Page 70 sur 110
*dist_g= corde;
calcul_vitesse_roues
(*dist_rob,*dist_g,*dist_d,vit_g,vit_d,pos_av_dec_g,pos_av_dec_d);
}
}
/*************************************************************/
/* Nom de fonction : calul_vecteur */
/* Description : calcul les coordonnées polaires du vecteur */
/* déplacement entre la position actuelle */
/* et la position future en cartésien */
/* */
/* Fournie teta et la norme du vecteur */
/* */
/* Variables : UINT 32 xo, yo, x1, y1, */
/* UINT 32* teta */
/* UINT 32* distance */
/*************************************************************/
char calcul_vecteur (UINT32 xo, UINT32 yo, UINT32 x1, UINT32 y1,
UINT16* teta_rob, UINT32* distance)
{
UINT8 index=0;
UINT16 quotient;
if(x1>xo && y1>=yo)
{
quotient= ((y1-yo)*1000)/(x1-xo);
while(quotient>=tab_tan[index])
{
index++;
}
if((quotient-tab_tan[index-1])<(tab_tan[index]-quotient))
*teta_rob=index-1;
else
*teta_rob=index;
}
else if(x1<xo && y1>=yo)
{
quotient= ((y1-yo)*1000)/(xo-x1); //symétrie
while(quotient>=tab_tan[index])
{
index++;
}
if((quotient-tab_tan[index-1])<(tab_tan[index]-quotient))
*teta_rob=180-(index-1);
else
*teta_rob=180-index;
Page 71 sur 110
}
else if(x1<xo && y1<yo)
{
quotient= ((yo-y1)*1000)/(xo-x1); //symétrie
while(quotient>=tab_tan[index])
{
index++;
}
if((quotient-tab_tan[index-1])<(tab_tan[index]-quotient))
*teta_rob=180+(index-1);
else
*teta_rob=180+index;
}
else if(x1>xo && y1<yo)
{
quotient= ((yo-y1)*1000)/(x1-xo); //symétrie
while(quotient>=tab_tan[index])
{
index++;
}
if((quotient-tab_tan[index-1])<(tab_tan[index]-quotient))
*teta_rob=360-(index-1);
else
*teta_rob=360-index;
}
else if(x1==xo && y1>yo)
*teta_rob= 90;
else if(x1==xo && y1<yo)
*teta_rob= 270;
else
return -1;
if(*teta_rob<45)
*distance= 100*(100*(x1-xo)/tab_sin[90-*teta_rob]);
else if(*teta_rob<=90)
*distance= 100*(100*(y1-yo)/tab_sin[*teta_rob]);
else if(*teta_rob<=180)
*distance= 100*(100*(y1-yo)/tab_sin[180-*teta_rob]);
else if(*teta_rob<=270)
*distance= 100*(100*(y1-yo)/tab_sin[*teta_rob-180]);
else if(*teta_rob<360)
*distance= 100*(100*(y1-yo)/tab_sin[*teta_rob-270]);
return 1;
Page 72 sur 110
}
/*************************************************************/
/* Nom de fonction : calul_vitesse_roues */
/* Description : calcul la vitesse max des deux roues et */
/* la position avant la décélération en */
/* fonction des distances à parcourir entre */
/* les roues */
/* */
/* */
/* Variables : UINT 32 distance_robot et roue extérieur */
/* et intérieur */
/* UINT 32* vitesse max extérieur et intérieur */
/* UINT 32* position avant décélération */
/* des roues droites et gauches */
/*************************************************************/
void calcul_vitesse_roues (UINT32 dist_rob, UINT32 dist_ext, UINT32
dist_int, UINT32* vit_ext, UINT32* vit_int, UINT32* pos_av_dec_ext,
UINT32* pos_av_dec_int)
{
UINT32 rapport_ext, rapport_int, ac_ext, ac_int, tps_acc,
dist_acc_ext, dist_acc_int;
rapport_ext= (dist_ext*1000UL)/dist_rob;
rapport_int= (dist_int*1000UL)/dist_rob;
*vit_ext= ((UINT32)rapport_ext*VROB)/1000UL; // imp/sec
*vit_int= ((UINT32)rapport_int*VROB)/1000UL;
ac_ext= ((UINT32)rapport_ext*AROB)/1000UL; // imp/sec
ac_int= ((UINT32)rapport_int*AROB)/1000UL;
tps_acc= (UINT32)VROB*100/AROB;
dist_acc_ext= ac_ext*tps_acc*tps_acc/20000UL;
dist_acc_int= ac_int*tps_acc*tps_acc/20000UL;
*pos_av_dec_ext= dist_ext-dist_acc_ext;
*pos_av_dec_int= dist_int-dist_acc_int;
*vit_ext= *vit_ext/100UL; // nb imp tt les 10ms
clock automate sur psoc
*vit_int= *vit_int/100UL;
}
Page 73 sur 110
Programme C de la carte capteur
//*************************************************************************************//
//********************** PROGRAMME CARTE CAPTEUR ***********************//
//********************** Coupe robotique Vierzon 2013-2014 ***********************//
//*************************************************************************************//
#include "X12_S1_lib.h"
#include <hidef.h>
#include "derivative.h"
#define Y1 0
#define Y2 1
#define D0 2
#define D1 3
#define D2 4
#define D3 5
#define D4 6
#define SEUIL_DISTANCE 10
#define SEUIL_LONG 50
#define SEUIL_COURT 15
#define WAIT 100
#define DETECTION_LONG 99
#define DETECTION_DANGER_CENTRE 98
#define DETECTION_DANGER_GAUCHE 97
#define DETECTION_DANGER_DROITE 96
#define DETECTION_DANGER_EXTREM_GAUCHE 95
#define DETECTION_DANGER_EXTREM_DROITE 94
#define BOUTON_POISSOIR 93
#define DETECTION_ZONE 92
unsigned char test_CAP_2Y(char select);
unsigned char test_CAP_2D(char select);
void PORTS_init(void);
void main (void)
{
//********************************* Déclaration de variable
*********************************//
UINT8 zero=0;
UINT8 val_bp=1;
UINT8 val_zone;
UINT8 passage=1;
UINT8 passage_zero_long=0;
UINT8 passage_zero_court_centre=0;
UINT8 passage_zero_court_gauche=0;
UINT8 passage_zero_court_droite=0;
UINT8 passage_zero_court_extrem_gauche=0;
UINT8 passage_zero_court_extrem_droite=0;
UINT8 taille_msg=1;
UINT8 detection_court_centre=0;
UINT8 detection_court_gauche=0;
UINT8 detection_court_droite=0;
UINT8 detection_court_extrem_gauche=0;
UINT8 detection_court_extrem_droite=0;
UINT8 etat=0x00;
UINT16 val_comp_10_gauche=0;
UINT16 val_comp_10_droite=0;
UINT8 CAN_back=0;
UINT8 cap_long_gauche;
UINT8 cap_long_droite;
Page 74 sur 110
UINT8 cap_court_centre;
UINT8 cap_court_gauche;
UINT8 cap_court_droite;
UINT8 cap_court_extrem_gauche;
UINT8 cap_court_extrem_droite;
UINT8 message;
PTT=0x00;
//******************************************************************************************//
CAN_init();
X12_init();
ATD_init();
PORTS_init();
IHM_clear();
PTP=0x00;
/*
ID CNY 0x31 CNY
Led Y0 (capteur long) = 0x01
Led Y1 (capteur long) = 0x02
Led D0 (capteur court) = 0x04
Led D1 (capteur court) = 0x08
Led D2 (capteur court) = 0x10
Led D3 (capteur court) = 0x20
Led D4 (capteur court) = 0x80
*/
while(1)
{
//*************** Cap_val ***************//
cap_long_gauche=test_CAP_2Y(Y1);
cap_long_droite=test_CAP_2Y(Y2);
cap_court_centre=test_CAP_2D(D0);
cap_court_gauche=test_CAP_2D(D1);
cap_court_droite=test_CAP_2D(D2);
cap_court_extrem_gauche=test_CAP_2D(D3);
cap_court_extrem_droite=test_CAP_2D(D4);
//**************************************//
switch(etat){
case WAIT:
///////***************************************************************************************
******///////
if( (cap_long_gauche || cap_long_droite ) < SEUIL_LONG )
etat=DETECTION_LONG;
else {
passage=1;
}
///////***************************************************************************************
******///////
if( (cap_court_centre < SEUIL_COURT) && detection_court_centre ) etat= DE-
TECTION_DANGER_CENTRE;
else {
detection_court_centre=1;
CAN_send_message(0x62,1,&zero);
}
///////***************************************************************************************
******///////
Page 75 sur 110
if( (cap_court_gauche < SEUIL_COURT) && detection_court_gauche ) etat= DE-
TECTION_DANGER_GAUCHE;
else {
detection_court_gauche=1;
CAN_send_message(0x63,1,&zero);
}
///////***************************************************************************************
******///////
if( (cap_court_droite < SEUIL_COURT) && detection_court_droite ) etat= DE-
TECTION_DANGER_DROITE;
else {
detection_court_droite=1;
CAN_send_message(0x64,1,&zero);
}
///////***************************************************************************************
******///////
if( (cap_court_extrem_gauche < SEUIL_COURT) && detec-
tion_court_extrem_gauche ) etat= DETECTION_DANGER_EXTREM_GAUCHE;
else {
detection_court_extrem_gauche=1;
CAN_send_message(0x65,1,&zero);
}
///////***************************************************************************************
******///////
if( (cap_court_extrem_droite < SEUIL_COURT) && detec-
tion_court_extrem_droite ) etat= DETECTION_DANGER_EXTREM_DROITE;
else {
detection_court_extrem_droite=1;
CAN_send_message(0x66,1,&zero);
}
///////***************************************************************************************
******///////
if((PORTB&07)==0) etat=BOUTON_POISSOIR;
///////***************************************************************************************
******///////
if((PTP&02 )==1) etat=DETECTION_ZONE;
/***/break;
///////***************************************************************************************
*****************************///////
case DETECTION_LONG:
if(cap_long_gauche < SEUIL_LONG) PTP=PTP|0x01;
if(cap_long_droite < SEUIL_LONG) PTP=PTP|0x02;
if( passage == 1 ){
val_comp_10_gauche=cap_long_gauche;
val_comp_10_droite=cap_long_droite;
passage=0;
}
else if (/**/(cap_long_gauche < val_comp_10_gauche-SEUIL_DISTANCE)/**/||
(cap_long_gauche > val_comp_10_gauche+SEUIL_DISTANCE)/**/||
(cap_long_droite < val_comp_10_droite-SEUIL_DISTANCE)/**/||
(cap_long_droite > val_comp_10_droite+SEUIL_DISTANCE)/**/)
{
CAN_send_message(0x60,1,&cap_long_gauche);
CAN_send_message(0x61,1,&cap_long_droite);
val_comp_10_gauche=cap_long_gauche;
Page 76 sur 110
val_comp_10_droite=cap_long_droite;
}
etat=WAIT;
break;
///////***************************************************************************************
******///////
case DETECTION_DANGER_CENTRE:
CAN_send_message(0x62,1,&cap_court_centre);
detection_court_centre=0;
etat=WAIT;
break;
///////***************************************************************************************
******///////
case DETECTION_DANGER_GAUCHE:
CAN_send_message(0x63,1,&cap_court_centre);
detection_court_gauche=0;
etat=WAIT;
break;
///////***************************************************************************************
******///////
case DETECTION_DANGER_DROITE:
CAN_send_message(0x64,1,&cap_court_centre);
detection_court_droite=0;
etat=WAIT;
break;
///////***************************************************************************************
******///////
case DETECTION_DANGER_EXTREM_GAUCHE:
CAN_send_message(0x65,1,&cap_court_centre);
detection_court_extrem_gauche=0;
etat=WAIT;
break;
///////***************************************************************************************
******///////
case DETECTION_DANGER_EXTREM_DROITE:
CAN_send_message(0x66,1,&cap_court_centre);
detection_court_extrem_droite=0;
etat=WAIT;
break;
///////***************************************************************************************
******///////
case BOUTON_POISSOIR:
Page 77 sur 110
CAN_send_message(0x67,1,&val_bp);
etat=WAIT;
break;
///////***************************************************************************************
******///////
case DETECTION_ZONE:
CAN_send_message(0x31,1,&val_zone);
etat=WAIT;
break;
///////***************************************************************************************
******///////
}
}
while(message==9) // Permiere version
/* While(1)*/{
//*************** Cap_val ***************//
cap_long_gauche=test_CAP_2Y(Y1);
cap_long_droite=test_CAP_2Y(Y2);
cap_court_centre=test_CAP_2D(D0);
cap_court_gauche=test_CAP_2D(D1);
cap_court_droite=test_CAP_2D(D2);
cap_court_extrem_gauche=test_CAP_2D(D3);
cap_court_extrem_droite=test_CAP_2D(D4);
//**************************************//
//********* Détection avec Capteurs Longs **********//
if( (cap_long_gauche || cap_long_droite ) < SEUIL_LONG ){
//LED Y0 Y1
if(cap_long_gauche < SEUIL_LONG) PTP=PTP|0x01;
if(cap_long_droite < SEUIL_LONG) PTP=PTP|0x02;
passage_zero_long=0;
//**
if( passage == 1 ){
val_comp_10_gauche=cap_long_gauche;
val_comp_10_droite=cap_long_droite;
passage=0;
}
else if (/**/(cap_long_gauche < val_comp_10_gauche-SEUIL_DISTANCE)/**/||
(cap_long_gauche > val_comp_10_gauche+SEUIL_DISTANCE)/**/||
(cap_long_droite < val_comp_10_droite-SEUIL_DISTANCE)/**/||
(cap_long_droite > val_comp_10_droite+SEUIL_DISTANCE)/**/)
{
CAN_send_message(0x60,1,&cap_long_gauche);
CAN_send_message(0x61,1,&cap_long_droite);
val_comp_10_gauche=cap_long_gauche;
val_comp_10_droite=cap_long_droite;
}
//**
Page 78 sur 110
}
else {
if(passage_zero_long==0)
{
CAN_send_message(0x60,1,&zero);
CAN_send_message(0x61,1,&zero);
PTP=PTP&0xFC;
passage_zero_long=1;
}
}
//************************************************//
//*************** ZONE DANGER ****************//
////////////// Court centre //////////////
if(cap_court_centre < SEUIL_COURT)
{
PTP=PTP|0x04;
passage_zero_court_centre=0;
if(detection_court_centre==0)
{
CAN_send_message(0x62,1,&cap_court_centre);
detection_court_centre=1;
}
}
else
{
PTP=PTP&0xFB;
detection_court_centre=0;
if(passage_zero_court_centre==0)
{
CAN_send_message(0x62,1,&zero);
passage_zero_court_centre=1;
}
}
////////////// Court gauche //////////////
if(cap_court_gauche < SEUIL_COURT)
{
PTP=PTP|0x08;
passage_zero_court_gauche=0;
if(detection_court_gauche==0)
{
CAN_send_message(0x63,1,&cap_court_gauche);
detection_court_gauche=1;
}
}
else
{
PTP=PTP&0xF7;
detection_court_gauche=0;
Page 79 sur 110
if(passage_zero_court_gauche==0)
{
CAN_send_message(0x63,1,&zero);
passage_zero_court_gauche=1;
}
}
////////////// Court droite //////////////
if(cap_court_droite < SEUIL_COURT)
{
PTP=PTP|0x10;
passage_zero_court_droite=0;
if(detection_court_droite==0)
{
CAN_send_message(0x64,1,&cap_court_droite);
detection_court_droite=1;
}
}
else
{
PTP=PTP&0xEF;
detection_court_droite=0;
if(passage_zero_court_droite==0)
{
CAN_send_message(0x64,1,&zero);
passage_zero_court_droite=1;
}
}
////////// Court extrem gauche ///////////
if(cap_court_extrem_gauche < SEUIL_COURT)
{
PTP=PTP|0x20;
passage_zero_court_extrem_gauche=0;
if(detection_court_extrem_gauche==0)
{
CAN_send_message(0x65,1,&cap_court_extrem_gauche);
detection_court_extrem_gauche=1;
}
}
else
{
PTP=PTP&0xDF;
detection_court_extrem_gauche=0;
if(passage_zero_court_extrem_gauche==0)
{
CAN_send_message(0x65,1,&zero);
passage_zero_court_extrem_gauche=1;
}
}
////////// Court extrem droite ///////////
if(cap_court_extrem_droite < SEUIL_COURT)
{
Page 80 sur 110
PTP=PTP|0x80;
passage_zero_court_extrem_droite=0;
if(detection_court_extrem_droite==0)
{
CAN_send_message(0x66,1,&cap_court_extrem_droite);
detection_court_extrem_droite=1;
}
}
else
{
PTP=PTP&0x7F;
detection_court_extrem_droite=0;
if(passage_zero_court_extrem_droite==0)
{
CAN_send_message(0x66,1,&zero);
passage_zero_court_extrem_droite=1;
}
}
/* Fin du While(1)*/}
}
//***************************************************************//
//******* Fonction converstion valeur numérique en cm ***********//
//***************************************************************//
unsigned char test_CAP_2Y(char select)
{
UINT16 cap_0 = 0;
UINT8 dis;
cap_0 = ATD_read(select);
if(cap_0<=2270 && cap_0>2070) //15-20
{
dis = (2870-cap_0)/40;
}
else if(cap_0<=2070 && cap_0>1240) //20-40
{
dis = (2900-cap_0)/41.5;
}
else if(cap_0<=1240 && cap_0>825) //40-60
{
dis = (2070-cap_0)/20.75;
}
else if(cap_0<=825 && cap_0>625) //60-80
{
dis = (1425-cap_0)/10;
}
else if(cap_0<=625 && cap_0>500) //80-100
{
dis = (1125-cap_0)/6.25;
}
else if(cap_0<=500 && cap_0>420) //100-110
{
dis = (1300-cap_0)/8;
}
else if(cap_0<=420 && cap_0>310) //110-130
Page 81 sur 110
{
dis = (1025-cap_0)/5.5;
}
else if(cap_0<=310) // -130
{
dis = (1480-cap_0)/9;
}
else
{
dis = 0;
}
return dis;
}
//***************************************************************
//***************************************************************
//***************************************************************
//***************************************************************
//***************************************************************
//***************************************************************
unsigned char test_CAP_2D(char select)
{
UINT16 cap_0 = 0;
UINT8 dis;
cap_0 = ATD_read(select);
if(cap_0<=2330 && cap_0>1640) //4-6
{
dis = (3710-cap_0)/345;
}
else if(cap_0<=1640 && cap_0>1250) //6-8
{
dis = (2810-cap_0)/195;
}
else if(cap_0<=1250 && cap_0>1020) //8-10
{
dis = (2170-cap_0)/115;
}
else if(cap_0<=1020 && cap_0>740) //10-14
{
dis = (1720-cap_0)/70;
}
else if(cap_0<=740 && cap_0>580) //14-18
{
dis = (1300-cap_0)/40;
}
else if(cap_0<=580 && cap_0>520) //18-20
{
dis = (1120-cap_0)/30;
}
else if(cap_0<=520 && cap_0>430) //20-24
{
dis = (970-cap_0)/22.5;
}
else if(cap_0<=430 && cap_0>370) //24-28
{
dis = (790-cap_0)/15;
}
else if(cap_0<=370 && cap_0>300) //28-34
{
dis = (696.66666667-cap_0)/11.666667;
}
Page 82 sur 110
else if(cap_0<=300 && cap_0>240) //34-40
{
dis = (640-cap_0)/10;
}
else
{
dis = 0;
}
return dis;
}
// *********************************************
// ******* Init PORTS boutons poussoirs ********
// *********************************************
void PORTS_init(void)
{
//Bits 0 à 2 en entrée
//Bits 3 à 7 en sortie
DDRB = 0xF8;
//Pull up
PUCR = 0x12;
//Val par défaut
}
//*********************************************************
Page 83 sur 110
Programme C de la carte d’émission infrarouge
#include <hidef.h>
#include "derivative.h"
#include "X12_S3_lib.h"
/***************************
Programme C
Carte d'émission
Balise Infrarouge
Vierzon 2014
**************************/
#define BAUD 1200 //bit/s
#define CODAGE 1 //0 RZ 1 Manchester 2 Miller
#define TAILLE_TRAME 31 //puiss de 2 -1
#define CODE 0x15; //max 31
// MSB -> LSB
void MCU_init(void);
void init_timer(void);
void init_port(void);
void genere_trame(void);
void codage(void);
unsigned char temp,
i = 0,
j = 0,
porteuse = 0,
cpt = 0,
bit = 0,
sortie,
trame[TAILLE_TRAME];
void main(void) {
MCU_init();
X12_init();
IHM_clear();
genere_trame();
init_port();
init_timer();
while (1);
}
// *************************
// ***** Interruptions *****
// *************************
interrupt void timer_p_it(void)
{
//génération de porteuse
porteuse = !porteuse;
if (bit == 0 && porteuse == 1)
PORTA = 0x07;
else
PORTA = 0;
//génération du code
if (cpt >= 36000/BAUD)
{
codage();
cpt = 0;
}
Page 84 sur 110
else
cpt++;
//flag_reset
PITTF = PITTF | 0x08;
}
// ****************************
// ***** Initialisations ******
// ****************************
void init_port(void)
{
//sortie
DDRA = 0xFF;
PORTA = 0;
}
void init_timer(void)
{
PITCFLMT = PITCFLMT & 0x7F; // bit PITE à 0 (disable PIT module)
PITMTLD1 = 36; // micro timer 1 à 648 Khz
PITMUX = 0x08; // timer 3 => utimer 1
PITLD3 = 8; // timer 3 à 72 kHz (porteuse à 36kHz)
PITCE = PITCE | 0x08; // 0x08 : active le pit 0
PITTF = 0xFF;
PITINTE = PITINTE | 0x08; // 0x08 : iT channel 0 ON
PITCFLMT = PITCFLMT | 0x80; // lance timer
}
//Met le code dans la trame
void genere_trame(void)
{
unsigned char k, code;
code = CODE;
for (k = 0; k < TAILLE_TRAME; k++)
{
if (k==0 || k==1 || k==7)
trame[k] = 10;
else if (k>=8)
trame[k] = 11;
else
trame[k] = (code>>(6-k)&0x01);
}
}
void codage(void)
{
if (CODAGE == 0)
{
//NRZ
if (trame[i]==11)
bit = 1;
else if (trame[i]==10)
bit = 0;
else
bit = trame[i];
}
else if (CODAGE == 1)
{
//Manchester
Page 85 sur 110
if (trame[i]==11)
bit = 1;
else if (trame[i]==10)
bit = 0;
else
bit = trame[i]^j;
}
//Incrémentation i et j
if (i == (TAILLE_TRAME-1) && j == 1)
i = 0;
else if (j == 1)
i++;
j=!j;
}
Page 86 sur 110
Programme C de la carte de réception infrarouge
#include <hidef.h>
#include "derivative.h"
#include "X12_S3_lib.h"
/***************************
Programme C
Carte de réception
Balise Infrarouge
Vierzon 2014
**************************/
#define BAUD 1200 //en bit/s
#define ID_RECEPT 0x80
#define ID_ENVOI 0x81
#define ASK_ANGLE 0x12 // 0x12 demander l'angle sur 8bits
#define TAILLE 1 // en octet
#define TAILLE_TAB 5 // en bit
#define DUREE_ECOUTE 10 // *6.67 ms
#define DUREE_DE_VIE 2400 // *417 us
#define CODE_SECRET 0x15 // max 0x1F ou 31
// MSB -> LSB
// messages : 0x00 = rien
// 0x01 = recepteur gauche
// 0x02 = recepteur central
// 0x04 = recepteur droit
void MCU_init(void);
void init_timer(void);
void init_port(void);
void init_ATD0(void);
void init_rs232(void);
void remplissage_tableau(void);
void reception_manchester(void);
void gestion_CAN(void);
void demo(void);
unsigned char etat = 0,
capteur = 0,
data[2*TAILLE_TAB],
val,
bit,
val_compas,
synchro,
code_recu,
recept_acc[3] = 0,
lu = 0,
hold;
unsigned short time_out = 0;
//*************** MAIN **************
void main(void)
{
MCU_init();
X12_init();
CAN_init_advanced(BAUDRATE_1M,0x00,0xFF,RX_IT_OFF);
//CAN_init;
remplissage_tableau();
demo();
init_port();
//init_ATD0();
//init_rs232();
init_timer();
Page 87 sur 110
while(1)
{
if (synchro == 1)
{
synchro = 0;
}
else if (synchro == 2)
{
//caractère reçu
IHM_gotoxy(0,0);
IHM_printf("%u ",val_compas);
synchro = 0;
}
// else
//PORTA &= 0x07;
}
}
//***********************
// FONCTIONS
//***********************
void reception_manchester()
{
//Detection de synchro et reception code
if (bit==data[etat] && etat < 2*TAILLE_TAB-1)
etat++;
else if (bit==data[etat] && etat == 2*TAILLE_TAB-1)
{
etat = 0;
synchro = 1;
recept_acc[capteur] = 1;
}
else
etat = 0;
}
void remplissage_tableau()
{
//Transforme un octet en codage manchester
//MSB -> LSB
short k;
for (k=0;k<TAILLE_TAB;k++)
{
if (((CODE_SECRET>>k) & 0x01) == 0)
{
data[2*(TAILLE_TAB-k-1)] = 0;
data[2*(TAILLE_TAB-k-1)+1] = 1;
}
else if (((CODE_SECRET>>k) & 0x01) == 1)
{
data[2*(TAILLE_TAB-k-1)] = 1;
data[2*(TAILLE_TAB-k-1)+1] = 0;
}
}
}
void gestion_CAN(void)
{
if (CAN_rx(&trame_Rx) == 0)
{
if (trame_Rx.id == ID_RECEPT && lu == 0 /*&&
trame_Rx.remote_transmitt_request == TRAME_DATA*/)
{
lu = 1;
trame_Tx.id = ID_ENVOI;
trame_Tx.length = TAILLE;
trame_Tx.remote_transmitt_request = TRAME_DATA;
Page 88 sur 110
//trame_Tx.priority = 0;
trame_Tx.data[0] = hold;
while(!CAN_check_tx_buffer_ready());
CAN_tx(&trame_Tx);
}
else if (trame_Rx.id != ID_RECEPT)
lu = 0;
}
else
lu = 0;
}
void demo(void)
{
//Animation LED à l'allumage
char y,z;
for(y=0;y<3;y++)
{
for(z=0;z<3;z++)
{
PORTA = 1<<z;
PIT_wait(200*TIMER_CLOCK_PER_MS);
}
}
PORTA = 2;
PIT_wait(500*TIMER_CLOCK_PER_MS);
PORTA = 5;
PIT_wait(500*TIMER_CLOCK_PER_MS);
PORTA = 2;
PIT_wait(500*TIMER_CLOCK_PER_MS);
PORTA = 5;
}
//***********************//
// //
// INTERRUPTIONS //
// //
//***********************//
interrupt void it_timer(void)
{
static unsigned char i = 0;
//Affichage LED
if (time_out < DUREE_DE_VIE)
{
time_out++;
}
else
{
time_out = 0;
hold = (recept_acc[2]+2*recept_acc[1]+4*recept_acc[1]);
PORTA &= 0xF8;
PORTA |= (hold&0x07);
gestion_CAN();
recept_acc[0]=0;
recept_acc[1]=0;
recept_acc[2]=0;
}
//Switch capteur
if (i == 16*DUREE_ECOUTE-1 && etat == 0)
{
Page 89 sur 110
i=0;
if (capteur==2)
capteur=0;
else
capteur++;
}
else
i++;
//Reception bit
bit=(PORTB>>capteur)&0x01;
//Réception / décodage
reception_manchester();
//lecture via ADC
//ATD0CTL5 = 0x02;
//flag
PITTF = PITTF | 0x01;
}
//IT TIMER demande de données via liason série
interrupt void it_timer_cmps(void)
{
SCI0DRH = 0;
SCI0DRL = ASK_ANGLE;
while((SCI0SR1&0x40)==0);
PITTF = PITTF | 0x03;
}
//IT ATD0 réception via ADC
interrupt void it_ATD0(void)
{
val = (unsigned char)ATD0DR0;
if (val>128) bit = 1; else bit = 0;
reception_manchester();
}
//IT SERIE lecture de la boussole
interrupt void it_serie(void)
{
synchro = 2;
SCI0SR1 = SCI0SR1;
val_compas = SCI0DRL/*&0x7F*/;
}
//RECEPTION CAN
interrupt void it_can_rx(void)
{
CAN0CTL0 = 0x80;
}
interrupt void it_can_wkup(void)
{
CAN0CTL0 = 0x80;
}
//***********************//
// //
// INIT //
// //
//***********************//
void init_timer(void)
{
PITCFLMT = PITCFLMT & 0x7F; // bit PITE à 0 (disable PIT module)
Page 90 sur 110
//PITMTLD0 = 249; // micro timer 0 à 96 Khz
PITMTLD0 = 9;
PITMTLD1 = 239; // micro timer 1 à 100 Khz
PITMUX = 0x0E; // timer 0 => utimer 0, timer1 => utimer1
//PITLD0 = (96000/(BAUD))-1; // timer 0 à 2 fois la vitesse de BAUD
PITLD0 = 999;
PITLD1 = 49999; // timer 1 à 2 Hz (boussole)
PITCE = 0x01; // active les pit 0 et 1
PITTF = 0xFF;
PITINTE = 0x03; // iT channel 0 et 1 ON
PITCFLMT = PITCFLMT | 0x80; // lance timer
}
void init_port(void)
{
DDRA = 0xFF; //tout en sortie
PORTA = 0;
DDRB = 0xF8; //0xF8 sortie 3-7, 0-2 entree
PORTB = 0;
}
void init_ATD0(void)
{
// 8 bits, calés à droite
ATD0CTL0 = 0x01; // wrap sur AN1 (pas utile ici car acq mono canal)
ATD0CTL1 = 0x00; // no ext trig, 8 bits, pas de décharge
ATD0CTL2 = 0x42; // AFFC auto, iT sur fin de séquence
ATD0CTL3 = 0x88; // justifié à droite, pas de fifo (résultat dans ATD0DR0)
ATD0CTL4 = 0x00; // échantillonnage freq max (4 ATD clock cycle)
}
void init_rs232(void)
{
SCI0BDH = 0;
SCI0BDL = 0x9C; // 9600 bauds
SCI0CR1 = 0;
SCI0CR2 = 0x2C; // iT en reception, Rx et Tx actifs
SCI0SR2 = 0; // polarité normale
}
Page 91 sur 110
Programme C de la bibliothèque des AX12
#include "AX12.h"
#include "mbed.h"
AX12::AX12(PinName tx, PinName rx, int ID, int baud)
: _ax12(tx,rx)
{
_baud = baud;
_ID = ID;
_ax12.baud(_baud);
}
int AX12::Set_Secure_Goal(int degres)
{
int error = 0;
int position = 0;
int difference = 0;
int timeout_secure = 0;
int autorisation = 0;
position = Get_Position();
error = Set_Goal(degres);
while ((autorisation == 0) && (timeout_secure < 1000) )
{
position = Get_Position();
//printf("position : %d", position );
error = Set_Goal(degres);
//printf("degres : %d", degres);
difference = degres - position;
//printf ("difference : %d", difference );
if (((difference < 2) && (difference > (-2) )))
autorisation = 1;
timeout_secure++;
}
if ( timeout_secure == 1000)
{
printf (" timeout secure error ");
return(-1);
}
return(error);
}
int AX12::Get_Return_Delay_Time(void)
{
char data[1];
int ErrorCode = read(_ID, AX12_REG_DELAY_TIME, 1, data);
int time = data[0];
time = 2.0 * time;
return(time);
}
int AX12::Get_Baud_Rate(void)
{
char data[1];
int ErrorCode = read(_ID, AX12_REG_BAUD, 1, data);
int baud = data[0];
baud = 2000000 / ( baud + 1 );
return(baud);
Page 92 sur 110
}
/** Reglage du courant minimum necessaire au bon fonctionnement de l'actionneur
// minimum >> Ox000 >> decimal 0
// maximum >> 0x3FF >> decimal 1023
// deflaut >> 0x20 >> decimal 32
*/
int AX12::Set_Punch(int punch)
{
char data[2];
data[0] = punch & 0xff; // bottom 8 bits
data[1] = punch >> 8; // top 8 bits
// write the packet, return the error code
return (write(_ID, AX12_REG_PUNCH, 2, data));
}
int AX12::Get_Punch (void)
{
char data[2];
int ErrorCode = read(_ID, AX12_REG_PUNCH, 2, data);
int punch = data[0] | (data[1]<<8);
return(punch);
}
int AX12::Get_Load_Direction (void)
{
char data[2];
int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data);
int direction = (data[1]>>2) & 0x01;
return(direction);
}
int AX12::Get_Load_Value (void)
{
char data[2];
int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data);
int Load = (data[0] | (data[1]<<8)) & 0x3FF;
return(Load);
}
int AX12::Get_Present_Speed (void)
{
char data[2];
int ErrorCode = read(_ID, AX12_REG_PRESENT_SPEED, 2, data);
int speed = data[0] | (data[1]<<8);
return(speed);
}
int AX12::Get_CCW_Angle_Limit (void)
{
char data[2];
int ErrorCode = read(_ID, AX12_REG_CCW_LIMIT, 2, data);
int angle = data[0] | (data[1]<<8);
angle = (angle * 300) / 1023;
return(angle);
}
Page 93 sur 110
int AX12::Get_CW_Angle_Limit (void)
{
char data[2];
int ErrorCode = read(_ID, AX12_REG_CW_LIMIT, 2, data);
int angle = data[0] | (data[1]<<8);
angle = (angle * 300) / 1023;
return(angle);
}
int AX12::Get_Torque_Enable(void)
{
char data[1];
int ErrorCode = read(_ID, AX12_REG_TORQUE_ENABLE, 1, data);
int enable = data[0];
return(enable);
}
int AX12::Set_Torque_Enable(int etat)
{
char data[1];
data [0] = etat;
int error = write(_ID, AX12_REG_TORQUE_ENABLE, 1, data);
return (error);
}
int AX12::Get_Up_Calibration (void)
{
char data[1];
int ErrorCode = read(_ID, AX12_REG_UP_CALIBRATION, 2, data);
int Up_calibration = data[0] | (data[1]<<8);
return(Up_calibration);
}
int AX12::Get_Down_Calibration (void)
{
char data[1];
int ErrorCode = read(_ID, AX12_REG_DOWN_CALIBRATION, 2, data);
int Dowm_calibration = data[0] | (data[1]<<8);
return(Dowm_calibration);
}
int AX12::Get_ID(void)
{
char data[1];
int ErrorCode = read(_ID, AX12_REG_ID, 1, data);
int id = data[0];
return(id);
}
// Lecture du couple maximum ( retourne la valeur du registre Max Torque de l'AX12 )
int AX12::Get_Max_Torque (void)
{
char data[2];
int ErrorCode = read(_ID, AX12_REG_MAX_TORQUE, 2, data);
int torque = data[0] | (data[1]<<8);
Page 94 sur 110
return(torque);
}
/** Reglage du couple maximum de l'actionneur
// minimum >> Ox000 >> decimal 0
// maximum >> 0x3FF >> decimal 1023
// deflaut >> >> decimal
*/
int AX12::Set_Max_Torque(int torque)
{
char data[2];
data[0] = torque & 0xff; // bottom 8 bits
data[1] = torque >> 8; // top 8 bits
// write the packet, return the error code
return (write(_ID, AX12_REG_MAX_TORQUE, 2, data));
}
/** Reglage de la desactivation des actionneurs si une erreur le concernant se produit
Bit Function
Bit 7 0
Bit 6 If set to 1, torque off when an Instruction Error occurs
Bit 5 If set to 1, torque off when an Overload Error occurs
Bit 4 If set to 1, torque off when a Checksum Error occurs
Bit 3 If set to 1, torque off when a Range Error occurs
Bit 2 If set to 1, torque off when an Overheating Error occurs
Bit 1 If set to 1, torque off when an Angle Limit Error occurs
Bit 0 If set to 1, torque off when an Input Voltage Error occurs
*/
int AX12::Set_Alarm_Shutdown(int valeur)
{
char data[1];
data [0] = valeur;
int val_alarm_shutdown = write(_ID, AX12_REG_ALARM_SHUTDOWN, 1, data);
return (val_alarm_shutdown);
}
/** Reglage de l'activation de l'alarme
Bit Function
Bit 7 0
Bit 6 If set to 1, the LED blinks when an Instruction Error occurs
Bit 5 If set to 1, the LED blinks when an Overload Error occurs
Bit 4 If set to 1, the LED blinks when a Checksum Error occurs
Bit 3 If set to 1, the LED blinks when a Range Error occurs
Bit 2 If set to 1, the LED blinks when an Overheating Error occurs
Bit 1 If set to 1, the LED blinks when an Angle Limit Error occurs
Bit 0 If set to 1, the LED blinks when an Input Voltage Error occurs
*/
int AX12::Set_Alarm_LED(int valeur)
{
char data[1];
data [0] = valeur;
int val_alarmLED = write(_ID, AX12_REG_ALARM_LED, 1, data);
return (val_alarmLED);
}
Page 95 sur 110
// Reglage de la réponse à une instruction
// 0 >> ne repond a aucune instructions
// 1 >> repond seulement aux instructions READ_DATA
// 2 >> repond à toutes les instructions
int AX12::Set_Status_Return_Level(int etat)
{
char data[1];
data [0] = etat;
int val_return_lvl = write(_ID, AX12_REG_SATUS_RETURN, 1, data);
return (val_return_lvl);
}
// Reglage de la tension minimale
// minimum >> Ox32 >> decimal 50
// maximum >> 0xFA >> decimal 250
// deflaut >> 0x3C >> decimal 60
int AX12::Set_Lowest_Voltage(int val_lowest_voltage)
{
char data[1];
data [0] = val_lowest_voltage;
int val_lowvolt = write(_ID, AX12_REG_LOWEST_VOLTAGE, 1, data);
return (val_lowvolt);
}
// Reglage de la tension maximale
// minimum >> Ox32 >> decimal 50
// maximum >> 0xFA >> decimal 250
// deflaut >> 0xBE >> decimal 190
int AX12::Set_Highest_Voltage(int val_highest_voltage)
{
char data[1];
data [0] = val_highest_voltage;
int val_highvolt = write(_ID, AX12_REG_HIGHEST_VOLTAGE, 1, data);
return (val_highvolt);
}
// Reglage du return time delay EN MICRO SECONDE 2uSec * val_delay_time
// minimum >> 0 us
// maximum >> 508 us
// deflaut >> 125 us
int AX12::Set_Delay_Time (int val_delay_time )
{
char data[1];
data [0] = val_delay_time/2.0;
int valdelay_time = write(_ID, AX12_REG_DELAY_TIME, 1, data);
return (valdelay_time );
}
// Reglage de la température max du cervo
// minimum >> Ox00 >> decimal 0
// maximum >> 0x96 >> decimal 150
int AX12::Set_Temperature_Max (int val_temperature )
{
char data[1];
data [0] = val_temperature;
Page 96 sur 110
int valtemp_max = write(_ID, AX12_REG_TEMP_MAX, 1, data);
return (valtemp_max );
}
// Etat LED
// 0 = off
// 1 = on
int AX12::Set_Etat_LED(int etat)
{
char data[1];
data [0] = etat;
int valLED = write(_ID, AX12_REG_LED, 1, data);
return (valLED);
}
// Set the mode of the servo
// 0 = Positional (0-300 degrees)
// 1 = Rotational -1 to 1 speed
int AX12::Set_Mode(int mode)
{
if (mode == 1) { // set CR
//wait(0.001);
Set_CW_Angle_Limit(0);
//wait(0.001);
Set_CCW_Angle_Limit(0);
//wait(0.001);
Set_CR_Speed(0.0);
//wait(0.001);
} else {
//wait(0.001);
Set_CW_Angle_Limit(0);
//wait(0.001);
Set_CCW_Angle_Limit(300);
//wait(0.001);
Set_CR_Speed(0.0);
//wait(0.001);
}
return(0);
}
// if flag[0] is set, were blocking
// if flag[1] is set, we're registering
// they are mutually exclusive operations
int AX12::Set_Goal(int degrees, int flags)
{
char reg_flag = 0;
char data[2];
// set the flag is only the register bit is set in the flag
if (flags == 0x2) {
reg_flag = 1;
}
// 1023 / 300 * degrees
short goal = (1023 * degrees) / 300;
#ifdef AX12_DEBUG
printf("SetGoal to 0x%x\n",goal);
#endif
data[0] = goal & 0xff; // bottom 8 bits
data[1] = goal >> 8; // top 8 bits
// write the packet, return the error code
Page 97 sur 110
int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
/*if (flags == 1) {
// block until it comes to a halt
while (isMoving())
{
}
}*/
return(rVal);
}
// Set continuous rotation speed from -1 to 1
int AX12::Set_CR_Speed(float speed)
{
// bit 10 = direction, 0 = CCW, 1=CW
// bits 9-0 = Speed
char data[2];
int goal = (0x3ff * abs(speed));
// Set direction CW if we have a negative speed
if (speed < 0) {
goal |= (0x1 << 10);
}
data[0] = goal & 0xff; // bottom 8 bits
data[1] = goal >> 8; // top 8 bits
// write the packet, return the error code
int rVal = write(_ID, 0x20, 2, data);
return(rVal);
}
int AX12::Set_CW_Angle_Limit (int degrees)
{
char data[2];
// 1023 / 300 * degrees
short limit = (1023 * degrees) / 300;
#ifdef AX12_DEBUG
printf("SetCWLimit to 0x%x\n",limit);
#endif
data[0] = limit & 0xff; // bottom 8 bits
data[1] = limit >> 8; // top 8 bits
// write the packet, return the error code
return (write(_ID, AX12_REG_CW_LIMIT, 2, data));
}
int AX12::Set_CCW_Angle_Limit (int degrees)
{
char data[2];
// 1023 / 300 * degrees
short limit = (1023 * degrees) / 300;
#ifdef AX12_DEBUG
Page 98 sur 110
printf("SetCCWLimit to 0x%x\n",limit);
#endif
data[0] = limit & 0xff; // bottom 8 bits
data[1] = limit >> 8; // top 8 bits
// write the packet, return the error code
return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
}
int AX12::Set_ID (int CurrentID, int NewID)
{
char data[1];
data[0] = NewID;
#ifdef AX12_DEBUG
printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
#endif
return (write(CurrentID, AX12_REG_ID, 1, data));
}
int AX12::Set_Baud (int baud)
{
char data[1];
data[0] = baud;
#ifdef AX12_DEBUG
printf("Setting Baud rate to %d\n",baud);
#endif
return (write(_ID, AX12_REG_BAUD, 1, data));
}
// return 1 is the servo is still in flight
int AX12::isMoving(void)
{
char data[1];
read(_ID,AX12_REG_MOVING,1,data);
return(data[0]);
}
void AX12::trigger(void)
{
char TxBuf[16];
char sum = 0;
#ifdef AX12_TRIGGER_DEBUG
// Build the TxPacket first in RAM, then we'll send in one go
printf("\nTriggered\n");
printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
#endif
TxBuf[0] = 0xFF;
TxBuf[1] = 0xFF;
Page 99 sur 110
// ID - Broadcast
TxBuf[2] = 0xFE;
sum += TxBuf[2];
#ifdef AX12_TRIGGER_DEBUG
printf(" ID : %d\n",TxBuf[2]);
#endif
// Length
TxBuf[3] = 0x02;
sum += TxBuf[3];
#ifdef AX12_TRIGGER_DEBUG
printf(" Length %d\n",TxBuf[3]);
#endif
// Instruction - ACTION
TxBuf[4] = 0x04;
sum += TxBuf[4];
#ifdef AX12_TRIGGER_DEBUG
printf(" Instruction 0x%X\n",TxBuf[5]);
#endif
// Checksum
TxBuf[5] = 0xFF - sum;
#ifdef AX12_TRIGGER_DEBUG
printf(" Checksum 0x%X\n",TxBuf[5]);
#endif
// Transmit the packet in one burst with no pausing
for (int i = 0; i < 6 ; i++) {
_ax12.putc(TxBuf[i]);
}
// This is a broadcast packet, so there will be no reply
return;
}
float AX12::Get_Position(void)
{
#ifdef AX12_DEBUG
printf("\nGetPosition(%d)",_ID);
#endif
char data[2];
int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
int position = data[0] | (data[1] << 8);
float angle = ((float)position * 300.0)/1023.0;
return (angle);
}
float AX12::Get_Temp (void)
{
#ifdef AX12_DEBUG
printf("\nGetTemp(%d)",_ID);
#endif
char data[1];
int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
float temp = data[0];
Page 100 sur 110
return(temp);
}
float AX12::Get_Volts (void)
{
#ifdef AX12_DEBUG
printf("\nGetVolts(%d)",_ID);
#endif
char data[1];
int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
float volts = data[0]/10.0;
return(volts);
}
int AX12::read(int ID, int start, int bytes, char* data) {
char PacketLength = 0x4;
char TxBuf[16];
char sum = 0;
char Status[16];
int timeout = 0;
int plen = 0;
int flag_out = 0;
int timeout_transmit = 0;
int i = 0;
int enable = 0;
int poubelle = 0;
int count = 0;
char vidage[50];
typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat;
type_etat etat = Header1;
Status[4] = 0xFE; // return code
/*********************************** CREATION DE LA TRAME A EVOYER
*****************************************/
#ifdef AX12_READ_DEBUG
printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
#endif
// Build the TxPacket first in RAM, then we'll send in one go
#ifdef AX12_READ_DEBUG
printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
#endif
TxBuf[0] = 0xff;
TxBuf[1] = 0xff;
// ID
TxBuf[2] = ID;
sum += TxBuf[2];
#ifdef AX12_READ_DEBUG
printf(" ID : %d\n",TxBuf[2]);
#endif
Page 101 sur 110
// Packet Length
TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
sum += TxBuf[3]; // Accululate the packet sum
#ifdef AX12_READ_DEBUG
printf(" Length : 0x%x\n",TxBuf[3]);
#endif
// Instruction - Read
TxBuf[4] = 0x2;
sum += TxBuf[4];
#ifdef AX12_READ_DEBUG
printf(" Instruction : 0x%x\n",TxBuf[4]);
#endif
// Start Address
TxBuf[5] = start;
sum += TxBuf[5];
#ifdef AX12_READ_DEBUG
printf(" Start Address : 0x%x\n",TxBuf[5]);
#endif
// Bytes to read
TxBuf[6] = bytes;
sum += TxBuf[6];
#ifdef AX12_READ_DEBUG
printf(" No bytes : 0x%x\n",TxBuf[6]);
#endif
// Checksum
TxBuf[7] = 0xFF - sum;
#ifdef AX12_READ_DEBUG
printf(" Checksum : 0x%x\n",TxBuf[7]);
#endif
/********************************************TRAME CONSTRUITE DANS
TxBuf***************************************/
/* Transmission de la trame construite precedemment dans le tableau TxBuf
*/
while ((timeout_transmit<100) && (i < (7+bytes)))
{
if (_ax12.writeable())
{
_ax12.putc(TxBuf[i]);
i++;
timeout_transmit = 0;
}
else timeout_transmit++;
}
if (timeout_transmit == 100 ) // dans le cas d'une sortie en timeout pour ne pas rester
bloquer !
{
printf ("timeout transmit erreur\r\n");
return(-1);
}
/* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le ser-
vomoteur
*/
Page 102 sur 110
// Wait for the bytes to be transmitted
wait (0.001);
// Skip if the read was to the broadcast address
if (_ID != 0xFE) {
/* Partie de reception de la trame de retour
*/
while ((flag_out != 1) && (timeout < (1000*bytes)))
{
// Les differents etats de l'automate on été créés au debut de la fonction write !
switch (etat)
{
case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF )
{
Status[plen] = _ax12.getc();
timeout = 0;
if (Status[plen] == 0xFF )
{
etat = Header2;
#ifdef AX12_DEBUG_READ
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
}
else etat = Header1;
}
else timeout++;
break;
case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF )
{
Status[plen] = _ax12.getc();
timeout = 0;
if (Status[plen] == 0xFF )
{
etat = ident;
#ifdef AX12_DEBUG_READ
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
}
else if (Status[plen] == ID ) // PERMET D'EVITER CERTAINES ERREUR
LORSQU'ON LIT PLUSIEURS REGISTRES !!!!
{
Status[plen] = 0;
#ifdef AX12_DEBUG_READ
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
Status[plen] = ID;
etat = length;
#ifdef AX12_DEBUG_READ
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
}
else
Page 103 sur 110
{
etat = Header1;
plen = 0;
}
}
else timeout++;
break;
case ident: if (_ax12.readable()) // reception de l'octet correspondant à
l'ID du servomoteur
{
Status[plen] = _ax12.getc();
timeout = 0;
if (Status[plen] == ID )
{
etat = length;
#ifdef AX12_DEBUG_READ
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
}
else
{
etat = Header1;
plen = 0;
}
}
else timeout++;
break;
case length: if (_ax12.readable()) // reception de l'octet correspondant à
la taille ( taille = 2 + nombre de paramètres )
{
Status[plen] = _ax12.getc();
timeout = 0;
if (Status[plen] == (bytes+2) )
{
etat = erreur;
#ifdef AX12_DEBUG_READ
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
}
else
{
etat = Header1;
plen = 0;
}
}
else timeout++;
break;
case erreur: if (_ax12.readable()) //reception de l'octet correspondant au
code d'erreurs eventuels ( 0 = pas d'erreur )
{
Status[plen] = _ax12.getc();
timeout = 0;
#ifdef AX12_DEBUG_READ
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
etat = reception;
}
Page 104 sur 110
else timeout++;
case reception: while ( enable < bytes ) // reception du ou des octect(s) de
donnés ( suivant la valeur de la variable length )
{
if (_ax12.readable())
{
Status[plen] = _ax12.getc();
timeout = 0;
#ifdef AX12_DEBUG_READ
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
enable++;
}
else timeout++;
}
etat = checksum;
break;
case checksum: if (_ax12.readable()) // reception du dernier octet ( Checksum ) >>>
checksum = NOT ( ID + length + somme des données ) >>>> dans le cas d'un retour d'un read!!
{
Status[plen] = _ax12.getc();
timeout = 0;
flag_out = 1;
etat = Header1;
#ifdef AX12_DEBUG_READ
printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]);
#endif
}
else timeout++;
break;
}
}
if (timeout == (1000*bytes) ) // permet d'afficher si il y a une erreur de timeout et de
ne pas rester bloquer si il y a des erreurs de trames
{
printf ("timeout erreur\n");
return(-1);
}
// copie des données dans le tableau data
for (int i=0; i < Status[3]-2 ; i++)
{
data[i] = Status[5+i];
}
} // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast
ID (ID!=0xFE)
return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
}
int AX12::write(int ID, int start, int bytes, char* data, int flag)
{
// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
char TxBuf[16];
char sum = 0;
char Status[6];
Page 105 sur 110
int timeout = 0;
int plen = 0;
int flag_out = 0;
int timeout_transmit = 0;
int i = 0;
int poubelle = 0;
int count = 0;
char vidage[50];
typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
type_etat etat = Header1;
#ifdef AX12_WRITE_DEBUG
printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
#endif
// Build the TxPacket first in RAM, then we'll send in one go
#ifdef AX12_WRITE_DEBUG
printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
#endif
TxBuf[0] = 0xff;
TxBuf[1] = 0xff;
// ID
TxBuf[2] = ID;
sum += TxBuf[2];
#ifdef AX12_WRITE_DEBUG
printf(" ID : %d\n",TxBuf[2]);
#endif
// packet Length
TxBuf[3] = 3+bytes;
sum += TxBuf[3];
#ifdef AX12_WRITE_DEBUG
printf(" Length : %d\n",TxBuf[3]);
#endif
// Instruction
if (flag == 1) {
TxBuf[4]=0x04;
sum += TxBuf[4];
} else {
TxBuf[4]=0x03;
sum += TxBuf[4];
}
#ifdef AX12_WRITE_DEBUG
printf(" Instruction : 0x%x\n",TxBuf[4]);
#endif
// Start Address
TxBuf[5] = start;
sum += TxBuf[5];
#ifdef AX12_WRITE_DEBUG
printf(" Start : 0x%x\n",TxBuf[5]);
#endif
// data
for (char i=0; i<bytes ; i++) {
TxBuf[6+i] = data[i];
sum += TxBuf[6+i];
#ifdef AX12_WRITE_DEBUG
Page 106 sur 110
printf(" Data : 0x%x\n",TxBuf[6+i]);
#endif
}
// checksum
TxBuf[6+bytes] = 0xFF - sum;
#ifdef AX12_WRITE_DEBUG
printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
#endif
/* Transmission de la trame construite precedemment dans le tableau TxBuf
*/
while ((timeout_transmit<100) && (i < (7+bytes)))
{
if (_ax12.writeable())
{
_ax12.putc(TxBuf[i]);
i++;
timeout_transmit = 0;
}
else timeout_transmit++;
}
if (timeout_transmit == 100 ) // dans le cas d'une sortie en timeout pour ne pas rester
bloquer !
{
printf ("TIMEOUT TRANSMIT ERROR\r\n");
return(-1);
}
/* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le ser-
vomoteur
*/
// Wait for data to transmit
wait (0.005);
// make sure we have a valid return
Status[4]=0x00;
// we'll only get a reply if it was not broadcast
if (_ID!=0xFE) {
/* Partie de reception de la trame de retour
*/
while ((flag_out != 1) && (timeout < 1000))
{
// Les differents etats de l'automate on été créés au debut de la fonction write !
switch (etat)
{
case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF )
{
Status[plen] = _ax12.getc();
timeout = 0;
if (Status[plen] == 0xFF )
{
etat = Header2;
#ifdef AX12_DEBUG_WRITE
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
}
Page 107 sur 110
else etat = Header1;
}
else timeout++;
break;
case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF )
{
Status[plen] = _ax12.getc();
timeout = 0;
if (Status[plen] == 0xFF )
{
etat = ident;
#ifdef AX12_DEBUG_WRITE
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
}
else
{
etat = Header1;
plen = 0;
}
}
else timeout++;
break;
case ident: if (_ax12.readable()) // reception de l'octet
correspondant à l'ID du servomoteur
{
Status[plen] = _ax12.getc();
timeout = 0;
if (Status[plen] == ID )
{
etat = length;
#ifdef AX12_DEBUG_WRITE
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
}
else
{
etat = Header1;
plen = 0;
}
}
else timeout++;
break;
case length: if (_ax12.readable()) // reception de l'octet correspondant à la
taille ( taille = 2 + nombre de paramètres )
{
Status[plen] = _ax12.getc();
timeout = 0;
if (Status[plen] == 2 ) // dans la trame de retour d'un write il
n'y a pas de paramètre la taille vaudra donc 2!!
{
etat = erreur;
#ifdef AX12_DEBUG_WRITE
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
}
else
{
etat = Header1;
plen = 0;
Page 108 sur 110
}
}
else timeout++;
break;
case erreur: if (_ax12.readable()) //reception de l'octet correspondant
au code d'erreurs eventuels ( 0 = pas d'erreur )
{
Status[plen] = _ax12.getc();
timeout = 0;
#ifdef AX12_DEBUG_WRITE
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
#endif
plen++;
etat = checksum;
}
else timeout++;
case checksum: if (_ax12.readable()) // recpetion du dernier octet (
Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write
{
Status[plen] = _ax12.getc();
timeout = 0;
flag_out = 1;
etat = Header1;
#ifdef AX12_DEBUG_WRITE
printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]);
#endif
}
else timeout++;
break;
}
}
if ( Status[4] != 0 )
{
printf ("erreur ! \r\n");
for (int i = 0; i<5; i++)
{
printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
}
}
if (timeout == 1000 ) // permet d'afficher si il y a une erreur de timeout et de ne pas
rester bloquer si il y a des erreurs de trames
{
printf ("timeout erreur\n\r");
return(-1);
}
// Build the TxPacket first in RAM, then we'll send in one go
}
return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
}
Programme C de la carte AX12
#include "mbed.h"
#include "AX12.h"
#define ID_AX12_MOTOR 6
#define ID_AX12_LASER 16
#define VIT_COMM_AX12 1000000 // mettre 1000000 pour envoyer des ordres à 1 Mb/s
CAN can_robot (p30, p29);
Page 109 sur 110
CAN can_test (p9, p10);
CANMessage message;
typedef enum {laser, moteur, attente} type_etat;
type_etat etat = attente;
void interupt_can (void);
DigitalIn txAx12(p27);
int main() {
long erreur = 0;
long count = 0;
Serial pc(USBTX, USBRX); // tx, rx
pc.baud(9600);
pc.printf ("open\n\r");
can_robot.frequency ( 1000000);
can_robot.attach(interupt_can);
AX12 myax12_motor (p28, p27, ID_AX12_MOTOR, VIT_COMM_AX12);
myax12_motor.Set_Mode(0x00); // (0x00) pour mode position de l'AX12 *** (0x01) pour
rotation continue
myax12_motor.Set_Baud(0x01); // réglage de la vitesse de reception du servo moteur
(0x01 =>> 1 Mb/s)
AX12 myax12_laser (p28, p27, ID_AX12_LASER, VIT_COMM_AX12);
myax12_laser.Set_Mode(0x00);
myax12_laser.Set_Baud(0x01);
txAx12.mode(PullUp);
erreur = myax12_motor.Set_Secure_Goal(150);
while (1) {
switch (etat)
{
case attente: break;
case laser: erreur = myax12_laser.Set_Goal(140);
wait(0.2);
erreur = myax12_laser.Set_Goal(160);
wait(0.2);
break;
case moteur: erreur = myax12_motor.Set_Goal(60);
wait(0.2);
etat = attente;
break;
}
}
}
void interupt_can (void)
{
if (can_robot.read(message))
Page 110 sur 110
{
//if ( message.id != 0x52 )
//printf ("ID : %x\r\n", message.id );
switch (message.id)
{
case 0x80: if (message.data[0] == 0xFF)
etat = laser;
if ( message.data[0] == 0x00 )
etat = attente;
break;
case 0x30: if (message.data[0] == 0xFF)
etat = moteur;
if ( message.data[0] == 0x00 )
etat = attente;
break;
}
}
}
Recommended