Self-made digital stabilizer made with Arduino

I made a digital stabilizer with Arduino and Acceleration & Gyroscope

 2018.9.2

Makeing of self-made digital stabilizer

1. Overview

I tried to create a digital stabilizer using three servos, Arduino Nano and acceleration & gyro sensor, so I will introduce it.
Please refer to the photograph for the finish. Move the handgrip and move so that the smartphone keeps the level.
I also made it possible to change the direction with a joystick.
digitalstabilizer Picture 1digitalstabilizer Picture 2

Main Components 

・Micro2BB Servo x3
・Arduino Nano Control board
・MPU6050 3 axis Acceleration & 3 axis Gyroscope and MPU9250 3 axis Acceleration & 3 axis Gyroscope & 3 axis magnetic sensor
・Joystick
・Board (Universal Board)
・6V NiMH Battery
・3D servo bracket and case

2. System designing

The components of the system are as follows.

digitalstabilizer Picture 11

Circuit diagram of the universal board is here

digitalstabilizer Picture 10

3. Acceleration & Gyroscope

It is a 3 axis accelerometer + 3 axis gyro sensor with a chip called MPU 6050 used this time.

Type:GY521
Weight:18.1 g
Size:3 x 2.1 x 0.5 cm Input Voltage:3-5V
Interface: I2C (2 wire, IIC)
Gyro rage：±250 500 1000 2000 °/s
Accelerometer rage:±2±4±8±16g

digitalstabilizer Picture 11

Data sheet

MPU-6050 DataSheet
MPU-6050 Register Map

4. Acceleration & Gyroscope & Magnetic sensor

And it is a 3 axis accelerometer + 3 axis gyro sensor + 3 axis magnetic sensor with a chip called MPU 9250.

Module model:GY-9250
Chip:MPU-9250
Core voltage:DC2.4V～3.6V
　・I/O voltage:1.71V～Core voltage
　・Interface：I2C or SPI
　・Size:13x11mm
　・Weight:0.45g
Acceleration
　・Measurement range ±2 / ±4 / ±8 / ±16g
　・Resolution:16bit
　・Output rate:0.24～4000Hz
Gyro
　・Measurement range ±250 / ±500 / ±1000 / ±2000dps（°/sec）
　・Resolution:16bit
　・Output rate:4～8000Hz
Compass（AK8963）
　・Measurement range:±4800μT
　・Resolution:14bit/16bit

digitalstabilizer Picture 12

MPU9250 is compatible to MPU6050.

digitalstabilizer Picture 13

This time I will use a complementary filter that is relatively easy to implement.

Basic idea

Angle = 0.98 x (Previous angle + Gyro value x Sampling cycle) x 0.02 x Accelerometer Angle

5. Structure designing

Structural design was done with Fusion 360 and outputted with a 3D printer.

digitalstabilizer Picture 3 digitalstabilizer Picture 4 digitalstabilizer Picture 5
digitalstabilizer Picture 6 digitalstabilizer Picture 7

6. Arduino's sketch

Arduino's sketch is below.

MPU6050 ver.

#include
#include 

const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcXr,AcYr,AcZr,Tmpr,GyXr,GyYr,GyZr;
float AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float AcXAng,AcZAng;
float angX = 0.0;
float angY = 0.0;
float angZ = 0.0;
int angXi = 0;
int angYi = 0;
int angZi = 0;
int angXiLast = 0;
int angYiLast = 0;
int angZiLast = 0;
int aX,aY,aZ;
int angXout, angYout,angZout;
int ZeroX = 93;
int ZeroY = 82;
int ZeroZ = 93;
float dt=30;
int djoy=3;
int joy_1 = 1;
int joy_2 = 2;
int val1,val2;
int angjoy1 =0;
int angjoy2 =0;
int dstep = 50;

Servo myservo2;
Servo myservo3;
Servo myservo4;

int Width_Min = 700;    //Minimum pulse width（ms）
int Width_Max = 2300;   //Maximum pulse width（ms）


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

  //LPF Setting
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1A);
  Wire.write(0x01);
  Wire.endTransmission();
  
  myservo2.attach(2,Width_Min,Width_Max);
  myservo3.attach(3,Width_Min,Width_Max);
  myservo4.attach(4,Width_Min,Width_Max);

  Serial.begin(9600);

} 

void loop() 
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcXr=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcYr=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZr=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmpr=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyXr=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyYr=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZr=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

// joystic data
  val1 = analogRead(joy_1);
  val2 = analogRead(joy_2);

  if (val1 < 300){
    angjoy1 = angjoy1 - djoy;
  }
  if (val1 > 700){
    angjoy1 = angjoy1 + djoy;
  }
  if (val2 < 300){
    angjoy2 = angjoy2 - djoy;
  }
  if (val2 > 700){
    angjoy2 = angjoy2 + djoy;
  }
  
// accel data
// full scale range ±2g LSB sensitivity 16384 LSB/g) -> ±2 x 16384 = ±32768 LSB
  AcX = AcXr/16384.0;
  AcY = AcYr/16384.0;
  AcZ = AcZr/16384.0;
  
// temperture
  Tmp = Tmpr/320.00+36.53;

// gyro data
// full scale range ±250 deg/s LSB sensitivity 131 LSB/deg/s -> ±250 x 131 = ±32750 LSB
  GyX = GyXr/131.0;
  GyY = GyYr/131.0;
  GyZ = GyZr/131.0;

  AcXAng = atan(AcZ/AcY)/3.142*180;
  AcZAng = atan(AcX/AcY)/3.142*180;

//Complementary filter
  angX = 0.95*(angX - GyX*dt*0.001*1.5) + 0.05*AcXAng;
  angZ = 0.95*(angZ + GyZ*dt*0.001*1.2) + 0.05*AcZAng;

//Vertical axis
  angY = angY - GyY*dt*0.001;

  angXiLast = angXi;
  angYiLast = angYi;
  angZiLast = angZi;

  angXi = (int) angX + angjoy1;
  angYi = (int) angY - angjoy2;
  angZi = (int) angZ;

  aX = angXi - angXiLast;
  aY = angYi - angYiLast;
  aZ = angZi - angZiLast;
  
  for (int k=1; k <= dstep ; k++){
    angXout = aX*k/dstep + angXiLast;
    angYout = aY*k/dstep + angYiLast;
    angZout = aZ*k/dstep + angZiLast;

    if (angXout < -40) angXout = -40;
    if (angXout > 20) angXout = 20;
    if (angYout < -80) angYout = -80;
    if (angXout > 80) angYout = 80;
    if (angZout < -70) angZout = -70;
    if (angZout > 70) angZout = 70;

    myservo2.write(ZeroY - angYout);
    myservo3.write(ZeroX - angXout);
    myservo4.write(ZeroZ - angZout);

    delayMicroseconds(dt/dstep*1000);
  }
}

MPU9250 ver.

#include
#include 

const int MPU_addr=0x68;  // I2C address of the MPU-6050
const int AK8963_addr=0x0c;  // I2C address of the AK8963
int16_t AcXr,AcYr,AcZr,Tmpr,GyXr,GyYr,GyZr,MgXr,MgYr,MgZr;
int16_t Mg[7];
float AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,MgX,MgY,MgZ,MgXmin,MgYmin,MgZmin,MgXmax,MgYmax,MgZmax,MgXoff,MgYoff,MgZoff;
float AcXAng,AcZAng,AcXAngPi,AcZAngPi,MgYAng;
float Upper,Lower;
float angX = 0.0;
float angY = 0.0;
float angZ = 0.0;
int angXi = 0;
int angYi = 0;
int angZi = 0;
int angXiLast = 0;
int angYiLast = 0;
int angZiLast = 0;
int aX,aY,aZ;
int angXout, angYout,angZout;
int ZeroX = 93;
int ZeroY = 82;
int ZeroZ = 93;
float dt=30;
int djoy=3;
int joy_1 = 1;
int joy_2 = 2;
int val1,val2;
int angjoy1 =0;
int angjoy2 =0;
int dstep = 50;

Servo myservo2;
Servo myservo3;
Servo myservo4;

int Width_Min = 700;    //Minimum pulse width（ms）
int Width_Max = 2300;   //Maximum pulse width（ms）


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

  //LPF Setting
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1A);
  Wire.write(0x01);
  Wire.endTransmission();

  //AK8963 Setting
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x37);  //Address for setting bypass mode of magnetic sensor
  Wire.write(0x02);  //bypass mode (Magnetic sensor can be used)
  Wire.endTransmission();
  Wire.beginTransmission(AK8963_addr);
  Wire.write(0x0a);  //Address for magnetic sensor setting
  Wire.write(0x16);  //Output cycle of the magnetic sensor (100Hz) 0x12: Output cycle of the magnetic sensor (8Hz)
  Wire.endTransmission();
  
  myservo2.attach(2,Width_Min,Width_Max);
  myservo3.attach(3,Width_Min,Width_Max);
  myservo4.attach(4,Width_Min,Width_Max);

  Serial.begin(9600);

} 

void loop() 
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcXr=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcYr=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZr=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmpr=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyXr=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyYr=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZr=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  Wire.beginTransmission(AK8963_addr);
  Wire.write(0x02);  // starting with register 0x02
  Wire.requestFrom(AK8963_addr,1,true);
  Wire.endTransmission(false);

  //ST1Bit=Wire.read();

  if((Wire.read() & 0x01)){   //Check if reading is ready
    Wire.beginTransmission(AK8963_addr);
    Wire.write(0x03);  // starting with register 0x02
    Wire.endTransmission(false);
    Wire.requestFrom(AK8963_addr,7,true);  // request a total of 7 registers
  
    MgYr=Wire.read()|(Wire.read()<<8);
    MgXr=Wire.read()|(Wire.read()<<8);
    MgZr=-(Wire.read()|(Wire.read()<<8));
  }

// joystic data
  val1 = analogRead(joy_1);
  val2 = analogRead(joy_2);

  if (val1 < 300){
    angjoy1 = angjoy1 - djoy;
  }
  if (val1 > 700){
    angjoy1 = angjoy1 + djoy;
  }
  if (val2 < 300){
    angjoy2 = angjoy2 - djoy;
  }
  if (val2 > 700){
    angjoy2 = angjoy2 + djoy;
  }
  
// accel data
// full scale range ±2g LSB sensitivity 16384 LSB/g) -> ±2 x 16384 = ±32768 LSB
  AcX = AcXr/16384.0;
  AcY = AcYr/16384.0;
  AcZ = AcZr/16384.0;
  
// temperture
  Tmp = Tmpr/320.00+36.53;

// gyro data
// full scale range ±250 deg/s LSB sensitivity 131 LSB/deg/s -> ±250 x 131 = ±32750 LSB
  GyX = GyXr/131.0;
  GyY = GyYr/131.0;
  GyZ = GyZr/131.0;
  
  AcZAngPi = atan(AcX/AcY);
  AcZAng = AcZAngPi/3.142*180;
  AcXAngPi = atan(-AcZ/(AcX*sin(AcZAngPi)+AcY*cos(AcZAngPi)));
  AcXAng = AcXAngPi/3.142*180;
  
// Magnetic data
  MgX = MgXr / 32768.0f * 4800.0f;//Conversion to [uT]
  MgY = MgYr / 32768.0f * 4800.0f;//Conversion to [uT]
  MgZ = MgZr / 32768.0f * 4800.0f;//Conversion to [uT]

//Calculation of offset

  if (MgX <= MgXmin) MgXmin = MgX;
  if (MgX >= MgXmax) MgXmax = MgX;
  if (MgY <= MgYmin) MgYmin = MgY;
  if (MgY >= MgYmax) MgYmax = MgY;
  if (MgZ <= MgZmin) MgZmin = MgZ;
  if (MgZ >= MgZmax) MgZmax = MgZ;

  MgXoff = (MgXmax - MgXmin)/2 + MgXmin;
  MgYoff = (MgYmax - MgYmin)/2 + MgYmin;
  MgZoff = (MgZmax - MgZmin)/2 + MgZmin;

  Upper = (MgY - MgYoff)*sin(AcZAngPi) - (MgX - MgXoff)*cos(AcZAngPi);
  Lower = (MgZ - MgZoff)*cos(AcXAngPi) + (MgX - MgXoff)*sin(AcXAngPi)*sin(AcZAngPi) + (MgY - MgYoff)*sin(AcXAngPi)*cos(AcZAngPi);
  
  if(Lower > 0.0){
    if(Upper > 0.0){
         MgYAng = atan(Upper/Lower)/3.142*180;
      }else if(Upper < 0.0){
         MgYAng = atan(Upper/Lower)/3.142*180;
      }
  }else if(Lower < 0.0){
    if(Upper > 0.0){
         MgYAng = atan(Upper/Lower)/3.142*180 + 180;
      }else if(Upper < 0.0){
         MgYAng = atan(Upper/Lower)/3.142*180 - 180;
      }
    }
  
//Complementary filter
  angX = 0.9*(angX - GyX*dt*0.001*1.5) - 0.1*AcXAng;
  angZ = 0.9*(angZ + GyZ*dt*0.001*1.2) + 0.1*AcZAng;
  angY = 0.9*(angY - GyY*dt*0.001) - 0.1*MgYAng;

  angXiLast = angXi;
  angYiLast = angYi;
  angZiLast = angZi;

  angXi = (int) angX + angjoy1;
  angYi = (int) angY - angjoy2;
  angZi = (int) angZ;

  aX = angXi - angXiLast;
  aY = angYi - angYiLast;
  aZ = angZi - angZiLast;
  
  for (int k=1; k <= dstep ; k++){
    angXout = aX*k/dstep + angXiLast;
    angYout = aY*k/dstep + angYiLast;
    angZout = aZ*k/dstep + angZiLast;

    if (angXout < -40) angXout = -40;
    if (angXout > 20) angXout = 20;
    if (angYout < -80) angYout = -80;
    if (angYout > 80) angYout = 80;
    if (angZout < -70) angZout = -70;
    if (angZout > 70) angZout = 70;

    myservo2.write(ZeroY - angYout);
    myservo3.write(ZeroX - angXout);
    myservo4.write(ZeroZ - angZout);

    delayMicroseconds(dt/dstep*1000);
  }
}

Update

  • 2017.11.14MPU9250 was added.NEW 
  • 2017.9.2Self-made digital stabilizer made with Arduino 
Copyright © 2017 robotakao All Rights Reserved.

inserted by FC2 system