Hexapode

HEXAPODES. La meilleure façon de marcher ne coule pas de source lorsque l'on a 6 pattes.
C'est pourtant le point commun entre les insectes et certains robots hexapodes, qui imitent la locomotion spécifique aux arthropodes du règne vivant, à savoir une marche asymétrique "en trépied", qui comprend à tout moment deux point d'appui d'un côté et un troisième support de l'autre.
Mais ce ne serait finalement pas le mode de locomotion le plus efficace... tout du moins dans un environnement en 2D.
C'est ce que révèle une étude de l'université de Lausanne et de l'École Polytechnique Fédérale de Lausanne (EPFL), à paraître dans Nature Communications.
Avec une méthodologie expérimentale qui recourt à des algorithmes évolutionnistes... mais aussi à de minuscules bottines pour limiter l'adhérence des pattes de drosophiles !
Il vas falloir faire très attention à l'adhérence au sol. Soit un groupe de trois est un trépied formé par les pattes avant et arrière d'une part, et la branche centrale du côté opposé.
Les trois branches constitutives de chaque trépied sont déplacées comme une unité.
Comme un trépied est levé, l'autre trépied pousse vers l'avant.
Dans cette démarche, il peut être utile de penser à chaque trépied comme un pied et le comparer à votre propre marche bipède où, comme un pied est levé l'autre pied pousse en avant.
A partir de 3 servomoteurs 90SG.
Avance de l'hexapode | ||||
---|---|---|---|---|
Milieu | gauche | droite | En images | |
0 | 120 | 120 | 60 | |
1 | 95+t | 80 | 30 | |
2 | 145+t | 150 | 90 | |
1 | 95+t | 80 | 30 | |
2 | 145+t | 150 | 90 | |
1 | 95+t | 80 | 30 | |
0 | 120 | 120 | 60 | |
Rotation de l'hexapode | ||||
Milieu | gauche | droite | En images | |
0 | 120 | 120 | 60 | |
1 | 95+t | 150 | 30 | |
2 | 145+t | 80 | 90 | |
1 | 95+t | 150 | 30 | |
2 | 145+t | 80 | 90 | |
1 | 95+t | 150 | 30 | |
0 | 120 | 120 | 60 |
+t avec tempo avant mouvement des bras d'environs 100ms
#include "Servo.h" Servo lift; Servo left; Servo right; //Variables int diff_ldr=0;//difference in ldr reading int dist_read=100;//distance reading void setup() { //Attach Servos lift.attach(10); left.attach(9); right.attach(11); //Servo Center positions startPos(); delay(2000); } void loop() { if(diff_ldr<200)//Check if being petted { if(dist_read>30) {forward();} else { turn(); //delay(1000); //délai pour mise en position } } else {petMode();} } void forward() { firstHalf(); delay(500); secondHalf(); delay(500); } void turn() { firstTurn(); delay(200); secondTurn(); delay(200); } void petMode() { startPos(); happyShake(); } void firstHalf() { lift.write(95);//right up delay(100); left.write(80);//left bk right.write(30);//right fwd } void secondHalf() { lift.write(145);//left up delay(100); left.write(150);//left fwd right.write(90);//right bk } void autoShut() { lift.detach(); left.detach(); right.detach(); } void firstTurn() { lift.write(95);//right up delay(100); left.write(150);//fwd right.write(30);//fwd } void secondTurn() { lift.write(145);//left up delay(100); left.write(80);//bk right.write(90);//bk } void startPos() { lift.write(120); left.write(120); right.write(56); } void HappyShake() { lift.write(60);//right up delay(200); lift.write(180);//left up delay(200); }
Evolution vers la "simplicité"
|
Le moteur gauche et droit font mouvoir deux pieds, chaque pied avant est relier à un pied arrière grâce à une biellette. Le moteur central quant à lui, permet de lever et baisser les pieds gauche et droite. La carte "Otto tiny" de ma composition est sur la partie inférieure et la batterie 3.7V est montée sur la partie supérieure. |
Le programme du robot Otto permet l'utilisation de 5 servos moteurs simultanément et de plus la platine électronique et parfaite pour l'hexapode (compact et efficace).
Comme je n'ai que 3 servos-moteurs, il me reste 3 entrées/sorties sur l'attiny.
Chaque moteur à une rotation de ±30°, ce qui simplifie aussi les opérations.
La vitesse de déplacement sera connue lorsque l'ensemble sera monté.
Les paramètres du programme :
Le paramètre dans regC est le nombre de mouvements.
Dans le programme, la valeur angulaire du servomoteur vas de 0 à 180° ce qui représente une valeur entre 0 et 127 dans le programme.
Le dessin ci-dessous vous simplifie la programmation
Le mouvement d'une moteur de 171 à 85 représente 43 positions entre chaqu'une de ces positons, La vitesse maximale d'un sevomoteur SG90 est de 180° soit 127 positions en 300.000μs, le temps minimum entre chaque position est de $\dfrac{300.000}{127}=2362\mu s$
Petite aparté sur la fonction de temps "Wait4xCycles", qui représente comme son nom l'indique 4 cycles d'horloge.
Un 4 cycles d'horloge par microseconde représente $\dfrac{F_{CPU}}{4000000}$.
#define C4PUS (F_CPU/4000000) ;/4 nombre de périodes par us attention aux approximation ;------------------------------------------------------------------------------ ; Input : XH:XL - number of CPU cycles to wait (divided by four) ;------------------------------------------------------------------------------ Wait4xCycles: sbiw XH:XL, 1 ; x-- (2 cycles) brne Wait4xCycles ; jump if not zero (2 cycles) ret ; 4 cycles ;Pour lancer la fonction ldi XH,high(C4PUS*1000) ;1000us soit 1ms ldi XL,low(C4PUS*1000) rcall Wait4xCyclesRevenons à nos 2362μs, temps pour attendre la vitesse maximale entre chaque position.
Je vais donc mettre un facteur de vitesse représenté par un octet, soit une valeur entre 0 et 255.
Pour un valeur de 255 le moteur mettra $2362\times 255=602.310\mu s$ pour changer de position, 180° represente 108.415,800ms soit 108 secondes.
Oups, c'est beaucoup trop!!!
Bon ok, je prends un pas de 0,125, je m'explique 0 représente $\dfrac{2362}{16}=148\mu s$, 1 représente 1,125 soit 2360μs, 2 représente 1,25 soit 2655μs, ...
Je vais pas tous vous les faires, mais vous voyer le principe, plus la valeur est grande plus je ralentis, à 255, 180° se réalise en 5s.
En gros, le calcul pour connaitre la durée entre chaque position en μs $Valeur=(Speed+16)\times 148$
Speed est le premier paramètre.
Pour le codage :
Les paramètres sont dans l'ordre suivant :
- Paramètre de temps entre 0 et 255, je viens tout juste de l'expliquer, c'est Speed, suivez!
- L'angle du moteur central entre 0 et 127 (ne tiens pas compte de la vitesse), vitesse maximale pour lui
- L'angle du moteur gauche entre 0 et 127
- L'angle du moteur droit entre 0 et 127
forward_move: ldi regC,4 ;nombre de mouvement ldi speed,40 ldi ZL,low(forward<<1) ;Z pointe tick ldi ZH,high(forward<<1) ;Z pointe tick rcall NewMove ret ... ... forward: .db 42,78,78 , 42,49,49 ;right up ;left backward, right forward .db 85,49,49 , 85,78,78 ;left up ;left forward, right backward ;60°=42=0x2A 90°=64=0x40 120°=85=0x55 ;70°=49=0x31 110°=78=0x4E
Montage d'un HC-SR04
Le capteur HC-SR04 utilise les ultrasons pour déterminer la distance d'un objet.Il offre une excellente plage de détection sans contact, avec des mesures de haute précision et stables.
Je vais donc choisir les mouvements en fonction des différentes distance d'objets.
Distance | Fonction | Nombre de répetitions |
---|---|---|
0 | Pas de détection (trop loin) Avance rapide | 2 |
≥ 35cm | Avance rapide | 2 |
de ≥25 à <35cm | Avance lente | 2 |
de ≥10 à <25 cm | Tourner à gauche | 2 |
de <10 cm | Recul rapide | 2 |
Les branchements
|
Pour le branchement des servo-moteur SG90, c'est simple S(Signal) fil jaune, + fils rouge, Masse fil marron Avec la batteries de 3700mAh l'autonomie avant recharge est d'environs 3 heures.
Si il y a deux leds à allumer en permanence la résistance fait 50Ω avec 3.7V. (Je sais le branchement n'est pas conventionnel) |
Le programme
Programme sans HC-SR04Programme avec HC-SR04 - 726 octets/8192 (Cool non!)
C'est tout pour aujourd'hui
Hexapode Arduino
D'après une idée de mon ami Fred, pour sa fille Talya, besoin d'un hexapode (Elle a aprèciées La version attiny), mais elle voudrai programmer elle-même, du moins les mouvements.Elle n'a que 10 ans, alors on vas simplifier l'utilisation.

- Objectif :Programmation Hexapode à partir du port serie.
- Moyen:
- Arduino Nano ATmega328P - 20mA
- 3 SG90
- Une batterie Lipo 3.7V 650mAh
Le language
A parti du port série, les commandes à envoyer.A.. Avance suivi de l'angle G.. Gauche suivi de l'angle D.. Droite suivi de l'angle M.. Milieu suivi de l'angle S Mémorisation de la commande qui vient d'être réalisée .
C'est tout pour aujourd'hui
Robot otto
- Objectif :Fabriquer et faire dancer le robot otto
- Moyen:
- Décalage (Offset): Il s'agit du décalage de 0 ° à définir comme centre d'oscillation;
il peut être positif ou négatif.
Notez que tout décalage ajouté réduit l'arc de mouvement de 180° moins la valeur du décalage. - Trim (A ajouter au décalage): Certains servos n’ont pas un niveau de précision et régler le décalage à 0 ° par exemple,
ne laisse pas la patte exactement au centre.
La compensation peut être utilisée pour ajuster cette position par incréments de 1/255ème de tour, positifs ou négatifs. - Amplitude(A): Il s'agit des degrés de mouvement à parcourir par rapport au décalage dans les sens positif et négatif.
Avec un décalage de 0 °, l'amplitude maximale est de 90 °, ce qui donnera un arc d'oscillation complet de 180 ° (-90 ° à + 90 °).
Notez que l'amplitude ne peut être supérieure à 90 °.
Dans ce cas, alors le mouvement atteint la position d'asservissement maximale, il s'arrête, le calcul d'oscillation se poursuit avec d'autres angles non valides envoyés au service jusqu'à ce que l'oscillation max soit atteinte et que le sens du mouvement change.
Ce n’est que lorsque l’angle revient dans la plage, que le servo commencera à revenir vers la position offset. - Période (T): Durée, en millisecondes, d'un cycle d'oscillation complet (0 à 360°).
- Phase (Phase0): C'est la position dans le cycle où le mouvement commence.
Ceci est surtout utile quand il y a deux servos ou plus, oscillant en même temps avec la même amplitude et la même période,
mais qu’un ou plusieurs doivent commencer à des positions différentes dans le cycle s’ils démarrent tous ensemble.
A la base, l'ensemble fonctionne plutôt bien avec un arduino nano, pour simplifier l'ensemble, je vais essayer de le réaliser avec :
Attiny85-20PU | 1,73€ |
4 moteurs SG90S | 9,00€ |
Batterie 852540 600mAh Ni-Po | 1,21€ |
Bouton Commutateur de verrouillage Auto/Off 8x8mm 6Pin | 0,14€ |
CI PCB chine | 1,00€ |
40 Broches 1x40 Simple Rangée Mâle | 0,07€ |
led 10mm Rouge (620-625nm) | 0,18€ |
Support fixation clip montage pour LED 10mm | 0,24€ |
Total | 13.57€ |
---|
Montage de la base
Oscillate
La fonction "oscillate" à pour principe de faire osciller le moteur avec plusieurs paramètres :Oscillation : Continue la rotation du servo d'une extrémité à l'autre autour de l'offset central.
L'angle d'oscillation maximum possible est de 180 ° (dépendant de l'asservissement),
ce qui correspond virtuellement à la cartographie de -90 ° à + 90 °, 0 ° étant le centre de l'oscillation.
Utilisation de la fonction "oscillate"
Le premier paramètre le nombre de pas. Nous prendrons 4 pas.Le second paramètre l'incrément en fonction du temps pour aller de la position initiale à la position finale.
Exemple : 594 ms pour l'aller-retour, le temps fixer par incrément est de 30ms, incrément sera donc de $255\times \dfrac{30}{594}=12,87878788$, en prenant l'arrondi inférieur $12$.
Pour les paramètres suivant attention :
Param | PIN_RR | PIN_RL | PIN_YR | PIN_YL |
---|---|---|---|---|
Amplitude | 25 | 25 | 0 | 0 |
Offset | -15 | 15 | 0 | 0 |
phase0 | 0 | 300 | 90 | 90 |
Phase0 | $Phase0\times \dfrac{255}{360}$ | 0 | 0x00 |
---|---|---|---|
Amplification | $A\times \dfrac{255}{90}$ | 71 | 0x47 |
Offset | $(Offset+90)\times \dfrac{255}{180}$ | 106 | 0x6A |
Le +90 de l'offset viens du fait de prendre la position milieu du servo en position initiale
puis on répète l'opération pour les 4 moteurs, ce qui donne :
ldi ZL,low(moonWalkRight<<1) ;en entrée ldi ZH,high(moonWalkRight<<1) rcall moonWalk_Move ... ... moonWalkRight: .db 0x4,0xC ;nombre de pas, incrément de phase: temps entre deux mouvements ;4 moteurs .db 0x0,0x47,0x6A, 0xD5,0x47,0x95 ;Phase0,Amplification, Offset des moteurs Phase0,Amplification, Offset des moteurs .db 0x40,0x0,0x80, 0x40,0x0,0x80 ;Phase0,Amplification, Offset des moteurs Phase0,Amplification, Offset des moteurs ;Phase0*255/360,A*255/90,(Offset+90+trim)*255/180 * 4 moteursToutes les 30ms, nous allons faire varier l'angle $Ph$, $Position~servo=A \times SIN(Ph+Phase0)+Offset+90$
Calcul du sinus
J'ai réaliser une table de sinus spécifique, au vue de la précision demandée par un SG90.L'angle d'entrée est en bdeg (inventé par moi-même, le "bit degré"), il suffit de se dire que le tour d'un cercle fait 256 bits, de 0 à 255, et voilà. La valeur 64, représente 90°.
La sortie de la fonction sinus est une valeur entre -128 et +127.
bdeg deg sin(bdeg) | 0 0,00 0 | 1 1,41 3 | 2 2,81 6 | 3 4,22 9 | 4 5,63 12 | 5 7,03 16 | 6 8,44 19 | 7 9,84 22 | 8 11,25 25 | 9 12,66 28 | 10 14,06 31 | 11 15,47 34 | 12 16,88 37 | 13 18,28 40 | 14 19,69 43 | 15 21,09 46 |
---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
bdeg deg sin(bdeg) | 16 22,50 49 | 17 23,91 51 | 18 25,31 54 | 19 26,72 57 | 20 28,13 60 | 21 29,53 63 | 22 30,94 65 | 23 32,34 68 | 24 33,75 71 | 25 35,16 73 | 26 36,56 76 | 27 37,97 78 | 28 39,38 81 | 29 40,78 83 | 30 42,19 85 | 31 43,59 88 |
bdeg deg sin(bdeg) | 32 45,00 90 | 33 46,41 92 | 34 47,81 94 | 35 49,22 96 | 36 50,63 98 | 37 52,03 100 | 38 53,44 102 | 39 54,84 104 | 40 56,25 106 | 41 57,66 107 | 42 59,06 109 | 43 60,47 111 | 44 61,88 112 | 45 63,28 113 | 46 64,69 115 | 47 66,09 116 |
bdeg deg sin(bdeg) | 48 67,50 117 | 49 68,91 118 | 50 70,31 120 | 51 71,72 121 | 52 73,13 122 | 53 74,53 122 | 54 75,94 123 | 55 77,34 124 | 56 78,75 125 | 57 80,16 125 | 58 81,56 126 | 59 82,97 126 | 60 84,38 126 | 61 85,78 127 | 62 87,19 127 | 63 88,59 127 |
Ce qui donne en assembleur
.db 0x0,0x3,0x6,0x9,0xc,0x10,0x13,0x16,0x19,0x1c,0x1f,0x22,0x25,0x28,0x2b,0x2e,
.db 0x31,0x33,0x36,0x39,0x3c,0x3f,0x41,0x44,0x47,0x49,0x4c,0x4e,0x51,0x53,0x55,0x58,
.db 0x5a,0x5c,0x5e,0x60,0x62,0x64,0x66,0x68,0x6a,0x6b,0x6d,0x6f,0x70,0x71,0x73,0x74,
.db 0x75,0x76,0x78,0x79,0x7a,0x7a,0x7b,0x7c,0x7d,0x7d,0x7e,0x7e,0x7e,0x7f,0x7f,0x7f
Pour ce qui est des détails, voir la fonction "calcsin" dans le programme.
Électronique
|
Pour le branchement des servo-moteur SG90, c'est simple S(Signal) fil jaune, + fils rouge, Masse fil marron Avec la batteries de 3000mAh l'autonomie avant recharge est d'environs 3000/900 soit 3 heures. Les leds 1 et 2, sont allumées en permanence pour les yeux. PB4 et PB5 ne sont pas utilisés, les deux ports sont prévues pour ou des options, mais la carte vas me servir pour plusieurs montages Attiny85-Servos. |
Programmation
Comme d'habitude en ce moment, en assembleur. Pour simplifier la mise en position, pour le montage, les 4 moteurs se mettent à l'allumage en position 90° durant 2 secondes.Avant le montage des servos, on allume 1seconde, et on eteint, puis on vis.
si vous pouvez graver directement le HEX, dans l'attiny: Programme 1764octets/8192 Flash et 100octets/512 RAM
Pour le graver faire un truc du genre:
"C:/program files (x86)/Arduino/hardware/tools/avr/bin/avrdude" -C "C:/program files (x86)/Arduino/hardware/tools/avr/etc/avrdude.conf" -c avrisp -P COM5 -b 19200 -p t85 -U flash:w:"servos.hex"
Tonneau tiny (En cours)
En allant sur le net, j'ai trouvé l'idée rigolote.
- Objectif :Un tonneau autonome.
- Moyen:
- 1 Arduino_pro_mini
- 2 SG90 (modifier 360°)
- 1 capteur HC-SR04
- 1 Module GY521 MPU6050, capteurs gyroscopiques analogiques à 3 axes + accéléromètre
- 1 batterie Lipo 3.7V 650mAh
Théorie
Pour comprendre le principe, prenez un balai, essayé de le faire tenir debout, il ne peut pas rester en équilibre tout seul. Il tombera tout simplement.
Alors comment l'équilibrer ? Nous déplaçons notre doigt dans la direction dans laquelle le balai tombe.
Le cas d'un robot auto-équilibré est similaire, sauf que le robot tombera vers l'avant ou vers l'arrière.
Tout comme nous équilibrons un balai sur notre doigt, nous équilibrons le robot en entraînant ses roues dans la direction dans laquelle il tombe.
Ce que nous essayons de faire ici est de maintenir le centre de gravité du robot exactement au-dessus du point de pivot.
Pour piloter les deux moteurs, devons connaître la direction dans laquelle le robot tombe, le degré d'inclinaison du robot et la vitesse à laquelle il tombe.
Toutes ces informations peuvent être déduites des lectures obtenues à partir du capteurs gyroscopiques MPU6050.
Choix du microcontrolleur
Il y a plusieurs calculs a réaliser, plus la gestion du PID.
Nombre de pattes nécessaires :
N° Broche Composant Fonction 1 Trig HC-SR04 Trigger input : Entrée de déclenchement de la mesure 2 Echo HC-SR04 Echo output: Sortie de mesure donnée en écho 3 PWM1 SG90 Pulse Width Modulation : Control Servomoteur 1 4 PWM2 SG90 Pulse Width Modulation : Control Servomoteur 2 5 SDA MPU6050 Serial Data Line : ligne de données bidirectionnelle 6 SCL MPU6050 Serial Clock Line : ligne d'horloge de synchronisation bidirectionnelle. 7 VCC - Alimentaion 8 GND - Alimentaion Mesurer l'angle d'inclinaison à l'aide d'un accéléromètre
accZ<0
accY<0
$\varphi =\theta-\pi$accZ=-1
accY=0
$\varphi =0$accZ<0
accY>0
$\varphi =-\theta+\pi$Le MPU6050 dispose d'un accéléromètre 3 axes et d'un gyroscope 3 axes.
L'accéléromètre mesure l'accélération le long des trois axes et le gyroscope mesure la vitesse angulaire autour des trois axes.
Pour mesurer l'angle d'inclinaison du robot, nous avons besoin de valeurs d'accélération le long des axes y et z.
La fonction atan2 (y,z) donne l'angle en radians entre l'axe z positif d'un plan et le point donné par les coordonnées (z,y) sur ce plan, avec signe positif pour les angles dans le sens antihoraire (demi-angle droit plan, y > 0), et signe négatif pour les angles dans le sens des aiguilles d'une montre (demi-plan gauche, y < 0).//Mesurer l'angle d'inclinaison à l'aide d'un accéléromètre #include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" #include "math.h" MPU6050 mpu; int16_t accY, accZ; float accAngle; void setup() { mpu.initialize(); Serial.begin(9600); } void loop() { accZ = mpu.getAccelerationZ(); accY = mpu.getAccelerationY(); accAngle = atan2(accY, accZ)*RAD_TO_DEG; if(isnan(accAngle)); else Serial.println(accAngle); }
Essayez de déplacer le robot vers l'avant et vers l'arrière tout en le maintenant incliné à un angle fixe.
Vous remarquerez que l'angle affiché sur votre moniteur série change soudainement.
Cela est dû au fait que la composante horizontale de l'accélération interfère avec les valeurs d'accélération des axes y et z.
Mesurer l'angle d'inclinaison à l'aide d'un gyroscope
Le gyroscope du MPU6050 mesure la vitesse angulaire suivant les 3 axes x,y et z.
Pour notre robot auto-équilibré, la vitesse angulaire le long de l'axe x est suffisante pour mesurer l'angle de chute du robot. Le code ci-dessous, permet de lire la valeur du gyroscope autour de l'axe x, la convertissons en degrés par seconde, puis la multiplions par le temps de boucle pour obtenir le changement d'angle.
Nous l'ajoutons à l'angle précédent pour obtenir l'angle actuel.//Mesurer l'angle d'inclinaison à l'aide d'un gyroscope #include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" MPU6050 mpu; int16_t gyroX, gyroRate; float gyroAngle=0; unsigned long currTime, prevTime=0, loopTime; void setup() { mpu.initialize(); Serial.begin(9600); } void loop() { currTime = millis(); loopTime = currTime - prevTime; prevTime = currTime; gyroX = mpu.getRotationX(); gyroRate = map(gyroX, -32768, 32767, -250, 250); gyroAngle = gyroAngle + (float)gyroRate*loopTime/1000; Serial.println(gyroAngle); }
Lors du démarrage, le MPU6050 met sont inclinaison à zéro, il faut donc mettre le robot dans une position stable a la mise sous tension.
Combiner les résultats avec un filtre complémentaire
La lecture de l'accéléromètre est affectée par des signaux de courte durée et la lecture du gyroscope par des signaux de longue durée.
Ces lectures sont, complémentaires les unes des autres. Pour avoir une lecture stable et précise de l'angle, le filtre complémentaire est essentiellement un filtre passe-haut agissant sur le gyroscope et un filtre passe-bas agissant sur l'accéléromètre pour filtrer la dérive et le bruit de la mesure.
$angle_{actuel}=\alpha\cdot (angle_{precedent}+angle_{Gyroscope})+(1-\alpha)\cdot angle_{acceleration}\\$
Avec le filtre passe-haut (HPF) $\alpha\cdot (angle_{precedent}+angle_{Gyroscope})$ et le filtre passe-bas (LPF) $(1-\alpha)\cdot angle_{acceleration}\\$ Ce qui donne $\alpha=\dfrac{\tau}{\tau+dt}=\dfrac{0,75}{0,75+0,005}=0,9934\\$ 0,005 est le temps d'échantillonnage choisi correspondant à 5ms
$1-\alpha=1-0,9934=0,0066$
$angle_{actuel}=0,9934\times (angle_{precedent}+angle_{Gyroscope})+0,0066 \times angle_{acceleration}\\$
0,9934 et 0,0066 sont des coefficients de filtre pour une constante de temps de filtre de 0,75 s.
Calibrage de du MPU6050
Ce code n'est pas de moi, un grand merci à Jeff Rowberg pour son travail.
// I2Cdev and MPU6050 must be installed as libraries #include "I2Cdev.h" #include "MPU6050.h" #include "Wire.h" /////////////////////////////////// CONFIGURATION ///////////////////////////// //Change this 3 variables if you want to fine tune the skecth to your needs. int buffersize=1000; //Quantité de lectures utilisées pour la moyenne, augmentez-la pour obtenir plus de précision, mais le programme sera plus lente (default:1000) int acel_deadzone=8; //Erreur d'accéléromètre autorisée, réduisez-la pour obtenir plus de précision, mais le programme peut ne pas converger (default:8) int giro_deadzone=1; //Erreur Giro autorisée, réduisez-la pour obtenir plus de précision, mais le programme peut ne pas converger (default:1) // default I2C address is 0x68 // des adresses I2C spécifiques peuvent être passées en paramètre ici // AD0 low = 0x68 (default for InvenSense evaluation board) // AD0 high = 0x69 //MPU6050 accelgyro; MPU6050 accelgyro(0x68); // <-- use for AD0 high int16_t ax, ay, az,gx, gy, gz; int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0; int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset; /////////////////////////////////// SETUP //////////////////////////////////// void setup() { // join I2C bus (I2Cdev library doesn't do this automatically) Wire.begin(); // COMMENTEZ LA LIGNE SUIVANTE SI VOUS UTILISEZ ARDUINO DUE TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz. // initialize serial communication Serial.begin(115200); // initialize device accelgyro.initialize(); // wait for ready while (Serial.available() && Serial.read()); //tampon vide while (!Serial.available()){ Serial.println(F("Envoyez n'importe quel caractère pour démarrer le programme.\n")); delay(1500); } while (Serial.available() && Serial.read()); // tampon à nouveau vide // start message Serial.println("\nMPU6050 Programme d'étalonnage"); delay(2000); Serial.println("\nVotre MPU6050 doit être placé en position horizontale, avec les lettres vers le haut. \nNe le touchez pas jusqu'à ce que vous voyiez le message de fin.\n"); delay(3000); // verify connection Serial.println(accelgyro.testConnection() ? "MPU6050 connexion réussie" : "MPU6050 la connexion a échoué"); delay(1000); // reset offsets accelgyro.setXAccelOffset(0); accelgyro.setYAccelOffset(0); accelgyro.setZAccelOffset(0); accelgyro.setXGyroOffset(0); accelgyro.setYGyroOffset(0); accelgyro.setZGyroOffset(0); } /////////////////////////////////// LOOP //////////////////////////////////// void loop() { if (state==0){ Serial.println("\nLecture des capteurs pour la première fois..."); meansensors(); state++; delay(1000); } if (state==1) { Serial.println("\nCalcul des décalages..."); calibration(); state++; delay(1000); } if (state==2) { meansensors(); Serial.println("\nCalcul terminé!"); Serial.print("\nRésultat des Calculs:\t"); Serial.print(mean_ax); Serial.print("\t"); Serial.print(mean_ay); Serial.print("\t"); Serial.print(mean_az); Serial.print("\t"); Serial.print(mean_gx); Serial.print("\t"); Serial.print(mean_gy); Serial.print("\t"); Serial.println(mean_gz); Serial.print("Vos décalages:\tmpu.setXAccelOffset("); Serial.print(ax_offset); Serial.print(");\nmpu.setYAccelOffset("); Serial.print(ay_offset); Serial.print(");\nmpu.setZAccelOffset("); Serial.print(az_offset); Serial.print(");\nmpu.setXGyroOffset("); Serial.print(gx_offset); Serial.print(");\nmpu.setYGyroOffset("); Serial.print(gy_offset); Serial.print(");\nmpu.setZGyroOffset("); Serial.println(gz_offset); Serial.print(");\n"); Serial.println("\nLes données sont données sous la forme : acelX acelY acelZ giroX giroY giroZ"); Serial.println("Vérifiez que les lectures de votre capteur sont proches de 0 0 16384 0 0 0"); Serial.println("Si l'étalonnage a réussi, notez vos décalages afin de pouvoir les définir dans vos projets en utilisant quelque chose de similaire à mpu.setXAccelOffset(votre décalage);"); while (1); //Fin de programme } } /////////////////////////////////// FUNCTIONS //////////////////////////////////// void meansensors(){ long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0; while (i<(buffersize+101)){ // read raw accel/gyro measurements from device accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); if (i>100 && i<=(buffersize+100)){ //Les 100 premières mesures sont ignorées buff_ax=buff_ax+ax; buff_ay=buff_ay+ay; buff_az=buff_az+az; buff_gx=buff_gx+gx; buff_gy=buff_gy+gy; buff_gz=buff_gz+gz; } if (i==(buffersize+100)){ mean_ax=buff_ax/buffersize; mean_ay=buff_ay/buffersize; mean_az=buff_az/buffersize; mean_gx=buff_gx/buffersize; mean_gy=buff_gy/buffersize; mean_gz=buff_gz/buffersize; } i++; delay(2); //Nécessaire pour ne pas avoir de mesures répétées } } void calibration(){ ax_offset=-mean_ax/8; ay_offset=-mean_ay/8; az_offset=(16384-mean_az)/8; gx_offset=-mean_gx/4; gy_offset=-mean_gy/4; gz_offset=-mean_gz/4; while (1){ int ready=0; accelgyro.setXAccelOffset(ax_offset); accelgyro.setYAccelOffset(ay_offset); accelgyro.setZAccelOffset(az_offset); accelgyro.setXGyroOffset(gx_offset); accelgyro.setYGyroOffset(gy_offset); accelgyro.setZGyroOffset(gz_offset); meansensors(); Serial.println("..."); if (abs(mean_ax)<=acel_deadzone) ready++; else ax_offset=ax_offset-mean_ax/acel_deadzone; if (abs(mean_ay)<=acel_deadzone) ready++; else ay_offset=ay_offset-mean_ay/acel_deadzone; if (abs(16384-mean_az)<=acel_deadzone) ready++; else az_offset=az_offset+(16384-mean_az)/acel_deadzone; if (abs(mean_gx)<=giro_deadzone) ready++; else gx_offset=gx_offset-mean_gx/(giro_deadzone+1); if (abs(mean_gy)<=giro_deadzone) ready++; else gy_offset=gy_offset-mean_gy/(giro_deadzone+1); if (abs(mean_gz)<=giro_deadzone) ready++; else gz_offset=gz_offset-mean_gz/(giro_deadzone+1); if (ready==6) break; } }
Contrôle PID pour générer les moteurs
$Consigne(t)=K_p\cdot \epsilon(t)+K_i \int \epsilon(\tau) \,d\tau+K_d\dfrac{d\epsilon(\tau)}{d\tau}$
void loop() { // lire les valeurs d'accélération et du gyroscopes accY = mpu.getAccelerationY(); accZ = mpu.getAccelerationZ(); gyroX = mpu.getRotationX(); // régler la puissance du moteur après l'avoir contrainte motorPower = constrain(motorPower, -255, 255); setMotors(motorPower, motorPower); // mesurer la distance toutes les 100 millisecondes if((count%20) == 0){ distanceCm = sonar.ping_cm(); } if((distanceCm < 20) && (distanceCm != 0)) { setMotors(-motorPower, motorPower); } } // L'ISR sera appelé toutes les 5 millisecondes ISR(TIMER1_COMPA_vect) { // calculer l'angle d'inclinaison accAngle = atan2(accY, accZ)*RAD_TO_DEG; gyroRate = map(gyroX, -32768, 32767, -250, 250); gyroAngle = (float)gyroRate*sampleTime; currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle); error = currentAngle - targetAngle; errorSum = errorSum + error; errorSum = constrain(errorSum, -300, 300); //calculate output from P, I and D values motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime; prevAngle = currentAngle; // basculer la led sur la broche 13 toutes les secondes count++; //200*0.005 = 1s if(count == 200) { count = 0; digitalWrite(13, !digitalRead(13)); } }
Programme complet
#include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" #include "math.h" #include <NewPing.h> #define leftMotorPWMPin 6 #define leftMotorDirPin 7 #define rightMotorPWMPin 5 #define rightMotorDirPin 4 #define TRIGGER_PIN 9 #define ECHO_PIN 8 #define MAX_DISTANCE 75 #define Kp 40 #define Kd 0.05 #define Ki 40 #define sampleTime 0.005 #define targetAngle -2.5 MPU6050 mpu; NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); int16_t accY, accZ, gyroX; volatile int motorPower, gyroRate; volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0; volatile byte count=0; int distanceCm; void setMotors(int leftMotorSpeed, int rightMotorSpeed) { if(leftMotorSpeed >= 0) { analogWrite(leftMotorPWMPin, leftMotorSpeed); digitalWrite(leftMotorDirPin, LOW); } else { analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed); digitalWrite(leftMotorDirPin, HIGH); } if(rightMotorSpeed >= 0) { analogWrite(rightMotorPWMPin, rightMotorSpeed); digitalWrite(rightMotorDirPin, LOW); } else { analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed); digitalWrite(rightMotorDirPin, HIGH); } } void init_PID() { // initialize Timer1 cli(); // disable global interrupts TCCR1A = 0; // set entire TCCR1A register to 0 TCCR1B = 0; // same for TCCR1B // set compare match register to set sample time 5ms OCR1A = 9999; //(FREQ*Tps/Div-1) ici : (16E6*0,005/8-1)=9999 cycles // turn on CTC mode TCCR1B |= (1 << WGM12); // Set CS11 bit for prescaling by 8 TCCR1B |= (1 << CS11); // enable timer compare interrupt TIMSK1 |= (1 << OCIE1A); sei(); // enable global interrupts } void setup() { // set the motor control and PWM pins to output mode pinMode(leftMotorPWMPin, OUTPUT); pinMode(leftMotorDirPin, OUTPUT); pinMode(rightMotorPWMPin, OUTPUT); pinMode(rightMotorDirPin, OUTPUT); // set the status LED to output mode pinMode(13, OUTPUT); // initialize the MPU6050 and set offset values mpu.initialize(); mpu.setYAccelOffset(1593); mpu.setZAccelOffset(963); mpu.setXGyroOffset(40); // initialize PID sampling loop init_PID(); } void loop() { // read acceleration and gyroscope values accY = mpu.getAccelerationY(); accZ = mpu.getAccelerationZ(); gyroX = mpu.getRotationX(); // set motor power after constraining it motorPower = constrain(motorPower, -255, 255); setMotors(motorPower, motorPower); // measure distance every 100 milliseconds if((count%20) == 0){ distanceCm = sonar.ping_cm(); } if((distanceCm < 20) && (distanceCm != 0)) { setMotors(-motorPower, motorPower); } } // The ISR will be called every 5 milliseconds ISR(TIMER1_COMPA_vect) { // calculate the angle of inclination accAngle = atan2(accY, accZ)*RAD_TO_DEG; gyroRate = map(gyroX, -32768, 32767, -250, 250); gyroAngle = (float)gyroRate*sampleTime; currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle); error = currentAngle - targetAngle; errorSum = errorSum + error; errorSum = constrain(errorSum, -300, 300); //calculate output from P, I and D values motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime; prevAngle = currentAngle; // toggle the led on pin13 every second count++; if(count == 200) { count = 0; digitalWrite(13, !digitalRead(13)); } }
Les déplacements
Le déplacement se fait par deux roues, gauche et droite pouvant aller en avant/en arrière à des vitesses différentes.
Cas de figure des déplacements. Le capteur HC-SR04 n'indique qu'une distance.Distance en cm Action gauche Action droite C'est tout pour aujourd'hui
Utilisation de moteur DC pour Fred
- Objectif :faire rouler un mobile.
- Moyen:
- 1 Arduino nano
- 4 moteurs TT 130 avec roues
- 1 PCA9685 PWM 12 bits 16 canaux I²C
- 2 MX1508 pilote de moteur dc, dc 2V-10V, 1,5a, vitesse PWM à deux voies, Pont en H L298N
- 1 capteur ultra-son HC-SR04
- 1 batterie Lipo 7,4V XXXmAh
Les déplacements
Le déplacement se fait quatre deux roues motorisées, pouvant aller en avant/en arrière à des vitesses différentes.
Branchement
C'est tout pour aujourd'hui