Muovere Due Motori Servo con un Giroscopio/Accelerometro

Incuriosito dal voler testare la sensibilità di un giroscopio/accelerometro mi sono orientato su un sensore a bassissimo costo, l'MPU-6050, verificandone il funzionamento associando il suo orientamento al movimento di due motori servo.

Ho creato pertanto un piccolo progetto con Arduino UNO dove, tramite il display LCD 1602 I2C, viene mostrato il valore dell'angolo di rotazione di ciascuno dei due motori servo che contestualmente si muovono in base all'orientamento del giroscopio/accelerometro, rispettivamente sull'asse X e sull'asse Y.

Di seguito lo schema elettrico del progetto (nell'immagine il sensore MPU-6050 non è esattamente rappresentativo in quanto non sono riuscito a trovarlo nella libreria del software Fritzing, tuttavia è molto similare e dal video poco sotto potete verificare che quello effettivo è di colore blu)

Fig. 1 - Schema per Arduino - Muovere Due Motori Servo con un Giroscopio/Accelerometro

Ed ecco la parte più interessante del progetto, lo sketch, dovutamente commentato per agevolarne la comprensione:

#include <SPI.h>
#include <Wire.h>
#include <Servo.h>
#define MPU 0x68
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

Servo ServoX, ServoY;
double AcX,AcY,AcZ;
int Pitch, Roll;

void setup() {
  Serial.begin(9600);
  ServoX.attach(9);
  ServoY.attach(10);
  init_MPU(); // Inizializzazione del sensore MPU6050
  lcd.begin(16, 2);
}
 
void loop() {

	// Richiamo alla funzione per l'acquisizione degli assi X, Y e Z del sensore MPU6050
  FunctionsMPU();
    
	// Calcolo dell'angolo intorno all'asse X (Roll) e intorno all'asse Y (Pitch)
  Roll = FunctionsPitchRoll(AcX, AcY, AcZ);
  Pitch = FunctionsPitchRoll(AcY, AcX, AcZ);

  int ServoRoll = map(Roll, -90, 90, 0, 179);
  int ServoPitch = map(Pitch, -90, 90, 179, 0);

  ServoX.write(ServoRoll);
  ServoY.write(ServoPitch);

	// Stampo i valori delle rotazioni sul monitor seriale
  Serial.print("Pitch: "); Serial.print(Pitch);
  Serial.print("	");
  Serial.print("Roll: "); Serial.print(Roll);
  Serial.print("
");

	// Stampo i valori delle rotazioni sul display LCD 1602
  lcd.setCursor(0, 0);
  lcd.write("Pitch:          ");
  lcd.setCursor(7, 0);
  lcd.print(Pitch);
  lcd.setCursor(0, 1);
  lcd.write("Roll:           ");
  lcd.setCursor(6, 1);
  lcd.print(Roll);

}

void init_MPU() {
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  delay(1000);
}

//Funzione per il calcolo degli angoli intorno all'asse X ed Y
double FunctionsPitchRoll(double A, double B, double C) {
  double DatoA, DatoB, Value;
  DatoA = A;
  DatoB = (B*B) + (C*C);
  DatoB = sqrt(DatoB);
  
  Value = atan2(DatoA, DatoB);
  Value = Value * 180/3.14;
  
  return (int)Value;
}

void FunctionsMPU() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
}

Di seguito una breve dimostrazione del funzionamento del progetto dove è evidente la sensibilità del sensore principale utilizzato:

Per consigli, suggerimenti o chiarimenti sentitevi liberi di lasciare un commento!