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

#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

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

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

C'est tout pour aujourd'hui

• 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

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

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é

Param	PIN_RR	PIN_RL	PIN_YR	PIN_YL
Amplitude	25	25	0	0
Offset	-15	15	0	0
phase0	0	300	90	90

Ce qui vas représenter pour le moteur RR


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

• 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

  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 cm	Action gauche	Action droite

  C'est tout pour aujourd'hui

• 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