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

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

前回、３軸加速度センサー＆３軸ジャイロスコープのMPU6050の加速度センサーのみをRaspberry Pi Zero Wを使ってテストしてみました。

今回はジャイロを使ってテストしてみたいと思います。



主要な構成（前回と同じ）
３軸加速度＆３軸ジャイロスコープ
Raspberry Pi Zero W (python)
Macbook (Processing)

Raspberry Pi側はBluetoothでシリアル接続できるようにしてあることが前提です。過去の記事を参照してください。

結線（前回と同じ）



手順（前回と同じ）
１）Raspberry Pi側でBluetoothのrfcommをlistenする。
２）MacbookのPrecessingコードを実行
３）Raspberry Pi側でpythonのコードを実行

ロジック的には今回はジャイロだけを使用して角加速度を単純積分して角度を概算しています。ジャイロの計測値を積分するため、ドリフトが顕在化するはずです。

結果
センサを回転させるとProcessingのキューブを回転させることができました。
やはりドリフトが顕在化しました。センサーを固定していてもじわじわずれてしまいます。
次回はドリフトの補正をしたいと思います。




Raspberry Pi側pythonコード（前回と同じ）

import smbus
import time
import serial

# slave address
DEV_ADDR = 0x68         # device address
# register address
ACCEL_XOUT = 0x3b
ACCEL_YOUT = 0x3d
ACCEL_ZOUT = 0x3f
TEMP_OUT = 0x41
GYRO_XOUT = 0x43
GYRO_YOUT = 0x45
GYRO_ZOUT = 0x47
PWR_MGMT_1 = 0x6b       # PWR_MGMT_1
PWR_MGMT_2 = 0x6c       # PWR_MGMT_2

bus = smbus.SMBus(1)

bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0)

s = serial.Serial('/dev/rfcomm0', 9600)

# 2byte read
def read_word(adr):
    high = bus.read_byte_data(DEV_ADDR, adr)
    low = bus.read_byte_data(DEV_ADDR, adr+1)
    val = (high << 8) + low
    return val

# Sensor data read
def read_word_sensor(adr):
    val = read_word(adr)
    if (val >= 0x8000):         # minus
        return -((65535 - val) + 1)
    else:                       # plus
        return val
# temperture
def get_temp():
    temp = read_word_sensor(TEMP_OUT)
    x = temp / 340 + 36.53
    return x

# gyro data
# full scale range ±250 deg/s LSB sensitivity 131 LSB/deg/s -> ±250 x 131 = ±32750 LSB
def get_gyro_data_deg():
    x = read_word_sensor(GYRO_XOUT)
    y = read_word_sensor(GYRO_YOUT)
    z = read_word_sensor(GYRO_ZOUT)

    x = x / 131.0
    y = y / 131.0
    z = z / 131.0
    return [x, y, z]

# accel data
# full scale range ±2g LSB sensitivity 16384 LSB/g) -> ±2 x 16384 = ±32768 LSB
def get_accel_data_g():
    x = read_word_sensor(ACCEL_XOUT)
    y = read_word_sensor(ACCEL_YOUT)
    z = read_word_sensor(ACCEL_ZOUT)

    x = x / 16384.0
    y = y / 16384.0
    z = z / 16384.0
    return [x, y, z]

while 1:
    
    temp = get_temp() # temperture
    gyro_x,gyro_y,gyro_z = get_gyro_data_deg() # gyro
    accel_x,accel_y,accel_z = get_accel_data_g() # accel

    print ('%f[deg C] gyro[deg/s] x:%f y:%f z:%f accel[g] x:%f y:%f z:%f' % (temp,gyro_x,gyro_y,gyro_z,accel_x,accel_y,accel_z))
    send_s = str(int(temp*100))+','+str(int(gyro_x*100))+','+str(int(gyro_y*100))+','+str(int(gyro_z*100))+','+str(int(accel_x*100))+','+str(int(accel_y*100))+','+str(int(accel_z*100))
    print(send_s)
    s.write(bytes(send_s,'UTF-8'))

    time.sleep(0.1)
    
s.close()

MacBook側Processing コード

import processing.serial.*;

Serial myPort;  // Create object from Serial class
String datline;

// ジャイロの前回の値と今回の値をミックスする比率
// 前回の値は使用せず今回の値のみを使用するのであれば1.0に設定
final float ratio = 0.5;

// 前回のx軸とy軸の値（初期値は0〜1023の中央値）
float xGyroValueLast = 0;
float yGyroValueLast = 0;
float zGyroValueLast = 0;

float xGyroValue;
float yGyroValue;
float zGyroValue;

float xGyroAngleValue = 0;
float yGyroAngleValue = 0;
float zGyroAngleValue = 0;

void setup() {
  size(800, 800, P3D);
  frameRate(30);
  noStroke();
  colorMode(RGB, 1);

// Serial Port 
  myPort = new Serial(this, "/dev/tty.raspberrypi-SerialPort", 9600);
}

void draw() {
  background(0.5);

  pushMatrix();

  translate(width / 2, height / 2, -30);
  
  if ( myPort.available() > 0) {  // If data is available,
    String datline=myPort.readString();
    int[] splitdata = int(datline.split(","));
    
    println(datline);
    
    xGyroValue = float(splitdata[1])/100;
    yGyroValue = float(splitdata[2])/100;
    zGyroValue = float(splitdata[3])/100;
    
    println(xGyroValue);
    println(yGyroValue);
    println(zGyroValue);

  // 前回の値と今回の値の割合を変えてスムージングする
  // 前回の値の割合を大きくすると、滑らかになる代わりに変化が遅くなり
  // 今回の値の割合を大きくすると、ノイズを拾いやすくなる代わりに変化が早くなる
  float xGyroValueSmoothed = xGyroValue * ratio + xGyroValueLast * (1.0 - ratio);
  float yGyroValueSmoothed = yGyroValue * ratio + yGyroValueLast * (1.0 - ratio);
  float zGyroValueSmoothed = zGyroValue * ratio + zGyroValueLast * (1.0 - ratio);

  // 前回の値としてスムージングした値をセット
  xGyroValueLast = xGyroValueSmoothed;
  yGyroValueLast = yGyroValueSmoothed;
  zGyroValueLast = zGyroValueSmoothed;
  
  // 角速度を積分して角度に変換
  xGyroAngleValue = xGyroAngleValue + xGyroValueSmoothed*0.005;
  yGyroAngleValue = yGyroAngleValue + yGyroValueSmoothed*0.005;
  zGyroAngleValue = zGyroAngleValue + zGyroValueSmoothed*0.005;
  
  println(xGyroAngleValue);
  println(yGyroAngleValue);
  println(zGyroAngleValue);
 
 }
 
   // マウスの代わりに加速度センサの値でコントロール
  rotateX(xGyroAngleValue);
  rotateY(zGyroAngleValue);
  rotateZ(yGyroAngleValue);

  scale(200);

  beginShape(QUADS);

  fill(0, 1, 1);
  vertex(-1, 1, 1);
  fill(1, 1, 1); 
  vertex( 1, 1, 1);
  fill(1, 0, 1); 
  vertex( 1, -1, 1);
  fill(0, 0, 1); 
  vertex(-1, -1, 1);

  fill(1, 1, 1); 
  vertex( 1, 1, 1);
  fill(1, 1, 0); 
  vertex( 1, 1, -1);
  fill(1, 0, 0); 
  vertex( 1, -1, -1);
  fill(1, 0, 1); 
  vertex( 1, -1, 1);

  fill(1, 1, 0); 
  vertex( 1, 1, -1);
  fill(0, 1, 0); 
  vertex(-1, 1, -1);
  fill(0, 0, 0); 
  vertex(-1, -1, -1);
  fill(1, 0, 0); 
  vertex( 1, -1, -1);

  fill(0, 1, 0); 
  vertex(-1, 1, -1);
  fill(0, 1, 1); 
  vertex(-1, 1, 1);
  fill(0, 0, 1); 
  vertex(-1, -1, 1);
  fill(0, 0, 0); 
  vertex(-1, -1, -1);

  fill(0, 1, 0); 
  vertex(-1, 1, -1);
  fill(1, 1, 0); 
  vertex( 1, 1, -1);
  fill(1, 1, 1); 
  vertex( 1, 1, 1);
  fill(0, 1, 1); 
  vertex(-1, 1, 1);

  fill(0, 0, 0); 
  vertex(-1, -1, -1);
  fill(1, 0, 0); 
  vertex( 1, -1, -1);
  fill(1, 0, 1); 
  vertex( 1, -1, 1);
  fill(0, 0, 1); 
  vertex(-1, -1, 1);

  endShape();
  popMatrix();
}



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






Tweet










スポンサードリンク