みなさん，こんにちは
おかしょです．
約2か月ほど前から始動していたキャリブレーションロボットのプロジェクトがついに終了したので，その完成形をご紹介します．
この記事を読むと以下のようなことがわかる・できるようになります．
- キャリブレーションロボットの製作過程
- キャリブレーションロボットとは
- キャリブレーションロボットの性能
この記事を読む前に
この記事は以下のキャリブレーションロボットの動作確認の続きとなっています．
まだ読んでいない方は以下の記事を先に読んでおくことをおすすめします．
キャリブレーションロボットとは
このプロジェクトの完成編ということで，まずはプロジェクトの振り返りをしていきたいと思います．
何かのセンサーを使う際に，測定精度をよくするためにキャリブレーションが必要なセンサーがあります．今回の場合は地磁気センサーを対象としています．
地磁気センサーの場合はセンサーの各軸方向で地磁気を測定するのですが，本来は測定した値の中心は0でなければなりません．
しかし，周りの環境などの影響で値の中心がずれてしまっています．それを修正するためにキャリブレーションという修正をする必要があります．
地磁気センサーのキャリブレーションはセンサーの各軸をあらゆる方向に向けて，時期を取得することで完了します．
このとき，センサーを向ける向きに偏りがあってはいけません．そのため，センサーの各軸をいろんな方向に向けるためにぐるぐる回転させる必要があります．
この作業が非常に面倒くさいです．
ぐるぐる回したところで本当に偏りがないのかもわからないですし，そもそも回転させるだけで一苦労です．
そこで，このキャリブレーションを自動で行うロボットを作ろうと思いました．
そうと決まれば，まずはロボットの構想から考えていきます．
この段階で，ロボットをどういう形にするのか，どのような電子部品を使用するのかなどを決めます．
そして使用する電子部品などが決まったら，その電子部品で目的の動きを達成できるのかを確認するために，プログラムを書きます．
使用する電子部品が確定したら機体の設計を行い，設計図の通りに作成をします．作成時は失敗することもあり，設計をもっと工夫してもう一度作成しました．
そして，機体ができたら導線などを繋げて動作確認を行います．
そして，最後に実際に使用することを考えて，使用環境を整えたら完成となります．
この記事では，実際に使用するときのことを考えたProcessingによるデータのビジュアル化について解説します．
Processingによるビジュアル化
Processingでアニメーションを作るには，まずは構想をしっかりと練ります．
人によっては書きながら考える人もいるかもしれませんが，私はしっかりと構想を練ってからプログラムを書き始めます．
構想
まず，表示したいのは地磁気のデータです．
この地磁気のデータを座標として3次元にプロットをしていくと球状になります．これをProcessingで表示したいと思います．
もちろんリアルタイムでデータを表示するので，どんどんプロットされるデータが増えていき球状になっていく様を見たいです．
今回はマイコンにArduino Unoを使用します．ProcessingとArduino Unoをシリアル通信をする方法はこちらで解説しています．
ただプロットしていくだけでは3次元に見にくいと思うので，座標軸や平面も表示します．座標軸は各軸ごとに色分けして表示し，xy平面がわかるようにメッシュ表示します．
座標軸の表示で使用する矢印の書き方はこちらの記事で解説しています．
さらに，数値的なデータも画面右上に表示できるようにします．数値で表示する場合は2次元の文字で表示する必要があるので，3次元の図形と2次元の文字を同時に表示する必要があります．
3次元のアニメーションに2次元の文字や図形を描く方法はこちらで解説しています．
また，データン中心点はキャリブレーションが終了するまでわかりません．そのため，視点をどこに置けばいいのか事前にはわからないのでリアルタイムで視点の変更も行えるようにします．マウスで変更するよりもキーボードで変更した方がやりやすそうなのでキーボードで視点を変更できるようにします．
視点の変更をキーボードで行うプログラムはこちらで公開しています．
また，視点の変更に対応して2次元の文字や図形を表示する方法はこちらで解説しています．
データをビジュアル化するプログラム
あとは構想の通りにプログラムを書くのみです．
実際に使用したプログラムは以下になります．
Processingのプログラム
import processing.opengl.*;
import processing.serial.*;
Serial port;
int[] data = new int[9];
float cameraX, cameraY, cameraZ;
int[][] mag;
int MagX, MagY, MagZ;
int[] MagMax = new int[3];
int[] MagMin = new int[3];
int j = 0;
int num = 10000;
int fin = 0;
void setup()
{
port = new Serial(this, Serial.list()[0], 9600); // Serial port設定
frameRate(10); // frame rateを5fpsに設定
background(0); // 背景の色指定 黒に指定
size(displayWidth,displayHeight, OPENGL); // windowのサイズを画面いっぱいにする
translate(width/2, height/2, 0); // 原点を画面の中心に
cameraX = 2000.0;
cameraY = 2000.0;
cameraZ = 2000.0;
camera(cameraX, cameraY, cameraZ, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0);
arrow(0, 0, 0, 1000, 0, 0, 150, 0, 0);
arrow(0, 0, 0, 0, 1000, 0, 0, 150, 0);
arrow(0, 0, 0, 0, 0, 1000, 0, 0, 150);
mag = new int[num][3];
}
void draw()
{
background(0); // 背景の色指定 黒に指定
if (keyPressed) {
if (keyCode == LEFT) cameraX -= 100;
if (keyCode == RIGHT) cameraX += 100;
if (keyCode == UP) cameraY -= 100;
if (keyCode == DOWN) cameraY += 100;
if (keyCode == CONTROL) cameraZ -= 100;
if (keyCode == SHIFT) cameraZ += 100;
if (keyCode == ENTER) fin = 1;
}
camera(cameraX, cameraY, cameraZ, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0);
stroke(50);
for(int i=-2000; i<=2000;)
{
line(-2000, i, 0, 2000, i, 0);
line(i, -2000, 0, i, 2000, 0);
i += 100;
}
arrow(0, 0, 0, 1000, 0, 0, 150, 0, 0);
arrow(0, 0, 0, 0, 1000, 0, 0, 150, 0);
arrow(0, 0, 0, 0, 0, 1000, 0, 0, 150);
// データのプロット
if((j<num) && (fin==0))
{
mag[j][0] = MagX;
mag[j][1] = MagY;
mag[j][2] = MagZ;
strokeWeight(5);
stroke(150, 150, 0);
for(int k=0; k<=j; k++)
{
if(k==j)
{
strokeWeight(10);
stroke(255, 255, 0);
}
point(mag[k][0], mag[k][1], mag[k][2]);
}
j = j+1;
}
else if((j>=num) || (fin == 1))
{
MagMax[0] = MagX;
MagMin[0] = MagX;
MagMax[1] = MagY;
MagMin[1] = MagY;
MagMax[2] = MagZ;
MagMin[2] = MagZ;
strokeWeight(10);
stroke(150, 150, 0);
for(int k=0; k<=j-1; k++)
{
point(mag[k][0], mag[k][1], mag[k][2]);
if(MagMax[0]>mag[k][0]) MagMax[0] = mag[k][0];
if(MagMax[1]>mag[k][1]) MagMax[1] = mag[k][1];
if(MagMax[2]>mag[k][2]) MagMax[2] = mag[k][2];
if(MagMin[0]<mag[k][0]) MagMin[0] = mag[k][0];
if(MagMin[1]<mag[k][1]) MagMin[1] = mag[k][1];
if(MagMin[2]<mag[k][2]) MagMin[2] = mag[k][2];
}
stroke(255, 0, 0);
point((MagMax[0]+MagMin[0])/2, (MagMax[1]+MagMin[1])/2, (MagMax[2]+MagMin[2])/2);
}
strokeWeight(1);
// 2次元描画
pushMatrix(); // 座標系の保存
rotateZ(-atan2(cameraX, cameraY)); // 視点の位置に対応する
rotateX(-atan2(sqrt(sq(cameraX)+sq(cameraY)), cameraZ));
translate(0, 0, sqrt(sq(cameraX)+sq(cameraY)+sq(cameraZ))-250);
hint(DISABLE_DEPTH_TEST); // z軸を無効化
fill(255);
textSize(10); // 文字の大きさ
textAlign(LEFT); // 文字の座標指定位置
text("Mag", (width/6)/5*3/2, -(height*290/1071)/5*3/2);
textAlign(RIGHT); // 文字の座標指定位置
text("x :", (width/6)*40/100, -(height*290/1071)/5*3/2+20);
text("y :", (width/6)*40/100, -(height*290/1071)/5*3/2+35);
text("z :", (width/6)*40/100, -(height*290/1071)/5*3/2+50);
if(fin==1)
{
fill(255, 0, 0);
text("zero x:", (width/6)*40/100, -(height*290/1071)/5*3/2+70);
text("zero y:", (width/6)*40/100, -(height*290/1071)/5*3/2+85);
text("zero z:", (width/6)*40/100, -(height*290/1071)/5*3/2+100);
}
// データの表示
fill(255);
textAlign(LEFT); // 文字の座標指定位置
text(MagX, (width/6)*40/100, -(height*290/1071)/5*3/2+20);
text(MagY, (width/6)*40/100, -(height*290/1071)/5*3/2+35);
text(MagZ, (width/6)*40/100, -(height*290/1071)/5*3/2+50);
if(fin==1)
{
fill(255, 0, 0);
text((MagMax[0]+MagMin[0])/2, (width/6)*40/100, -(height*290/1071)/5*3/2+70);
text((MagMax[1]+MagMin[1])/2, (width/6)*40/100, -(height*290/1071)/5*3/2+85);
text((MagMax[2]+MagMin[2])/2, (width/6)*40/100, -(height*290/1071)/5*3/2+100);
}
hint(ENABLE_DEPTH_TEST); // z軸を有効化
popMatrix(); // 保存した座標系の出力
}
void cone(int L, float radius, int Color1, int Color2, int Color3)
{
float x, y;
noStroke();
fill(Color1, Color2, Color3);
beginShape(TRIANGLE_FAN); // 底面の円の作成
vertex(0, 0, -L);
for(float i=0; i<2*PI; )
{
x = radius*cos(i);
y = radius*sin(i);
vertex(x, y, -L);
i = i+0.01;
}
endShape(CLOSE);
beginShape(TRIANGLE_FAN); // 側面の作成
vertex(0, 0, 0);
for(float i=0; i<2*PI; )
{
x = radius*cos(i);
y = radius*sin(i);
vertex(x, y, -L);
i = i+0.01;
}
endShape(CLOSE);
}
void arrow(int x1, int y1, int z1, int x2, int y2, int z2, int Color1, int Color2, int Color3)
{
int arrowLength = 100;
float arrowAngle = 0.5;
float phi = -atan2(y2-y1, x2-x1);
float theta = PI/2-atan2(z2-z1, x2-x1);
stroke(Color1, Color2, Color3);
strokeWeight(4);
line(x1, y1, z1, x2, y2, z2);
strokeWeight(1);
pushMatrix();
translate(x2, y2, z2);
rotateY(theta);
rotateX(phi);
cone(arrowLength, arrowLength*sin(arrowAngle), Color1, Color2, Color3);
popMatrix();
}
void serialEvent(Serial port)
{
// シリアルポートからデータを受け取ったら
if (port.available() >=11)
{
// シリアルデータ受信
if(port.read()==1)
{
if(port.read()==200)
{
// MagX
data[0] = port.read();
data[1] = port.read();
data[2] = port.read();
// MagY
data[3] = port.read();
data[4] = port.read();
data[5] = port.read();
// MagZ
data[6] = port.read();
data[7] = port.read();
data[8] = port.read();
if(data[0]==0)
{
MagX = data[1]*256+data[2];
}
else
{
MagX = -1*(data[1]*256+data[2]);
}
if(data[3]==0)
{
MagY = data[4]*256+data[5];
}
else
{
MagY = -1*(data[4]*256+data[5]);
}
if(data[6]==0)
{
MagZ = data[7]*256+data[8];
}
else
{
MagZ = -1*(data[7]*256+data[8]);
}
}
}
}
}
Arduinoサーボ制御用プログラム
また，以前公開したArduinoのプログラムではx軸の回転が速すぎたので，x軸のゲインを少し変更しています．それが以下のプログラムになります．
#include <Servo.h>
Servo servo;
#define OutputPin1 2
#define OutputPin2 3
#define OutputPin3 4
#define InputPin1 5
#define InputPin2 6
#define InputPin3 7
Servo servoX;
Servo servoY;
Servo servoZ;
float phi = 0;
float theta = 0;
float psi = 0;
float phiP = 0;
float thetaP = 0;
float psiP = 0;
bool turn1 = true;
bool turn2 = true;
bool turn3 = true;
unsigned long tHigh1, tLow1, tLowS1, tLowF1;
unsigned long tHigh2, tLow2, tLowS2, tLowF2;
unsigned long tHigh3, tLow3, tLowS3, tLowF3;
int count1 = 0;
int count2 = 0;
int count3 = 0;
int SWITCH = 1;
float angle = 0.0;
float angle1 = 0.0;
float angle2 = 0.0;
float angle3 = 0.0;
int Kp = 1;
int ch1 = 0;
int ch2 = 0;
int ch3 = 0;
float dAngle = 30.0;
void setup()
{
Serial.begin(9600);
pinMode(InputPin1, INPUT);
pinMode(InputPin2, INPUT);
pinMode(InputPin3, INPUT);
servoX.attach(OutputPin1);
servoY.attach(OutputPin2);
servoZ.attach(OutputPin3);
tLowS1 = micros();
tLowF1 = micros();
tHigh1 = 0;
tLow1 = 0;
tLowS2 = micros();
tLowF2 = micros();
tHigh2 = 0;
tLow2 = 0;
tLowS3 = micros();
tLowF3 = micros();
tHigh3 = 0;
tLow3 = 0;
}
void loop()
{
if(SWITCH==1)
{
if(ch1==0)
{
angle1 = 0.0;
}
else if(ch1==1)
{
angle1 = 360.0;
}
else if(ch1==-1)
{
angle1 = 0.0;
}
angle = angle1;
// servo.attach(OutputPin1);
/////////////// servo1 ///////////////////////
if(digitalRead(InputPin1)==LOW && turn1)
{
tLowS1 = micros();
tHigh1 = tLowS1-tLowF1;
turn1 = false;
}
else if(digitalRead(InputPin1) && turn1==false)
{
tLowF1 = micros();
tLow1 = tLowF1-tLowS1;
unsigned long t = tHigh1+tLow1;
if(t>1000 && t<1200)
{
phiP = phi-360*count1;
float duty = 100*tHigh1/(tHigh1+tLow1);
phi = (duty-2.9)/(97.1-2.9)*360;
if(phiP>270 && phi <90)
{
count1++;
}
else if(phiP<90 && phi>270)
{
count1--;
}
phi = 360*count1+phi;
Serial.print(phi);
Serial.print(", ");
Serial.print(theta);
Serial.print(", ");
Serial.println(psi);
float error = angle - phi;
int u = (int)(-1*0.3*(int)(error));
if(u>200)
{
u = 200;
}
else if(u<-200)
{
u = -200;
}
if(u>0)
{
u = u+40;
}
else if(u<0)
{
u = u-40;
}
if(error>-5.0 && error<5.0)
{
servoX.writeMicroseconds(1500);
if(ch1==0)
{
SWITCH =2;
ch1 = 1;
}
else if(ch1==1)
{
SWITCH =2;
ch1 = -1;
}
else if(ch1==-1)
{
SWITCH =2;
ch1 = 1;
}
}
else
{
servoX.writeMicroseconds(1500+u);
}
}
phi = phi;
turn1 = true;
}
}
if(SWITCH==2)
{
angle = angle2;
// servo.attach(OutputPin2);
/////////////// servo2 ///////////////////////
if(digitalRead(InputPin2)==LOW && turn2)
{
tLowS2 = micros();
tHigh2 = tLowS2-tLowF2;
turn2 = false;
}
else if(digitalRead(InputPin2) && turn2==false)
{
tLowF2 = micros();
tLow2 = tLowF2-tLowS2;
unsigned long t = tHigh2+tLow2;
if(t>1000 && t<1200)
{
thetaP = theta-360*count2;
float duty = 100*tHigh2/(tHigh2+tLow2);
theta = (duty-2.9)/(97.1-2.9)*360;
if(thetaP>270 && theta <90)
{
count2++;
}
else if(thetaP<90 && theta>270)
{
count2--;
}
theta = 360*count2+theta;
Serial.print(phi);
Serial.print(", ");
Serial.print(theta);
Serial.print(", ");
Serial.println(psi);
float error = angle - theta;
int u = -1*Kp*(int)(error);
if(u>200)
{
u = 200;
}
else if(u<-200)
{
u = -200;
}
if(u>0)
{
u = u+40;
}
else if(u<0)
{
u = u-40;
}
if(error>-5.0 && error<5.0)
{
servoY.writeMicroseconds(1500);
if(ch2==0)
{
angle2 = angle2+dAngle;
ch2 = 1;
SWITCH =3;
}
else if(ch2==1)
{
if(angle2>=360)
{
angle2 = angle2-dAngle;
ch2 = -1;
SWITCH = 3;
}
else
{
angle2 = angle2+dAngle;
SWITCH = 1;
}
}
else if(ch2==-1)
{
if(angle2<=0)
{
angle2 = angle2+dAngle;
ch2 = 1;
SWITCH = 3;
}
else
{
angle2 = angle2-dAngle;
SWITCH = 1;
}
}
}
else
{
servoY.writeMicroseconds(1500+u);
}
}
theta = theta;
turn2 = true;
}
}
if(SWITCH==3)
{
angle = angle3;
// servo.attach(OutputPin3);
/////////////// servo3 ///////////////////////
if(digitalRead(InputPin3)==LOW && turn3)
{
tLowS3 = micros();
tHigh3 = tLowS3-tLowF3;
turn3 = false;
}
else if(digitalRead(InputPin3) && turn3==false)
{
tLowF3 = micros();
tLow3 = tLowF3-tLowS3;
unsigned long t = tHigh3+tLow3;
if(t>1000 && t<1200)
{
psiP = psi-360*count3;
float duty = 100*tHigh3/(tHigh3+tLow3);
psi = (duty-2.9)/(97.1-2.9)*360;
if(psiP>270 && psi<90)
{
count3++;
}
else if(psiP<90 && psi>270)
{
count3--;
}
psi = 360*count3+psi;
Serial.print(phi);
Serial.print(", ");
Serial.print(theta);
Serial.print(", ");
Serial.println(psi);
float error = angle - psi;
int u = -1*Kp*(int)(error);
if(u>200)
{
u = 200;
}
else if(u<-200)
{
u = -200;
}
if(u>0)
{
u = u+40;
}
else if(u<0)
{
u = u-40;
}
if(error>-5.0 && error<5.0)
{
servoZ.writeMicroseconds(1500);
if(ch3==0)
{
angle3 = angle3+dAngle;
SWITCH =1;
ch3 = 1;
}
else if(ch3==1)
{
if(angle3>=360.0)
{
angle3 = angle3-dAngle;
ch3 = -1;
}
else
{
angle3 = angle3+dAngle;
SWITCH = 1;
}
}
else if(ch3==-1)
{
SWITCH = 0;
}
}
else
{
servoZ.writeMicroseconds(1500+u);
}
}
psi = psi;
turn3 = true;
}
}
}また，このキャリブレーションロボットではArduino Unoを二つ使用していて，上のプログラムはキャリブレーションロボットのサーボを動かすプログラムです．
Arduinoデータ処理用プログラム
そして，Processingと通信をして地磁気センサーのデータを送信するArduino Unoのプログラムが以下になります．
#include <Wire.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 OFFSET_X_REG 0x05
#define OFFSET_Y_REG 0x07
#define OFFSET_Z_REG 0x09
#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 PRINT_RATE 500 // PRINT_RATE Hzで出力する
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 = 1; // Output date rate: 1=10, 4=238, 2=50, 5=476, 3=119, 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 = 5; // 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;
// 時間変数
unsigned long InitTime, RunTime, PrintTime;
int16_t MagX, MagY, MagZ;
int16_t MagZero[3];
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 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 CalibMag()
{
int16_t MagMax[3];
int16_t MagMin[3];
GetMag();
MagMax[0] = MagX;
MagMin[0] = MagX;
MagMax[1] = MagY;
MagMin[1] = MagY;
MagMax[2] = MagZ;
MagMin[2] = MagZ;
for(uint16_t i=0; i<30000;)
{
GetMag();
if(MagX>MagMax[0])
{
MagMax[0] = MagX;
}
else if(MagX<MagMin[0])
{
MagMin[0] = MagX;
}
if(MagY>MagMax[1])
{
MagMax[1] = MagY;
}
else if(MagY<MagMin[1])
{
MagMin[1] = MagY;
}
if(MagZ>MagMax[2])
{
MagMax[2] = MagZ;
}
else if(MagZ<MagMin[2])
{
MagMin[2] = MagZ;
}
if((millis()-PrintTime)>1/PRINT_RATE*1000)
{
i++;
PrintTime = millis();
}
}
for(uint8_t i=0; i<3; i++)
{
MagZero[i] = (MagMax[i]+MagMin[i])/2;
}
uint8_t RegisterData = 0;
RegisterData=(uint8_t)(MagZero[0]&0x00FF);
I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_X_REG, RegisterData);
RegisterData=(uint8_t)((MagZero[0]&0xFF00)>>8);
I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_X_REG+1, RegisterData);
RegisterData=(uint8_t)(MagZero[1]&0x00FF);
I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_Y_REG, RegisterData);
RegisterData=(uint8_t)((MagZero[1]&0xFF00)>>8);
I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_Y_REG+1, RegisterData);
RegisterData=(uint8_t)(MagZero[2]&0x00FF);
I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_Z_REG, RegisterData);
RegisterData=(uint8_t)((MagZero[2]&0xFF00)>>8);
I2Cwrite1byte(M_DEVICE_ADDRESS, OFFSET_Z_REG+1, RegisterData);
}
void setup() {
Serial.begin(9600); // シリアル通信のレートを設定
Wire.begin(); // I2C通信の初期化
TCCR1A = 0; // 初期化
TCCR1B = 0; // 初期化
OCR1A = 6250;
OCR1B = 3125;
TCCR1B |= (1 << CS12) | (1 << WGM12);
TIMSK1 |= (1 << OCIE1A) | (1 << OCIE1B);
if(LSM9DS1setting())
{
Serial.println("LSM9DS1 setting success");
}
else
{
Serial.println("LSM9DS1 setting failure");
while(1);
}
// CalibMag();
// Serial.print("int16_t MagZero[3] = {");
// Serial.print(MagZero[0]);
// Serial.print(',');
// Serial.print(MagZero[1]);
// Serial.print(',');
// Serial.print(MagZero[2]);
// Serial.print("};");
//
}
ISR(TIMER1_COMPA_vect) {
// ダミーデータの送信
Serial.write((byte)(1));
Serial.write((byte)(200));
// 地磁気の送信(2 byte)
// x軸から算出
if(MagX>=0)
{
Serial.write(0);
Serial.write((byte)((MagX)>>8));
Serial.write((byte)((MagX)));
}
else if(MagX<0)
{
Serial.write(1);
Serial.write((byte)((MagX*(-1))>>8));
Serial.write((byte)((MagX*(-1))));
}
if(MagY>=0)
{
Serial.write(0);
Serial.write((byte)((MagY)>>8));
Serial.write((byte)((MagY)));
}
else if(MagY<0)
{
Serial.write(1);
Serial.write((byte)((MagY*(-1))>>8));
Serial.write((byte)((MagY*(-1))));
}
if(MagZ>=0)
{
Serial.write(0);
Serial.write((byte)((MagZ)>>8));
Serial.write((byte)((MagZ)));
}
else if(MagZ<0)
{
Serial.write(1);
Serial.write((byte)((MagZ*(-1))>>8));
Serial.write((byte)((MagZ*(-1))));
}
}
ISR(TIMER1_COMPB_vect) {
}
void loop() {
GetMag();
}
実行結果
以上のプログラムを書きこみ，実行すると以下のような画面が表示されます．
サーボの回転に合わせてデータも円を描くようにプロットされていきます．
せっかく視点をキーボードで変更できるようにしたので，いろいろな角度からデータを見てみます．
そして，キャリブレーションロボットが動作を終了したらENTERキーを押してデータを確定させ，地磁気のオフセット計算を行います．
地磁気のオフセットの計算方法は以下の記事で解説しています．
地磁気センサーのキャリブレーション方法
ENTERキーを押すと，以下の図のようにデータの下に赤字でオフセット値が表示され，その座標が赤い色でプロットされます．
確かに，キャリブレーションの結果は球体の中心にあることが確認できます．
ちゃんとキャリブレーションロボットとしての役割を果たしてくれています．
まとめ
この記事ではキャリブレーションロボットの完成編として，実際に動く様子を報告しました．
前回の記事でも言ったように動作している途中でサーボが引っかかってしまい止まってしまいます．
キャリブレーションロボット二号機ではそのようなことがないように設計をしていきたいと思います．
改善点はまだありますが，これでキャリブレーションロボットは完成とします．二号機の作成は他のプロジェクトの進行具合を見て始動していきたいと思います．
続けて読む
このブログではキャリブレーションロボット以外のプロジェクトについても更新しています．
興味のある方は，以下のページから読みたいプロジェクトを見てみてください．
Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．
それでは最後まで読んでいただきありがとうございました．
コメント