coupe de robotique des iut à vierzon - gesi.asso.fr · page 4 sur 110 résumé la coupe de...

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

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

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.

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.


Mots-clés

IUT (Institut Universitaire de Technologie), coupe de Vierzon, asservissement, calcul de trajectoire,

capteurs, balise infrarouge, servomoteurs AX12, laser.


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é ».


So

urc

e :

règ

lem

ent

du

co

nco

urs

de

rob

oti

qu

e d

es I

UT

.

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.


Figure 1. Châssis du robot Figure 2. Schéma des parcours prédéfinis

sur 110

Figure 3. Schéma du système des cartes électroniques

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

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


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

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

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.


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.

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

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))

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.


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.

sur 110


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.


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.

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

sur 110

Figure 18 : Schéma de câblage du CNY70


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.

sur 110


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.

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.

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.


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.


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.


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.

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

sur 110


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

sur 110


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).


4 Le 7805 et le traco sont des régulateurs de tension qui convertissent une tension plus élevée en 5V stable et

continu

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.


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

sur 110


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.


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

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.


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;

}

sur 110


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.


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

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


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

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

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

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.

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

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

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

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.

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

sur 110

Le diagramme d''état de la main est le suivant :

Figure 41. Diagramme d’état de la main robotisée


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.


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

sur 110

Schéma de connexions des codeurs incrémentaux

sur 110

PCB de la carte capteur

sur 110

sur 110

PCB de la carte d’émission infrarouge

sur 110

sur 110

PCB de la carte de réception infrarouge

sur 110

sur 110

PCB de la carte AX12

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

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;

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);


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.

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;


{

alpha_Gauche = -1.0; //On le limite Ã -1

}


{

alpha_Gauche = 1.0; //On le plafonne Ã 1

}


{

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,


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

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

{


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

}


Codeur_Gauche = QuadDec_Gauche_GetCounter(); //Lecture dela valeur du codeir


alpha_Gauche = kp_Gauche * erreur_Gauche; //Calcul du futur rapport cyclique


{

alpha_Gauche = -1.0; //On le limite Ã -1

}


{


}


{



vers l'avant

}


{



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

}

}

sur 110

else if (etat_Gauche == 2) //TroisiÃ¨me partie de la machine Ã Ã©tat

{


cons_pos_Gauche += vitesse_Gauche; //Augmentation de la consigne de position




alpha_Gauche = kp_Gauche * erreur_Gauche; //Calcul du futur rapport cyclique


{

alpha_Gauche=-1.0; //On le limite Ã -1

}


{


}


{



vers l'avant

}


{



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

{


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

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

}




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


{

alpha_Gauche=-1.0; //On le limite Ã -1

}


{

alpha_Gauche = 1.0; //On le limite Ã 1

}


{



vers l'avant

}


{



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;



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

{


etat_Gauche = (4);

}

}

Codeur_Gauche_Can = Codeur_Gauche_1 + Codeur_Gauche;


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

}


{

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)

{


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;

sur 110

ki_Droite=0.000000001;

coeff_rayon = (double)cons_finale_Droite/((double)cons_finale_Gauche);

}


{


vitesse_Droite = coeff_rayon*vitesse_Gauche;

if (cons_finale_Droite >= 0)

{

cons_pos_Droite += vitesse_Droite;

}

else

{

cons_pos_Droite -= vitesse_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);


}

else

{

PWM_1_WriteCompare2(-alpha_Droite*1199);


}

if (etat_Gauche == 2)

{

if (cons_finale_Droite >= 0)

{

vitesse_Droite = vmax_Droite;

}

else

{

vitesse_Droite = -vmax_Droite;

}

etat_Droite=2;

}

}


{






alpha_Droite = kp_Droite * erreur_Droite;


{

sur 110

alpha_Droite=-1.0;

}


{

alpha_Droite = 1.0;

}

if (alpha_Droite>0)

{



}

else

{



}

if (fabs(cons_pos_Droite) >= fabs(pos_vit_D))

{

etat_Droite=3;

}

}


{


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;

}

}


if ((fabs(cons_pos_Droite)) >= (fabs(cons_finale_Droite)))

{

cons_pos_Droite = cons_finale_Droite;

etat_Droite = 4;

}




somme_Droite += erreur_Droite;

alpha_Droite = kp_Droite * erreur_Droite + ki_Droite * somme_Droite;


{

alpha_Droite=-1.0;

}


{

alpha_Droite = 1.0;

}

if (alpha_Droite>0)

{



}

else

sur 110

{



}

}


{

if (ack_dec==0xFF)

{

//BRAKE_Droite_Write(1);


vitesse_Droite=0;

}


//cons_finale_Droite = 0;

//cons_finale_Droite_Can = 0;



if ((etat_Gauche == 4)||(etat_Gauche == (-1)))

{

// can_SendMsgcan_t_consigne_finie();


Codeur_Droite_1 += Codeur_Droite;

//Codeur_Droite = 0;

//anc_codeur_Droite=0;

//QuadDec_Droite_SetCounter(0);

etat_Droite = (-1);

}

else

{


etat_Droite = (4);

}

}

Codeur_Droite_Can = Codeur_Droite_1 + Codeur_Droite;


}

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 -


}

else if(teta_rob<=270)

{

x= x - (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -


sur 110

y= y - (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -


}

else if(teta_rob<360)

{

x= x + (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -


y= y - (((Codeur_Droite - anc_codeur_Droite) + (Codeur_Gauche -


}

}

/* [] END OF FILE */

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,

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);

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)

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;


PIT_wait(50*MS);

etat=0;

en-

voie_can(msg_avance,msg_avance,msg_vit_recul_avance,msg_pos_vit_avan

ce,message_dec);

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;


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;


PIT_wait(50*MS);

etat=0;

en-

voie_can(msg_tourg2,msg_tourd2,msg_vit_recul_avance,msg_pos_vit_tour

2,message_dec);

sur 110

while(etat==0)

{

fct_rcpt();

}

etat=0;


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 */

/* */

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;


(*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;

sur 110

*dist_g= corde;


(*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


{

index++;

}


*teta_rob=180-(index-1);

else

*teta_rob=180-index;

sur 110

}

else if(x1<xo && y1<yo)

{

quotient= ((yo-y1)*1000)/(xo-x1); //symétrie


{

index++;

}


*teta_rob=180+(index-1);

else

*teta_rob=180+index;

}

else if(x1>xo && y1<yo)

{

quotient= ((yo-y1)*1000)/(x1-xo); //symétrie


{

index++;

}


*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]);


*distance= 100*(100*(y1-yo)/tab_sin[180-*teta_rob]);


*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;

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;

}

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;

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





*/

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);

}

///////***************************************************************************************

******///////

sur 110

if( (cap_court_gauche < SEUIL_COURT) && detection_court_gauche ) etat= DE-

TECTION_DANGER_GAUCHE;

else {

detection_court_gauche=1;


}

///////***************************************************************************************

******///////

if( (cap_court_droite < SEUIL_COURT) && detection_court_droite ) etat= DE-

TECTION_DANGER_DROITE;

else {

detection_court_droite=1;


}

///////***************************************************************************************

******///////

if( (cap_court_extrem_gauche < SEUIL_COURT) && detec-

tion_court_extrem_gauche ) etat= DETECTION_DANGER_EXTREM_GAUCHE;

else {

detection_court_extrem_gauche=1;


}

///////***************************************************************************************

******///////

if( (cap_court_extrem_droite < SEUIL_COURT) && detec-

tion_court_extrem_droite ) etat= DETECTION_DANGER_EXTREM_DROITE;

else {

detection_court_extrem_droite=1;


}

///////***************************************************************************************

******///////

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);


sur 110


}

etat=WAIT;

break;

///////***************************************************************************************

******///////

case DETECTION_DANGER_CENTRE:

CAN_send_message(0x62,1,&cap_court_centre);


etat=WAIT;

break;

///////***************************************************************************************

******///////

case DETECTION_DANGER_GAUCHE:



etat=WAIT;

break;

///////***************************************************************************************

******///////

case DETECTION_DANGER_DROITE:



etat=WAIT;

break;

///////***************************************************************************************

******///////

case DETECTION_DANGER_EXTREM_GAUCHE:



etat=WAIT;

break;

///////***************************************************************************************

******///////

case DETECTION_DANGER_EXTREM_DROITE:



etat=WAIT;

break;

///////***************************************************************************************

******///////

case BOUTON_POISSOIR:

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 ){



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);



}

//**

sur 110

}

else {

if(passage_zero_long==0)

{



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)

{



}

}

else

{

PTP=PTP&0xFB;


if(passage_zero_court_centre==0)

{


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);


}

}

else

{

PTP=PTP&0xF7;


sur 110

if(passage_zero_court_gauche==0)

{


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);


}

}

else

{

PTP=PTP&0xEF;


if(passage_zero_court_droite==0)

{


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);


}

}

else

{

PTP=PTP&0xDF;


if(passage_zero_court_extrem_gauche==0)

{


passage_zero_court_extrem_gauche=1;

}

}

////////// Court extrem droite ///////////

if(cap_court_extrem_droite < SEUIL_COURT)

{

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);


}

}

else

{

PTP=PTP&0x7F;


if(passage_zero_court_extrem_droite==0)

{


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

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;

}

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

}

//*********************************************************

sur 110

Programme C de la carte d’émission infrarouge

#include <hidef.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;

}

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

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;

}

sur 110

Programme C de la carte de réception infrarouge

#include <hidef.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();

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;

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;


PORTA = 5;


PORTA = 2;


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)

{

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


}

//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);


}

//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)

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

}

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);

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);

}

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);

sur 110

return(torque);

}

/** Reglage du couple maximum de l'actionneur


// 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


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);

}

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


// 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


// 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


// maximum >> 0x96 >> decimal 150

int AX12::Set_Temperature_Max (int val_temperature )

{

char data[1];

data [0] = val_temperature;

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


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


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


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

sur 110

printf("SetCCWLimit to 0x%x\n",limit);

#endif

data[0] = limit & 0xff; // bottom 8 bits

data[1] = limit >> 8; // top 8 bits


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;

sur 110

// ID - Broadcast

TxBuf[2] = 0xFE;

sum += TxBuf[2];


printf(" ID : %d\n",TxBuf[2]);

#endif

// Length

TxBuf[3] = 0x02;

sum += TxBuf[3];


printf(" Length %d\n",TxBuf[3]);

#endif

// Instruction - ACTION

TxBuf[4] = 0x04;

sum += TxBuf[4];


printf(" Instruction 0x%X\n",TxBuf[5]);

#endif

// Checksum

TxBuf[5] = 0xFF - sum;


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];

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



printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");

#endif

TxBuf[0] = 0xff;

TxBuf[1] = 0xff;

// ID

TxBuf[2] = ID;

sum += TxBuf[2];



#endif

sur 110

// Packet Length

TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)

sum += TxBuf[3]; // Accululate the packet sum


printf(" Length : 0x%x\n",TxBuf[3]);

#endif

// Instruction - Read

TxBuf[4] = 0x2;

sum += TxBuf[4];


printf(" Instruction : 0x%x\n",TxBuf[4]);

#endif

// Start Address

TxBuf[5] = start;

sum += TxBuf[5];


printf(" Start Address : 0x%x\n",TxBuf[5]);

#endif

// Bytes to read

TxBuf[6] = bytes;

sum += TxBuf[6];


printf(" No bytes : 0x%x\n",TxBuf[6]);

#endif

// Checksum

TxBuf[7] = 0xFF - sum;


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())

{


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

*/

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 )

{


timeout = 0;


{

etat = ident;



#endif

plen++;

}

else if (Status[plen] == ID ) // PERMET D'EVITER CERTAINES ERREUR

LORSQU'ON LIT PLUSIEURS REGISTRES !!!!

{

Status[plen] = 0;



#endif

plen++;

Status[plen] = ID;

etat = length;



#endif

plen++;

}

else

sur 110

{

etat = Header1;

plen = 0;

}

}

else timeout++;

break;

case ident: if (_ax12.readable()) // reception de l'octet correspondant à

l'ID du servomoteur

{


timeout = 0;

if (Status[plen] == ID )

{

etat = length;



#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 )

{


timeout = 0;

if (Status[plen] == (bytes+2) )

{

etat = erreur;



#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 )

{


timeout = 0;



#endif

plen++;

etat = reception;

}

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())

{


timeout = 0;



#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!!

{


timeout = 0;

flag_out = 1;

etat = Header1;


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];

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



printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");

#endif

TxBuf[0] = 0xff;

TxBuf[1] = 0xff;

// ID

TxBuf[2] = ID;

sum += TxBuf[2];



#endif

// packet Length

TxBuf[3] = 3+bytes;

sum += TxBuf[3];


printf(" Length : %d\n",TxBuf[3]);

#endif

// Instruction

if (flag == 1) {

TxBuf[4]=0x04;

sum += TxBuf[4];

} else {

TxBuf[4]=0x03;

sum += TxBuf[4];

}


printf(" Instruction : 0x%x\n",TxBuf[4]);

#endif

// Start Address

TxBuf[5] = start;

sum += TxBuf[5];


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];


sur 110

printf(" Data : 0x%x\n",TxBuf[6+i]);

#endif

}

// checksum

TxBuf[6+bytes] = 0xFF - sum;


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())

{


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 )

{


timeout = 0;


{

etat = Header2;

#ifdef AX12_DEBUG_WRITE


#endif

plen++;

}

sur 110

else etat = Header1;

}

else timeout++;

break;

case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF )

{


timeout = 0;


{

etat = ident;



#endif

plen++;

}

else

{

etat = Header1;

plen = 0;

}

}

else timeout++;

break;

case ident: if (_ax12.readable()) // reception de l'octet

correspondant à l'ID du servomoteur

{


timeout = 0;

if (Status[plen] == ID )

{

etat = length;



#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 )

{


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;



#endif

plen++;

}

else

{

etat = Header1;

plen = 0;

sur 110

}

}

else timeout++;

break;

case erreur: if (_ax12.readable()) //reception de l'octet correspondant

au code d'erreurs eventuels ( 0 = pas d'erreur )

{


timeout = 0;



#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

{


timeout = 0;

flag_out = 1;

etat = Header1;


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++)

{


}

}

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);

}


}

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);

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))

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;

}

}

}

coupe de robotique des iut à vierzon - gesi.asso.fr · page 4 sur 110 résumé la coupe de...

Documents