Arduinoを使用して、ジャイロセンサMPU6050の角度変化をProcessingで表現したいです。

Question

Arduinoを使用して、ジャイロセンサMPU6050の角度変化をProcessingで表現したいです。

■現在の状況
正確な角度がProcessingの動きに反映されない
小刻みにモデルが動く(delayやxGyroValueの閾値を入れるが完ぺきには改善されません)

■Arduino
Arduinoから以下信号をI2CでProcessingに送っております。
xGyroValue = Roll
yGyroValue = Pitch
zGyroValue = Yaw

■Processing出力結果

■Processing

import processing.serial.*; 

 int lf = 10;
 Serial myPort;
 String datline = null; 
 PrintWriter file;

float xGyroValueLast = 0;
float yGyroValueLast = 0;
float zGyroValueLast = 0;

float xGyroValue;
float yGyroValue;
float zGyroValue; 

float xGyroAngleValue1 = 0;
float yGyroAngleValue1 = 0;
float zGyroAngleValue1 = 0;
float xGyroAngleValue = 0;
float yGyroAngleValue = 0;
float zGyroAngleValue = 0;

float time = 0;
 
void setup(){ 
 println(Serial.list());
 frameRate(20);
 noStroke();  
 file = createWriter("xGyroValue.csv");

myPort = new Serial(this, "COM3", 115200);
myPort.clear();
datline = myPort.readStringUntil(lf);
datline = null;
 } 

void settings(){
 size(1200, 600, P3D);
 } 

void draw() {
 background(0); 
 pushMatrix(); 
 translate(width / 2, height / 2, -30); 

while( myPort.available() > 0) { 
 datline = myPort.readStringUntil(lf);
 if(datline != null){
   println(datline); 

float[] splitdata = float(datline.split(","));
if(splitdata.length == 4) {
 time = splitdata[0];
 xGyroValue = splitdata[1];
 yGyroValue = splitdata[2];
 zGyroValue = splitdata[3]; 
}
  
  xGyroAngleValue1 = xGyroValueLast - xGyroValue;
  yGyroAngleValue1 = yGyroValueLast - yGyroValue;
  zGyroAngleValue1 = zGyroValueLast - zGyroValue;
 
  xGyroValueLast = xGyroValue;
  yGyroValueLast = yGyroValue;
  zGyroValueLast = zGyroValue;
  
  for(int i = 0; i < 50; i++){ //50回センサ値を読み込んで平均を算
    xGyroAngleValue = xGyroAngleValue + xGyroAngleValue1;
    yGyroAngleValue = yGyroAngleValue + yGyroAngleValue1;
    zGyroAngleValue = zGyroAngleValue + zGyroAngleValue1;
    }
    xGyroAngleValue = xGyroAngleValue / 50;
    yGyroAngleValue = yGyroAngleValue / 50;
    zGyroAngleValue = zGyroAngleValue / 50;
 
    if(xGyroValue < 1){
      xGyroValue = 0;
    }
    
    if(yGyroValue < 1){
      yGyroValue = 0;
    }
      
    if(zGyroValue < 1){
      zGyroValue = 0;
    }
      
 println(time);
 println(xGyroValue);
 println(yGyroValue);
 println(zGyroValue);

//file.print(time);file.print(",");file.print(xGyroAngleValue);file.print(",");file.print(yGyroAngleValue);file.print(",");file.println(zGyroAngleValue);

 }
}
    rotateX(xGyroAngleValue);
    rotateY(yGyroAngleValue);
    rotateZ(zGyroAngleValue);

scale(100);
beginShape(QUADS); 

fill(255, 0, 0);
 vertex(-3, 0.5, 0.5);
 vertex( 3, 0.5, 0.5);
 vertex( 3, -0.5, 0.5);
 vertex(-3, -0.5, 0.5); 

fill(0, 255, 0); 
 vertex( 3, 0.5, 0.5);
 vertex( 3, 0.5, -0.5);
 vertex( 3, -0.5, -0.5);
 vertex( 3, -0.5, 0.5); 

fill(0, 0, 255); 
 vertex( 3, 0.5, -0.5);
 vertex(-3, 0.5, -0.5);
 vertex(-3, -0.5, -0.5);
 vertex( 3, -0.5, -0.5); 

fill(255, 255, 0); 
 vertex(-3, 0.5, -0.5);
 vertex(-3, 0.5, 0.5);
 vertex(-3, -0.5, 0.5);
 vertex(-3, -0.5, -0.5); 

fill(0, 255, 255); 
 vertex(-3, 0.5, -0.5);
 vertex( 3, 0.5, -0.5);
 vertex( 3, 0.5, 0.5);
 vertex(-3, 0.5, 0.5); 

fill(255, 0, 255); 
 vertex(-3, -0.5, -0.5);
 vertex( 3, -0.5, -0.5);
 vertex( 3, -0.5, 0.5);
 vertex(-3, -0.5, 0.5); 

endShape();
popMatrix();
delay(100);
 }
 
 void keyPressed(){
  if (key == ENTER) {
       file.flush();
       file.close();
       exit();
     }
}

Comments

[y_waiwai]

このままではコードが見づらいので、質門を編集し、<code>ボタンで、出てくる’’’の枠の中にコードを貼り付けてください

Answer 1

ジャイロで得られるのは回転の加速度です。
角度に変換するためには積分しなきゃいけないはずですが、そういうプログラムになっていないように見えます。また、スケーリングもめちゃくちゃでは。

私はMPU9250しか持っていないので確認は出来ませんが、MPU6050で検索すればサンプルプログラムは見つかると思います。ありがちなArduino(C++)で書かれていてもProcessing(Java)に書き換えるのはそんなに難しくないと思いますので、確認されてみては。

Answer 2

どういうフォーマットでシリアルで送ってるかわかりませんが、
想定通りに受信されてるでしょうか。そこらへんをまずチェックしましょう

＃想定どおりに受信されてるならあとはデータの取扱の問題

---

for(int i = 0; i < 50; i++){ //50回センサ値を読み込んで平均を算

xGyroAngleValue = xGyroAngleValue + xGyroAngleValue1;
yGyroAngleValue = yGyroAngleValue + yGyroAngleValue1;
zGyroAngleValue = zGyroAngleValue + zGyroAngleValue1;
}

これ、単に同じ値を５０回足してるだけで、平均にもなってないですね
また
xGyroAngleValue、yGyroAngleValue、zGyroAngleValue の初期値はどうなってるねん、という問題もありますねー

Comments

[msmsmsmsmsms]

データは想定通り受信しております。
そのデータを使用してProcessingで表現するところに苦心しております。

[y_waiwai]

んでは、
> println(time);
> println(xGyroValue);
> println(yGyroValue);
> println(zGyroValue);
はきちんと受信データ通りの数値が出ている？

[msmsmsmsmsms]

Processing受信データを追記いたしました。
おかしな点や足りない点がありましたらご指摘いただけないでしょうか?