Les robots

Les projets réalisés et encours.

Les idées germent doucement, … à voir
Comme beaucoup de projets, ceux-ci sont autonomes, j'en donnerai la consommation en mAh quand se sera nécessaire.
Cette partie me sert de bloc-note, si vous avez des idées n'hésitez pas.

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
 MilieugauchedroiteEn images
012012060 .
195+t8030
2145+t15090
195+t8030
2145+t15090
195+t8030
012012060
Rotation de l'hexapode
 MilieugauchedroiteEn images
012012060 .
195+t15030
2145+t8090
195+t15030
2145+t8090
195+t15030
012012060

+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 principe de fonctionnement avec 3 servos, un Attiny85 et le tout en assembleur.
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 vas être décomposé de manière à pouvoir accélérer ou ralentir.
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   Wait4xCycles
Revenons à 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 :
  1. Paramètre de temps entre 0 et 255, je viens tout juste de l'expliquer, c'est Speed, suivez!
  2. L'angle du moteur central entre 0 et 127 (ne tiens pas compte de la vitesse), vitesse maximale pour lui
  3. L'angle du moteur gauche entre 0 et 127
  4. 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.
DistanceFonctionNombre de
répetitions
0Pas de détection (trop loin)
Avance rapide
2
≥ 35cmAvance rapide2
de ≥25 à <35cmAvance lente2
de ≥10 à <25 cmTourner à gauche2
de <10 cmRecul rapide2

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.
  • SRV1 : moteur du milieu
  • SRV2 : moteur de gauche
  • SRV3 : moteur de droite
  • SRV4 : TRIG du HC-SR04
  • PB4 : ECHO du HC-SR04
Si il y a une led à allumer en permanence la résistance fait 220Ω, en effet si la tension d'entrée est de 5V pour des tests par exemple.
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-SR04
Programme 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
    SMé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:
  • 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-20PU1,73€
    4 moteurs SG90S9,00€
    Batterie 852540 600mAh Ni-Po1,21€
    Bouton Commutateur de verrouillage Auto/Off 8x8mm 6Pin0,14€
    CI PCB chine1,00€
    40 Broches 1x40 Simple Rangée Mâle 0,07€
    led 10mm Rouge (620-625nm)0,18€
    Support fixation clip montage pour LED 10mm0,24€
    Total13.57€
    Soit environs13.57€ mais plus compact et complet avec sa batteries Lipo 3.7V 600mAh, et le CI fait maison.

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

    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ètres en degré
    ParamPIN_RRPIN_RLPIN_YRPIN_YL
    Amplitude252500
    Offset-151500
    phase003009090
    Ce qui vas représenter pour le moteur RR
    Phase0$Phase0\times \dfrac{255}{360}$00x00
    Amplification$A\times \dfrac{255}{90}$710x47
    Offset$(Offset+90)\times \dfrac{255}{180}$1060x6A

    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 moteurs
            
    Toutes 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
    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 :
    BrocheComposantFonction
    1TrigHC-SR04Trigger input : Entrée de déclenchement de la mesure
    2EchoHC-SR04Echo output: Sortie de mesure donnée en écho
    3PWM1SG90Pulse Width Modulation : Control Servomoteur 1
    4PWM2SG90Pulse Width Modulation : Control Servomoteur 2
    5SDAMPU6050Serial Data Line : ligne de données bidirectionnelle
    6SCLMPU6050Serial Clock Line : ligne d'horloge de synchronisation bidirectionnelle.
    7VCC-Alimentaion
    8GND-Alimentaion
    Trig = Entrée de déclenchement de la mesure (Trigger input). • Echo = Sortie de mesure donnée en écho (Echo output).

    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 cmAction gaucheAction 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