みなさん，こんにちは
おかしょです．
地磁気センサーをキャリブレーションするにはセンサーの軸をあらゆる方向に向ける必要があります．
地磁気センサーのキャリブレーションをしようと，ぐるぐる回っていたら気分が悪くなってしまい，これを自動で行うロボットを作ることにしました．
今回は，そのロボットで使うプログラムを公開します．
この記事を読むと以下のようなことがわかる・できるようになります．
- センサーキャリブレーションロボットのプログラム
- センサーキャリブレーションロボットの電子回路
- サーボの動かし方
この記事を読む前に
この記事は以下の記事の続きとなっています．
以下の記事では，このキャリブレーションロボットの構想について解説しています．
まだ読んでいない方はこちらの記事を先に読んでおくことをおすすめします．
使用するサーボ
このロボットはセンサーの各軸をあらゆる方向に向けるために，3つのサーボを用います．
また，すべての方向に向けるために，各サーボが360°回転できなければなりません．角度の制御もする必要があるので，サーボが現在どのような角度なのかもわからなければなりません．
このような条件を満たすサーボを探したところParallax Feedback 360°High-Speed Servoというサーボが見つかりました．
このサーボの角度を制御する方法はこちらを参照してください．
サーボの動かし方
このロボットはセンサーのキャリブレーションを行うためにセンサーをあらゆる方向に向けることができなければなりません．
そこで，3つのサーボの回転軸をx軸，y軸，z軸に合わせて配置します．
そして，以下のようにサーボを回転させます．
- x軸周りに1回転させる
- y軸周りに少し回転させる
- 1, 2を繰り返す
- y軸周りに1回転したら，z軸周りに少し回転させる
- 1, 2を繰り返す
- z軸周りに1回転したら終了
つまり，プログラムとしては最初にx軸周りに回転できるサーボを1回転させます．
x軸周りのサーボの回転角が360°になったら，y軸周りに回転するサーボを少しだけ回転させます．
このとき，回転角度はP制御をして収束させます．
y軸周りのサーボの回転角が収束したら，x軸周りに回転するサーボを先程とは逆方向に回転させます．
なぜ，ここで逆方向に回転させるのかというと，導線が絡まるのを防ぐためです．
ずっと同じ方向に回転させ続けると，導線がロボットに絡まってしまいます．そうなってしまうと危険なので，絡まらないために回転方向を交互に切り替えることで防ぎます．
x軸方向のサーボが反対方向に1回転したら，もう一度y軸方向のサーボを少しだけ回転するようにP制御器で制御します．
y軸方向も1回転したらz軸方向のサーボもy軸のサーボと同じように少しだけ回転させます．
また，y軸方向のサーボはここから反対方向に回転するようにします．
最終的にz軸方向のサーボが1回転したら終了するようにします．
キャリブレーション用のサーボのプログラム
以下にArduino Unoを使って，上のようにサーボを動かすようなプログラムを公開します．
みなさんもぜひ参考にしてみてください．
#include <Servo.h>
Servo servo;
#define OutputPin1 2
#define OutputPin2 3
#define OutputPin3 4
#define InputPin1 5
#define InputPin2 6
#define InputPin3 7
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);
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 = -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)
{
servo.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
{
servo.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)
{
servo.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
{
servo.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)
{
servo.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
{
servo.writeMicroseconds(1500+u);
}
}
psi = psi;
turn3 = true;
}
}
}このプログラムではあえて関数を作ったりはしていません．
個人的に関数を使ってしまうと，プログラムの流れがわかりにくくなってしまうからです．
関数を使って，プログラムをスッキリさせたい方はこのプログラムを基にしてやってみてください．
電子回路
上記のプログラムを試すために使用した電子回路は以下のようになります．
この回路の通りにサーボをArduino Unoに接続すると動作確認ができます．
動作確認結果
上のプログラムを使って動作させた結果，以下のような図で表すように3つのサーボが回転しました．
この図はArduino IDEのシリアルプロッタで表示しました．
この図の青い線で示されている回転角度はx軸周りに回転させることができるサーボです．赤い線はy軸周り，緑色の線はz軸周りのサーボの回転角度です．
先程説明したとおりにサーボが動作してくれているのが確認できました．
まとめ
この記事ではキャリブレーションロボットのプログラム編として，実際にサーボを動かすことができるプログラムを公開しました．
キャリブレーションをするにはセンサーも動作させなければならないのですが，センサーとサーボを同時に動かすのは難しいので，もう一つArduino Unoを用意して，サーボを動かすだけのArduinoとセンサーを動作させるArduinoの２つを使用したいと思います．
続けて読む
とりあえず，サーボを動かすことができたので，次は機体の設計を行います．
機体の設計については以下の記事で解説しているので，続けて読んでください．
Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．
それでは最後まで読んでいただきありがとうございました．
コメント