Self-made digital stabilizer made with Arduino

I made a digital stabilizer with Arduino and Acceleration & Gyroscope

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

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

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

5. Arduino's sketch

Arduino's sketch is below.

#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;    //最小パルス幅(ms)
int Width_Max = 2300;   //最大パルス幅(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設定
  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;

//相補フィルター
  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;

//鉛直軸
  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);
  }
}

Update

  • 2017.9.2Self-made digital stabilizer made with ArduinoNEW 

inserted by FC2 system