みなさん，こんにちは
おかしょです．
このブログでは様々なプロジェクトを実施していますが，この記事ではセンサーキャリブレーションロボットの動作確認編として説明していきます．
プロジェクトに関する過去の記事は以下の記事から見ることができるので，ぜひ読んでみてください．
プロジェクト
この記事を読むと以下のようなことがわかる・できるようになります．
- ロボットの作成過程
- 動作確認方法
- キャリブレーションロボットの作成方法
この記事を読む前に
キャリブレーションロボットの作成過程は構想段階から解説していて，この記事は以下の記事の続きとなっています．
まだ読んでいない方は以下の記事を先に読んでおくことをおすすめします．
導線の整理
前回の記事から何をやったのかというと，導線の整理をしました．
このロボットはサーボが回転するので，導線がサーボに絡まらないようにしなければなりません．
そのため，もともとあった導線を一度切断してビニル線で延長しました．さらに，線がぐちゃぐちゃにならないようにマスキングテープで束にしたりもしました．
ですが，サーボを動かしてみるとどうしても導線が絡まってしまいます．
絡まると，サーボの動きが止まってしまうので，導線が絡まらないように途中でほどいてあげなければなりません．
これは完全に設計の時点でのミスです．
設計をしていた時点では導線はどうにでもできると思って，設計の時点では導線のことはあまり考えていませんでした．てきとうに導線のガイドは作っていただけでした．しかし，実際に作って動作させてみるとどうにもできませんでした．
今回は仕方がないので，このままいくことにしました．
このロボットの設計についてはこちらを参照してください．
動作確認
さて，導線のことは置いといて動作確認を行います．
その際に使用したArduinoのプログラムはこちらの記事にのっています．
上の記事にのっているプログラムはサーボを動かすプログラムで，センサーのプログラムはこちらの記事にのっています．
このプログラムを試したところ，サーボも動きだしセンサーからのデータも取得することができました．
しかし，サーボの動きが思ったものとは違いました．
最初に回転する一つ目のサーボは思った通りに回転するのですが，二つ目のサーボが思った通りに回転しません．
写真の赤丸で示したサーボがうまく動きません．
原因を探ってみると，どうやら1つ目のサーボの重さで角度の制御ができていないようです．
なぜこのように角度の制御ができないのかを考えると，その原因はプログラムにありました．
動作確認をしたプログラムではサーボの制御を一つずつ順番に行っていました．そのため，一つのサーボが動作している時は他のサーボは制御されないようになっています．
このせいで1つ目のサーボが回転している時は赤く囲ったサーボは制御されないので角度が変わってしまい，思った通りの回転をしていませんでした．
そこで，すべてのサーボを常に制御できるようにプログラムを一部書き換えました．
書き換えたプログラムは以下のようになります．
#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 = -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)
{
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;
}
}
}具体的にどこを書き換えたのかというと，各サーボごとにサーボのクラスを設定しました．プログラム中の11から13行目で定義しています．
そして，そのクラスを使って各サーボごとにピンの設定(50から52行目)，サーボの動作をさせています．
この結果，すべてのサーボを同時に制御することが可能となり，思った通りの動きをするようになりました．
まとめ
この記事ではキャリブレーションロボットの動作確認を報告しました．
プログラムを少し修正して動作確認はできましたが，導線がサーボと途中で絡まってしまうので改善が必要です．
ただ，当初の目的のキャリブレーションを楽にするというものはこのままでも達成できそうなので，設計などの改善は後回しにします．
とりあえず，あとはデータ処理をできるようにするのみなのでProcessingでデータのビジュアル化をしたいと思っています．
キャリブレーションロボット (MARKⅡ)の作成は他のプロジェクトとの兼ね合いを考えて実行していきたいと思います．
続けて読む
今回，動作確認をして完成まで間近となりました．
以下の記事ではこのプロジェクト，キャリブレーションロボットの完成編となります．このプロジェクトの最後の記事で，キャリブレーションロボットの性能確認を行っています．ぜひ続けて読んでみてください．
Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．
それでは最後まで読んでいただきありがとうございました．
コメント