極力ローコスト ロボット製作 ブログ

RoboTakaoです。ようこそ「極力ローコスト ロボット製作 ブログ」にお越しくださいました。

前回までで３軸加速度センサー＆３軸ジャイロスコープで色々テストしてきましたが、今回はスマホ用にデジタル・スタビライザー(Digital Stabilizer)風のものを自作してみたいと思います。





デジタル・スタビライザーとはビデオカメラ等を手ブレなく撮影するための機材で２軸または３軸のモーターで構成されて振動を吸収するものです。これを簡易的に作ってみようというものです。

今回も前回同様、加速度から算出した角度とジャイロから算出した角度で相補フィルターを使って角度を算出します。

角度 = 0.98 x (前回の角度 + ジャイロの値 x サンプリング周期) x 0.02 x 加速度センサーで出した角度

主要な構成
３軸加速度＆３軸ジャイロスコープ
Arduino Nano
サーボ　GWS Micro2BB
セルカ棒の頭（ダイソー）
ラップの芯
サーボブラケット（以前自分で作った二足歩行ロボットの関節）

スマホとの接続はダイソーのセルカ棒を流用しました。


結線



結果
ラップの棒で作った握り手をグルグル回すと、なんとなく水平をキープしています。
でも画面の中ではブレブレです。なかなか改良の余地があります。
おそらくモーター軸とスマホの重心が一致していないのでモーメントが発生して振動しているのだと思います。
いつか改良してみようと思います。



関連記事
ArduinoとProcessingをFirmataで繋げてみました　その２　加速度センサ
加速度センサー MPU6050 x RaspberryPi x python x Processing
ジャイロ MPU6050 x RaspberryPi x python x Processing
MPU6050 x RaspberryPi x python x Processing 相補フィルター Complementary filter

Arduinoのスケッチ

#include <Wire.h>
#include <Servo.h>

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 angZ = 0.0;
int angXi = 0;
int angZi = 0;
int angXiLast = 0;
int angZiLast = 0;
int aX,aZ;
int angXout, angZout;
float dt=50;

Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;

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);
  
  myservo2.attach(2,Width_Min,Width_Max);
  myservo3.attach(3,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)

// 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.0015) + 0.05*AcXAng;
  angZ = 0.95*(angZ + GyZ*dt*0.0015) + 0.05*AcZAng;

  angXiLast = angXi;
  angZiLast = angZi;

  angXi = (int) angX;
  angZi = (int) angZ;

  aX = angXi - angXiLast;
  aZ = angZi - angZiLast;
  
  for (int k=1; k <=10 ; k++){
    angXout = aX*k/10 + angXiLast;
    angZout = aZ*k/10 + angZiLast;
    
    myservo2.write(95+angXout);
    myservo3.write(100+angZout);

    delay(dt*0.1);
  }
}

それでは今回はこの辺で失礼します！ありがとうございます。