Processingでロボットの姿勢をリアルタイムで表示する（プログラム編）

Processing
2020.06.29

みなさん，こんにちは
おかしょです．

Arduinoなどのマイコンを使ってロボットの制御をするときに，リアルタイムでデータをグラフ化したり，アニメーションを表示したくなることがあります．

作製したアニメーションは学会の発表などにも使用できて，発表の幅を広げることができます．

この記事では，リアルタイムでロボットの姿勢をグラフ化，ビジュアライズ化するためのプログラムを公開します．

この記事を読むと以下のようなことがわかる・できるようになります．

  • Processingでリアルタイムでグラフを作る方法
  • 姿勢角の算出方法の比較
  • ArduinoとProcessingを通信させる方法

　

この記事を読む前に

この記事は以下の記事の続きとなっています．

以下の記事では今回作成したアニメーションの構想を解説しています．まだ読んでいない方は以下の記事を先に読んでおくと，プログラムを読み解くこともできると思うので読んでおくことをおすすめします．

Processingでロボットの姿勢をリアルタイムで表示する（構想編）
みなさん，こんにちは．おかしょです． Processingを使うと簡単にアニメーションを作成することが可能です． 私はまだ，国際学会などでProcessingを使ったアニメーションは発表したことがないのですが，それを使えば発表の幅が大きく広...
okasho-engineer.com
2020.06.04

　

Processing側のプログラム

それでは，さっそくProcessingのプログラムから記していきます．

import processing.opengl.*;
import processing.serial.*;
Serial port;
int[] data = new int[21];
int time, phiA, thetaA, phiG, thetaG, phi, theta;  // リアルタイムデータ

// cameraの位置を座標で指定
int cameraX = 0;
int cameraY = -300;
int cameraZ = 1000;

int[] PlotData1 = new int[300];
int[] PlotData2 = new int[300];
int[] PlotData3 = new int[300];
int[] PlotData4 = new int[300];
int[] PlotData5 = new int[300];
int[] PlotData6 = new int[300];


void setup() {
  size(displayWidth,displayHeight, OPENGL);  // windowのサイズを画面いっぱいにする
  smooth();
  port = new Serial(this, Serial.list()[0], 9600);  // Serial port設定
  background(0, 0, 0);  // 背景を黒にする
  frameRate(10);  // frame rateを5fpsに設定
  translate(width/2, height/2, 0);  // 原点を画面の中心に
  camera(cameraX,cameraY,cameraZ,0,0,0,0,1,0);  // 原点を向く
  for(int i=0; i<300; i++)  // plotするデータの初期化
  {
    PlotData1[i] = 0;
    PlotData2[i] = 0;
    PlotData3[i] = 0;
    PlotData4[i] = 0;
    PlotData5[i] = 0;
    PlotData6[i] = 0;
  }
  time = 0;
}

void draw() {
  background(0);
  //phiA = 30;
  //phi = 50;
  //phiG = 0;
  //thetaA = 50;
  //theta = 40;
  //thetaG = -60;
  //time = time + 100;
  // 3次元描画
  pushMatrix();  // 座標系の保存
  RotateBox(0, height/4, 0, 50, 255, 50, phi, theta);  // 真ん中の直方体の描画
  RotateBox(-width/3, height/4, 0, 255, 50, 50, phiA, thetaA);  // 左の直方体の描画
  RotateBox(width/3, height/4, 0, 50, 50, 255, phiG, thetaG);  // 右の直方体の描画
  popMatrix();  // 保存した座標系の出力
  // 2次元描画
  rotateY(atan2(cameraX, cameraZ));  // カメラの向きから2次元に描画できるように座標軸を回転
  rotateX(-atan2(cameraY, sqrt(sq(cameraX)+sq(cameraZ))));
  hint(DISABLE_DEPTH_TEST);  // z軸を無効化
  TimeHistory(-width/2, -height/2+height/2/5, 0, "φ[deg]", 'r', phiA, phi, phiG);  // 時間履歴の表示
  TimeHistory(-width/6+width/3/10, -height/2+height/2/5, 0, "θ[deg]", 'p', thetaA, theta, thetaG);  // 時間履歴の表示
  textSize(height*0.04);  // 文字の大きさ
  textAlign(CENTER);  // 文字の座標指定位置
  text("Accel", -width/3, height/2*0.95);  // 使用センサ描画
  text("Fusion", 0, height/2*0.95);  // 使用センサ描画
  text("Gyro", width/3, height/2*0.95);  // 使用センサ描画
  hint(ENABLE_DEPTH_TEST);  // z軸を有効化
}

void RotateBox(int positionX, int positionY, int positionZ, int ColorR, int ColorG, int ColorB, int roll, int pitch)
{
  pushMatrix();  // 座標系の保存
  fill(ColorR, ColorG, ColorB);  // 直方体の色
  translate(positionX, positionY, positionZ);  // 直方体の位置
  rotateZ(-radians(pitch));
  rotateX(-radians(roll));
  box(450, 20, 300);  // 直方体の描画
  popMatrix();  // 保存した座標系を出力
}

void TimeHistory(int positionX, int positionY, int positionZ, String name, char ch, int data1, int data2, int data3)
{
  // 引数説明 //
  // positionX, positionY, positionZはグラフを表示する枠の左上の点
  // nameはグラフの縦軸のラベル名
  // data0はリアルタイム，data0Pは1ステップ前の時間
  // data1はリアルタイムのデータ，data1Pは1ステップ前のデータ
  // 枠線の描画
  float Width = width/3*0.85;  // 枠の幅
  float Height = height/2*0.6;  // 枠の高さ
  pushMatrix();  // 座標系の保存
  noFill();  // 塗りつぶしなし
  stroke(100);  // 枠線の色
  strokeWeight(3);  // 枠線の幅
  translate(positionX, positionY, positionZ);  // 枠の左上の点を指定
  rect(0, 0, Width, Height);  // 枠の描画
  // メモリ線の描画
  strokeWeight(1);  // メモリ線の幅
  for (int i=1; i<6; i++)  // メモリ線の描画
  {
    line(0, Height/6*i, Width, Height/6*i);  // 縦軸
    line(Width/6*i, 0, Width/6*i, Height);  // 横軸
  }
  // メモリ値の描画
  stroke(0);
  fill(100);  // メモリ値の文字色
  // 縦軸のメモリ値
  textSize(Height*0.06);  // 文字の大きさ
  textAlign(RIGHT);  // 文字の座標指定位置
  int j = -90;  // 縦軸の目盛りの最低値
  for (int i=6; i>=0;i--)  // メモリ戦の描画
  {
    text(j, -width/3*0.02, Height/6*i);  // 文字の描画
    j = j + 30;  // 縦軸の目盛りの更新
  }
  pushMatrix();  // 座標系の保存
  rotate(radians(-90));  // 座標軸を90度回転
  textAlign(CENTER);  // 文字の座標指定位置
  text(name, -Height/2, -width/3*0.1);  // 縦軸ラベルの描画
  popMatrix();  // 保存した座標系の出力
  // 横軸のメモリ値
  textAlign(CENTER);  // 文字の座標指定位置
  text("now", Width, Height*1.08);  // 文字の描画
  j = -30;  // 横軸の目盛りの最低値
  for(int i=0; i<=5; i++)
  {
    text(j, Width/6*i, Height*1.08);  // 文字の描画
    j = j + 5;  // 横軸の目盛りの更新
  }
  text("time [s]", Width/2, Height*1.2);  // 横軸ラベルの描画
  // グラフの描画
  pushMatrix();
  translate(0, Height/2, 0);  // 座標の原点を縦軸の中心に移動
  if(data1>90)  // 枠からはみ出ないようにデータを修正 
  {
    data1 = 90;
  }
  else if(data1<-90)
  {
    data1 = -90;
  }
  if(data2>90)
  {
    data2 = 90;
  }
  else if(data2<-90)
  {
    data2 = -90;
  }
  if(data3>90)
  {
    data3 = 90;
  }
  else if(data3<-90)
  {
    data3 = -90;
  }
  if(ch=='r')  // ロール角の場合
  {
    PlotData1[299] = data1;  // データの更新
    PlotData2[299] = data2;
    PlotData3[299] = data3;
    for(int i=0; i<300-1; i++)  // plotするデータ処理
    {
      PlotData1[i] = PlotData1[i+1];
      PlotData2[i] = PlotData2[i+1];
      PlotData3[i] = PlotData3[i+1];
    }
    for(int i=0; i<300-1; i++)
    {
      stroke(255, 0, 0);  // 線の色
      line(Width/300*i, Height/6*PlotData1[i]/30, Width/300*(i+1), Height/6*PlotData1[i+1]/30);
      stroke(0, 255, 0);  // 線の色
      line(Width/300*i, Height/6*PlotData2[i]/30, Width/300*(i+1), Height/6*PlotData2[i+1]/30);
      stroke(0, 0, 255);  // 線の色
      line(Width/300*i, Height/6*PlotData3[i]/30, Width/300*(i+1), Height/6*PlotData3[i+1]/30);
    }
  }
  if(ch=='p')  // ピッチ角の場合
  {
    PlotData4[299] = data1;  // データの更新
    PlotData5[299] = data2;
    PlotData6[299] = data3;
    for(int i=0; i<300-1; i++)  // plotするデータ処理
    {
      PlotData4[i] = PlotData4[i+1];
      PlotData5[i] = PlotData5[i+1];
      PlotData6[i] = PlotData6[i+1];
    }
    for(int i=0; i<300-1; i++)
    {
      stroke(255, 0, 0);  // 線の色
      line(Width/300*i, Height/6*PlotData4[i]/30, Width/300*(i+1), Height/6*PlotData4[i+1]/30);
      stroke(0, 255, 0);  // 線の色
      line(Width/300*i, Height/6*PlotData5[i]/30, Width/300*(i+1), Height/6*PlotData5[i+1]/30);
      stroke(0, 0, 255);  // 線の色
      line(Width/300*i, Height/6*PlotData6[i]/30, Width/300*(i+1), Height/6*PlotData6[i+1]/30);
    }
  }
  popMatrix();  
  stroke(0);  // 線の色を戻す
  popMatrix();  // 保存した座標系の出力
}

void serialEvent(Serial port)
{
  // シリアルポートからデータを受け取ったら
  if (port.available() >=23) 
  {
    // シリアルデータ受信
    if(port.read()==1)
    {
      if(port.read()==200)
      {
        // 時間
        data[0] = port.read();
        data[1] = port.read();
        data[2] = port.read();
        // 加速度　φ
        data[3] = port.read();
        data[4] = port.read();
        data[5] = port.read();
        // 加速度　θ
        data[6] = port.read();
        data[7] = port.read();
        data[8] = port.read();
        // 角速度　φ
        data[9] = port.read();
        data[10] = port.read();
        data[11] = port.read();
        // 角速度　θ
        data[12] = port.read();
        data[13] = port.read();
        data[14] = port.read();
        // センサフュージョン　φ
        data[15] = port.read();
        data[16] = port.read();
        data[17] = port.read();
        // センサフュージョン　θ
        data[18] = port.read();
        data[19] = port.read();
        data[20] = port.read();
        time = data[0]*65536+data[1]*256+data[2];
        if(data[3]==0)
        {
          phiA = data[4]*256+data[5];
        }
        else
        {
          phiA = -1*(data[4]*256+data[5]);
        }
        if(data[6]==0)
        {
          thetaA = data[7]*256+data[8];
        }
        else
        {
          thetaA = -1*(data[7]*256+data[8]);
        }
        if(data[9]==0)
        {
          phiG = data[10]*256+data[11];
        }
        else
        {
          phiG = -1*(data[10]*256+data[11]);
        }
        if(data[12]==0)
        {
          thetaG = data[13]*256+data[14];
        }
        else
        {
          thetaG = -1*(data[13]*256+data[14]);
        }
        if(data[15]==0)
        {
          phi = data[16]*256+data[17];
        }
        else
        {
          phi = -1*(data[16]*256+data[17]);
        }
        if(data[18]==0)
        {
          theta = data[19]*256+data[20];
        }
        else
        {
          theta = -1*(data[19]*256+data[20]);
        }
      }
    }
  }
}

構想編では動画も表示するつもりだったのですが，それはまだできていません．

このプログラムの解説は他の記事で行っているので，このブログのProcessingのカテゴリから探してください．

　

Arduino側のプログラム

先程のProcessingのプログラムではデータの処理をする関数が最後にありました．

Processingでデータの処理をするには，処理できる形のデータをArduino側が送らなければなりません．

さらに，今回は姿勢角を表示するためセンサーのデータ処理もArduino側で行います．

それらのことを実行するプログラムは以下のようになります．

#include <Wire.h>
#include <math.h>

#define AG_DEVICE_ADDRESS 0x6B
#define M_DEVICE_ADDRESS  0x1E
#define AG_WHO_AM_I       0x0F
#define CTRL_REG1_G       0x10
#define CTRL_REG2_G       0x11
#define CTRL_REG3_G       0x12
#define ORIENT_CFG_G      0x13
#define OUT_T             0x15
#define STATUS_REG_T      0x17
#define OUT_G             0x18
#define CTRL_REG4         0x1E
#define CTRL_REG5_XL      0x1F
#define CTRL_REG6_XL      0x20
#define CTRL_REG7_XL      0x21
#define STATUS_REG_AG     0x27
#define OUT_A             0x28
#define M_WHO_AM_I        0x0F
#define CTRL_REG1_M       0x20
#define CTRL_REG2_M       0x21
#define CTRL_REG3_M       0x22
#define CTRL_REG4_M       0x23
#define CTRL_REG5_M       0x25
#define STATUS_REG_M      0x27
#define OUT_M             0x28

#define ACCELERATION_SENSITIVITY  0.000244  // scale 2g=0.061, 4g=0.122, 8g=0.244, 16g=0.732 mg/LSB
#define MAGNETIC_SENSITIVITY      0.00043   // scale 4=0.14, 8=0.29, 12=0.43, 16=0.58 mgauss/LSB
#define ANGULAR_SENSITIVITY       0.00875   // scale 245=8.75, 500=17.5, 2000=70 mdps/LSB
#define TEMPERATURE_SENSITIVITY   16        // LSB/℃


uint8_t ODR_G = 3;        // Output data rate: 1=14.9, 2=59.5, 3=119, 4=238, 5=476, 6=952　Hz
uint8_t FS_G = 0;         // Full scale: 0=245, 1=500, 2=no, 3=2000 dps
uint8_t BW_G = 0;         // cutoff frequency: table 47
uint8_t LP_mode = 0;      // low power mode: 0=disable, 1=enable
uint8_t HP_EN = 1;        // high pass filter: 0=disable, 1=enable
uint8_t HPCF_G = 1;       // high pass cutoff frequency: table 52
uint8_t Zen_G = 1;        // enable gyro Z:  0=disable, 1=enable
uint8_t Yen_G = 1;        // enable gyro Y:  0=disable, 1=enable
uint8_t Xen_G = 1;        // enable gyro X:  0=disable, 1=enable
uint8_t LIR_XL1 = 1;      // latch interrupt: 0=no request, 1=request
uint8_t FourD_XL1 = 0;
uint8_t SignX_G = 0;      // x axis sign: 0=positive, 1=negative
uint8_t SignY_G = 0;      // y axis sign: 0=positive, 1=negative
uint8_t SignZ_G = 0;      // z axis sign: 0=positive, 1=negative
uint8_t Orient = 0;
uint8_t DEC_ = 0;
uint8_t Zen_XL = 1;       // enable accel Z:  0=disable, 1=enable
uint8_t Yen_XL = 1;       // enable accel Y:  0=disable, 1=enable
uint8_t Xen_XL = 1;       // enable accel X:  0=disable, 1=enable
uint8_t ODR_XL = 3;       // Output date rate: 1=10, 2=50, 3=119, 4=238, 5=476, 6=952 Hz
uint8_t FS_XL = 3;        // Full scale: 0=2, 1=16, 2=4, 3=8 g
uint8_t BW_SCAL_ODR = 1;  // 0=band width determined by ODR, 1=determined by BW_XL
uint8_t BW_XL = 0;        // band width: 0=408, 1=211, 2=105, 3=50 Hz
uint8_t HR = 0;           // high resolution mode: 0=disable, 1=enable
uint8_t DCF = 0;          // low pass cutoff frequency: 0=ODR/50, 2=ODR/9,　1=ODR/100, 3=ODR/400
uint8_t FDS = 0;
uint8_t HPIS = 0;  
uint8_t TEMP_COMP = 0;    // temperature compensation: 0=disable, 1=enable
uint8_t OM = 0;           // operation mode: 0=continuous-conversion, 1=single-conversion,　2=power-down
uint8_t DO = 7;           // Output data rate: 0=0.625, 1=1.25, 2=2.5, 3=5, 4=10, 5=20, 6=40, 7=80 Hz
uint8_t FAST_ODR = 0;
uint8_t ST = 0;
uint8_t FS = 2;           // full scale: 0=4, 1=8, 2=12, 3=16 gauss
uint8_t REBOOT = 0;
uint8_t SOFT_RST = 0;
uint8_t I2C_DISABLE = 0;  // I2c interface: 0=enable, 1=disable
uint8_t LP = 0;           // low power mode: 0=disable, 1=enable
uint8_t SIM = 0;          // SPI interface
uint8_t MD = 0;           // operation mode: 0=continuous-conversion,　1=single-conversion,　2=power-down
uint8_t OMZ = 3;          // z axis operation mode: 0=low power, 1=medium, 2=high, 3=ultra-high performance
uint8_t BLE = 0;
uint8_t FAST_READ = 0;
uint8_t BDU = 0;

int16_t AccX, AccY, AccZ;
float Ax, Ay, Az;
int16_t GyrX, GyrY, GyrZ;
float Gx, Gy, Gz;
int16_t MagX, MagY, MagZ;
float Mx, My, Mz;
int16_t Temp;
float Te;

double phiA, thetaA;
double phig, thetag, psig;
double phiG, thetaG, psiG;
double phi, theta, psi;

const double pi = 3.14;

uint8_t I2Cread1byte(uint8_t DeviceAddress, uint8_t SlaveAddress)
{
  Wire.beginTransmission(DeviceAddress); // 通信を開始する
  Wire.write(SlaveAddress);             // レジスターアドレスを送信
  Wire.endTransmission(false);  // リスタートメッセージをリクエストの後に送信し，接続を続ける
  Wire.requestFrom(DeviceAddress, (uint8_t) 1);  // 1 byte分のデータを要求する
  return Wire.read();         // データを返す
}

uint8_t I2CreadMULTIbyte(uint8_t DeviceAddress, uint8_t SlaveAddress, uint8_t num, uint8_t * data)
{
  Wire.beginTransmission(DeviceAddress);
  Wire.write(SlaveAddress|0x80);
  Wire.endTransmission(false);
  Wire.requestFrom(DeviceAddress, num);
  for (uint8_t i=0; i<num; i++)
  {
    data[i] = Wire.read();
  }
  return num;
}

void I2Cwrite1byte(uint8_t DeviceAddress, uint8_t SlaveAddress, uint8_t data)
{
  Wire.beginTransmission(DeviceAddress); // 通信を開始する
  Wire.write(SlaveAddress);             // レジスターアドレスを送信
  Wire.write(data);             // レジスターアドレスを送信
  Wire.endTransmission();       // 通信を終了する
}

bool LSM9DS1setting()
{
  uint8_t who_ag, who_m;

  uint8_t RegisterData = 0;

  RegisterData=ODR_G<<5;
  RegisterData|=FS_G<<3;
  RegisterData|=BW_G;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG1_G, RegisterData);
  RegisterData=0;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG2_G, RegisterData);
  RegisterData=LP_mode<<7;
  RegisterData|=HP_EN<<6;
  RegisterData|=HPCF_G;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG3_G, RegisterData);
  RegisterData=0;
  RegisterData=Zen_G<<5;
  RegisterData|=Yen_G<<4;
  RegisterData|=Xen_G<<3;
  RegisterData|=LIR_XL1<<2;
  RegisterData|=FourD_XL1;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG4, RegisterData);
  RegisterData=0;
  RegisterData=SignX_G<<5;
  RegisterData|=SignY_G<<4;
  RegisterData|=SignZ_G<<3;
  RegisterData|=Orient;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, ORIENT_CFG_G, RegisterData);
  RegisterData=0;
  RegisterData=DEC_<<6;
  RegisterData|=Zen_XL<<5;
  RegisterData|=Yen_XL<<4;
  RegisterData|=Xen_XL<<3;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG5_XL, RegisterData);
  RegisterData=0;
  RegisterData=ODR_XL<<5;
  RegisterData|=FS_XL<<3;
  RegisterData|=BW_SCAL_ODR<<2;
  RegisterData|=BW_XL;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG6_XL, RegisterData);
  RegisterData=0;
  RegisterData=HR<<7;
  RegisterData|=DCF<<5;
  RegisterData|=FDS<<2;
  RegisterData|=HPIS;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG7_XL, RegisterData);
  RegisterData=0;
  RegisterData=TEMP_COMP<<7;
  RegisterData|=OM<<5;
  RegisterData|=DO<<2;
  RegisterData|=FAST_ODR<<1;
  RegisterData|=ST;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG1_M, RegisterData);
  RegisterData=0;
  RegisterData=FS<<5;
  RegisterData|=REBOOT<<3;
  RegisterData|=SOFT_RST<<2;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG2_M, RegisterData);
  RegisterData=0;
  RegisterData=I2C_DISABLE<<7;
  RegisterData|=LP<<5;
  RegisterData|=SIM<<2;
  RegisterData|=MD;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG3_M, RegisterData);
  RegisterData=0;
  RegisterData=OMZ<<2;
  RegisterData|=BLE<<1;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG4_M, RegisterData);
  RegisterData=0;
  RegisterData=FAST_READ<<7;
  RegisterData|=BLE<<6;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG5_M, RegisterData);


  who_ag = I2Cread1byte(AG_DEVICE_ADDRESS, AG_WHO_AM_I);
  who_m = I2Cread1byte(M_DEVICE_ADDRESS, M_WHO_AM_I);
  if (who_ag==0x68&&who_m==0x3D)
  {
    return true;
  }
  else
  {
    return false;
  }
}

void GetAccel()
{
  uint8_t data[6];
  if(I2Cread1byte(AG_DEVICE_ADDRESS, STATUS_REG_AG)&1)  // データの更新確認
  {
    if(I2CreadMULTIbyte(AG_DEVICE_ADDRESS, OUT_A, 6, data)==6)
    {
      AccX = ((data[1]<<8)|data[0]);
      AccY = ((data[3]<<8)|data[2]);
      AccZ = ((data[5]<<8)|data[4]);
    }
  }
}

void GetGyro()
{
  uint8_t data[6];
  if(I2Cread1byte(AG_DEVICE_ADDRESS, STATUS_REG_AG)&2)  // データの更新確認
  {
    if(I2CreadMULTIbyte(AG_DEVICE_ADDRESS, OUT_G, 6, data)==6)
    {
      GyrX = (data[1]<<8)|data[0];
      GyrY = (data[3]<<8)|data[2];
      GyrZ = (data[5]<<8)|data[4];
    }
  }
}

void GetTemp()
{
  uint8_t data[6];
  if(I2Cread1byte(AG_DEVICE_ADDRESS, STATUS_REG_AG)&4)  // データの更新確認
  {
    if(I2CreadMULTIbyte(AG_DEVICE_ADDRESS, OUT_T, 2, data)==2)
    {
      Temp = (data[1]<<8)|data[0];
    }
  }
}

void GetMag()
{
  uint8_t data[6];
  if(I2Cread1byte(M_DEVICE_ADDRESS, STATUS_REG_M)&8)  // データの更新確認
  {
    if(I2CreadMULTIbyte(M_DEVICE_ADDRESS, OUT_M, 6, data)==6)
    {
      MagX = (data[1]<<8)|data[0];
      MagY = (data[3]<<8)|data[2];
      MagZ = (data[5]<<8)|data[4];
    }
  }
}

void DataSensitivity()
{
  Ax = AccZ*ACCELERATION_SENSITIVITY;
  Ay = AccY*ACCELERATION_SENSITIVITY;
  Az = -AccX*ACCELERATION_SENSITIVITY;
  Gx = GyrZ*ANGULAR_SENSITIVITY*PI/180;
  Gy = GyrY*ANGULAR_SENSITIVITY*PI/180;
  Gz = -GyrX*ANGULAR_SENSITIVITY*PI/180;
}

void CalcAngle()
{
  phiA = atan2(-Ay,Az);
  thetaA = atan2(Ax,sqrt(pow(Ay,2)+pow(Az,2)));
  phig = phig+(Gx+sin(phig)*tan(thetag)*Gy+cos(phig)*tan(thetag)*Gz)*0.1;
  thetag = thetag+(cos(phig)*Gy-sin(phig)*Gz)*0.1;
  psig = psig+(sin(phig)/cos(thetag)*Gy+cos(phig)/cos(thetag)*Gz)*0.1;
  phiG = phi+(Gx+sin(phi)*tan(theta)*Gy+cos(phi)*tan(theta)*Gz)*0.1;
  thetaG = theta+(cos(phi)*Gy-sin(phi)*Gz)*0.1;
  psiG = psi+(sin(phi)/cos(theta)*Gy+cos(phi)/cos(theta)*Gz)*0.1;
  phi = 0.95*phiG+0.05*phiA;
  theta = 0.95*thetaG+0.05*thetaA;
  psi = 1.00*psiG;
}

void setup() {
  Serial.begin(9600); // シリアル通信のレートを設定
  Wire.begin();         // I2C通信の初期化

  if(LSM9DS1setting())
  {
    Serial.println("LSM9DS1 setting success");
  }
  else
  {
    Serial.println("LSM9DS1 setting failure");
  }

  TCCR1A = 0; // 初期化
  TCCR1B = 0; // 初期化
  OCR1A = 6250;
  OCR1B = 3125;
  TCCR1B |= (1 << CS12) | (1 << WGM12);
  TIMSK1 |= (1 << OCIE1A) | (1 << OCIE1B); 
}

ISR(TIMER1_COMPA_vect) {
  DataSensitivity();
  CalcAngle();
//  Serial.print(millis());
//  Serial.print(", ");
//  Serial.print((int)(phi/pi*180));
//  Serial.print(", ");
//  Serial.println((int)(theta/pi*180));
  
  // ダミーデータの送信
  Serial.write((byte)(1));
  Serial.write((byte)(200));
  // 経過時間を送信(3 byte)
  Serial.write((byte)(millis()>>16));
  Serial.write((byte)(millis()>>8));
  Serial.write((byte)(millis()));
  // 角度の送信(2 byte)
  // 加速度から算出
  if(phiA>=0)
  {
    Serial.write(0);
    Serial.write((byte)((int)(phiA/pi*180)>>8));
    Serial.write((byte)((int)(phiA/pi*180)));
  }
  else if(phiA<0)
  {
    Serial.write(1);
    Serial.write((byte)((int)(phiA/pi*180*(-1))>>8));
    Serial.write((byte)((int)(phiA/pi*180*(-1))));
  }
  if(thetaA>=0)
  {
    Serial.write(0);
    Serial.write((byte)((int)(thetaA/pi*180)>>8));
    Serial.write((byte)((int)(thetaA/pi*180)));
  }
  else if(thetaA<0)
  {
    Serial.write(1);
    Serial.write((byte)((int)(thetaA/pi*180*(-1))>>8));
    Serial.write((byte)((int)(thetaA/pi*180*(-1))));
  }
  // 角速度から算出
  if(phig>=0)
  {
    Serial.write(0);
    Serial.write((byte)((int)(phig/pi*180)>>8));
    Serial.write((byte)((int)(phig/pi*180)));
  }
  else if(phig<0)
  {
    Serial.write(1);
    Serial.write((byte)((int)(phig/pi*180*(-1))>>8));
    Serial.write((byte)((int)(phig/pi*180*(-1))));
  }
  if(thetag>=0)
  {
    Serial.write(0);
    Serial.write((byte)((int)(thetag/pi*180)>>8));
    Serial.write((byte)((int)(thetag/pi*180)));
  }
  else if(thetag<0)
  {
    Serial.write(1);
    Serial.write((byte)((int)(thetag/pi*180*(-1))>>8));
    Serial.write((byte)((int)(thetag/pi*180*(-1))));
  }
  // センサフュージョンから算出
  if(phi>=0)
  {
    Serial.write(0);
    Serial.write((byte)((int)(phi/pi*180)>>8));
    Serial.write((byte)((int)(phi/pi*180)));
  }
  else if(phi<0)
  {
    Serial.write(1);
    Serial.write((byte)((int)(phi/pi*180*(-1))>>8));
    Serial.write((byte)((int)(phi/pi*180*(-1))));
  }
  if(theta>=0)
  {
    Serial.write(0);
    Serial.write((byte)((int)(theta/pi*180)>>8));
    Serial.write((byte)((int)(theta/pi*180)));
  }
  else if(theta<0)
  {
    Serial.write(1);
    Serial.write((byte)((int)(theta/pi*180*(-1))>>8));
    Serial.write((byte)((int)(theta/pi*180*(-1))));
  }
}

ISR(TIMER1_COMPB_vect) {
}

void loop() {
  GetAccel();
  GetGyro();
}

　

電子回路

これらのプログラムを実行するために，使用する電子回路はこちらの記事で詳細に解説しています．

回路図や使用しているセンサーなどが書いてありますので，センサーなどを持っている場合は自分のセンサーが使用できるようにプログラムを書き換えてください．

もし，センサーなどを持っていない場合は同じものを使用すれば全く同じプログラムで動作するので試してみてください．

　

実行結果

このプログラムを実行した結果，以下のようになります．

実行直後はこのようになり，左上にロール角の時間履歴が表示され，その右隣にピッチ角の時間履歴が表示されます．

その下にはデータ処理方法ごとに姿勢を表す直方体が表示されます．

実際にセンサーの傾きを変えると，このように時系列グラフや直方体から姿勢角の変化が読み取れます．

　

まとめ

この記事では，姿勢角の算出方法ごとにデータをグラフなどで表示して，比較をするアニメーションのプログラムをご紹介しました．

　

続けて読む

この記事の続きは以下のリンクから読むことができます．

以下の記事では今回公開したプログラムを使用できる電子回路の作成を行っています．

興味のある方は続けて参考にしてください．

Processingでロボットの姿勢角をリアルタイムで表示する(電子回路作成編）
みなさん，こんにちはおかしょです． ロボットの姿勢角を求めるには加速度センサーやジャイロセンサーが必要です． このブログでは様々なプロジェクトを実行していますが，このプロジェクトでは違う方法で姿勢角を求めた時にどのように結果が異なるのかを比...
okasho-engineer.com
2020.08.29

Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．

それでは最後まで読んでいただきありがとうございました．

コメント

タイトルとURLをコピーしました