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

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

以前、ArduinoをProcessingからコントロールするFirmataで「Prototyping Lab」という書籍を参考に加速度センサーのレシピを試してみました。

今回は、以前紹介した３軸加速度センサー＆３軸ジャイロスコープPMU6050を使って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 xAxisValueLast = 512;
float yAxisValueLast = 512;
float zAxisValueLast = 512;

float xAxisValue;
float yAxisValue;
float zAxisValue;

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);
    
    xAxisValue = float(splitdata[4])/100;
    yAxisValue = float(splitdata[5])/100;
    zAxisValue = float(splitdata[6])/100;
    
    println(xAxisValue);
    println(yAxisValue);
    println(zAxisValue);
    
  }

  // 前回の値と今回の値の割合を変えてスムージングする
  // 前回の値の割合を大きくすると、滑らかになる代わりに変化が遅くなり
  // 今回の値の割合を大きくすると、ノイズを拾いやすくなる代わりに変化が早くなる
  float xAxisValueSmoothed 
    = xAxisValue * ratio + xAxisValueLast * (1.0 - ratio);
  float yAxisValueSmoothed 
    = yAxisValue * ratio + yAxisValueLast * (1.0 - ratio);
  float zAxisValueSmoothed 
    = zAxisValue * ratio + zAxisValueLast * (1.0 - ratio);

  // 前回の値としてスムージングした値をセット
  xAxisValueLast = xAxisValueSmoothed;
  yAxisValueLast = yAxisValueSmoothed;
  zAxisValueLast = zAxisValueSmoothed;
  
  // スムージングした値をsinθの値とする
  float xAxisSinTheta = xAxisValueSmoothed;
  float yAxisSinTheta = yAxisValueSmoothed;
  float zAxisSinTheta = zAxisValueSmoothed;

  // マウスの代わりに加速度センサの値でコントロール
  rotateZ(asin(xAxisSinTheta/zAxisSinTheta));
  rotateX(asin(yAxisSinTheta/zAxisSinTheta));

  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










スポンサードリンク