みなさん，こんにちは．
おかしょです．
この記事で使用するサーボはサーボの角度をセンサーで取得し，フィードバックすることができます．
この機能を利用して，サーボの角度を指定して制御することが可能です．
この記事ではサーボから角度を取得する方法について解説します．
この記事を読むと以下のようなことがわかる・できるようになります．
- Parallax Feedback 360°High-Speed Servoの使い方
- Parallax Feedback 360°High-Speed Servoから角度を取得する方法
- Arduinoのプログラミング
この記事を読む前に
この記事は，以下の記事の続きとなっています．
以下の記事ではParallax Feedback 360°High-Speed Servoのデータシートを読みながらデータシートの読み方を解説しています．
Parallax Feedback 360°High-Speed Servoの性能やデータシートの読み方などを知りたい方は，先に読んでおくことをおすすめします．
角度を取得する方法
Parallax Feedback 360°High-Speed Servoの角度の取得の仕方は以前の記事で詳しく説明しているので，この記事では簡単に解説します．
サーボには4本の線がつながれており，角度の情報を取得できる線は黄色の線になります．
この線から送られる信号は1周期が910Hzとなっていて，サーボの角度に応じた時間だけHIGHになります．なので，HIGHになっている時間と1周期にかかる時間の比を利用して，角度を求めることができます．
この角度を求める数式は以下になります．
$$ angle=\frac{デューティ比\times 100-2.9}{97.1-2.9} $$
デューティ比を取得するプログラム
上記の角度を求める数式内のデューティ比を求めるプログラムの解説を行います．
まず，プログラムは以下のようになります．
#define InputPin 3
bool turn = true;
unsigned long tHigh, tLow, tLowS, tLowF;
float duty;
void setup()
{
Serial.begin(9600);
pinMode(InputPin, INPUT);
tLowS = micros();
tLowF = micros();
tHigh = 0;
tLow = 0;
}
void loop()
{
if(digitalRead(InputPin)==LOW && turn)
{
tLowS = micros();
tHigh = tLowS-tLowF;
turn = false;
}
else if(digitalRead(InputPin) && turn==false)
{
tLowF = micros();
tLow = tLowF-tLowS;
unsigned long t = tHigh+tLow;
if(t>1000 && t<1200)
{
duty = 100*tHigh/(tHigh+tLow);
Serial.println(duty);
}
turn = true;
}
}
いろいろな定義
まず1行目では信号をする線を挿すArduinoのpin番号を定義します．
私は3番に黄色い信号線を差し込みました．
#define InputPin 32行目ではbool形で定義しています．bool型というのはtrueかfalseのどちらかの値しかとらない変数に使われる方のことです．数値で表すと，0か1の値をとります．ここでは，最初にtrueとして設定しています．この変数の意味については後で説明します．
3行目で定義しているunsigned long型というのは，符号がない4 byteの範囲の値を意味します．つまり，0~4,294,967,296の範囲を表現できます．
ここで定義している変数にはmicros()という関数を使って，プログラムを実行した時からどれくらいの時間が経ったのかをマイクロ秒単位で与えます．
micros()関数は約70分間でリセットされるので，最大で70分間をマイクロ秒に変換した値が入ります．70分をマイクロ秒に変換すると4,200,000,000なので，unsigned long型が適していることがわかります．
以上のような理由でunsigned long型を使用しました．
unsigned long tHigh, tLow, tLowS, tLowF;次の4行目ではfloat型で定義しています．float型というのは小数を扱える型です．この型を使ってデューティ比を表すdutyを定義しています．
float duty;
set up()関数
set up()関数では，まずシリアル通信のボーレートを設定しています．
シリアル通信では，あとでloop()関数内で計算したデューティ比を表示するようにします．
Serial.begin(9600);次にピンのモード設定を行います．
変数定義のところで設定したInputPinは信号線と接続するため，INPUTとして設定しています．
pinMode(InputPin, INPUT);最後に，変数の初期化を行います．これらの変数は信号線から送られてくる信号がHIGHになった時の開始時刻（tHighS）と終了時刻（tHighF），HIGHになっている時間（tHigh），LOWになっている時間（tLow）を表していて，set up()関数内で初期化を行います．
tHighS = micros();
tHighF = micros();
tHigh = 0;
tLow = 0;
loop()関数
信号線からは910Hzの周期で信号が送られてきます．そのうちのHIGHになっている時間と1周期にかかる時間の比がデューティ比になります．
loop()関数ではHIGHになっている時間とLOWになっている時間によってif文を使って，以下のように条件分岐させています．
if(LOWになっている時)
{
処理内容;
}
else if(HIGHになっている時)
{
処理内容;
}LOWになっている時というのは，最初にInputPinに設定したピンにLOWという信号が送られた時です．なので，if文の条件式は以下のようにしています．
if(digitalRead(InputPin)==LOW && turn)ここで使用しているdigitalRead()という関数は引数に指定しているピンから入ってくる信号がHIGHなのかLOWなのかを判定します．
また，条件式にはturnというbool型で設定した変数がかかれています．これはInputPinにLOWが印加された場合，以下のようにして，ここまでで経過した時間を計測しています．
tLowS = micros();tLowSというのはLOWになった開始時刻を表しています．そのため，LOWになっている途中などは計測してはいけません．LOWになった瞬間だけ計測すればいいのです．
なので，一度だけ計測するためにturnという変数をif文の条件式に書き込んでいます．turnは最初はtrueとして設定しているため，最初は必ずこのif文に入るようになっています．
一度だけ計測すればいいので，その後の文で以下のようにturnをfalseにして，次からはこのif文には入らないようにしています．
turn = false;ここで，このif文に入った時がLOWになった瞬間ということは，信号がHIGHであったのが終了したということになります．
なので，このif文の中ではLOWになった開始時刻の計測と同時に，以下のようにしてHIGHであった時間を算出しています．
tHigh = tLowS-tLowF;先程も説明しましたが，tLowSというのはLOWになった時の開始時刻，tLowFというのは終了時刻を表しています．それらの差がHIGHであった時刻を表すので，上のようにしてHIGHであった時間tHighが求められます．
次に，信号線から送られてくる信号がHIGHになった時の処理をします．
このときに入るif文の条件式は以下のようになっています．
else if(digitalRead(InputPin) && turn==false)このif文内ではLOWの終了時刻を計測するため，LOWの開始時刻を測定してある必要があります．なので，turnはfalseでなければなりません．だから，if文の条件式には”turn==false”が加えてあります．
このif文でも，先程と同様にしてLOWであった終了時刻を以下のようにして計測しています．
tLowF = micros();LOWの終了時刻がわかったということは，LOWであった時間が以下のように計算できます．
tLow = tLowF-tLowS;これで信号線から送られてくる信号がHIGHの時間とLOWの時間が求められました．ということは，1周期にかかった時間が以下のようにして求められます．
unsigned long t = tHigh+tLow;この値は910Hzのはずです．つまり，約1.1m秒です．HIGHの時間やLOWの時間はマイクロ秒で求めていたので，1周期は1100マイクロ秒です．なので，ここで求められたtも1100マイクロ秒となるはずです．
しかし，この値の通りに求められるとは限りません．理論上はそうであっても，現実にはノイズなどの様々な要因によってこの値は乱れることがあります．また，データの取りこぼしが起こる可能性も0ではありません．
そこで，以下のようなif文を組んでデータの取りこぼしを避けます．
if(t>1000 && t<1200)
{
処理内容;
}ここで，if文の条件式に幅を持たせているのはノイズの影響を考慮して，1周期にかかる時間を±100マイクロ秒まで許容するためです．このようにすることで，データの取りこぼしなどの影響で1周期にかかる時間が理論通りでない場合はデューティ比の計算をしないようにします．
このif文内では以下のようにしてデューティ比を計算します．
duty = 100*tHigh/(tHigh+tLow);%単位でデューティ比は計算されます．次の行で，その結果を表示するようにしています．
Serial.println(duty);そして，最後に以下のようにしてHIGHとなっている時の処理を終了し，LOWとなっている時の処理を行う許可をします．
turn = true;以上のようにして，デューティ比を取得します．
角度を取得するプログラム（0~360°）
デューティ比が取得できれば，あとは角度を求める計算をするだけです．
以下の式をデューティ比を求める式の次の行に挿入すれば角度を算出することが可能です．
float theta = (duty-2.9)/(97.1-2.9)*360;この式では，最後に360が掛けられているのでdegree単位の角度が算出されます．
もし，radian単位の角度が欲しければ，360のところを/(2\pi/)にすればできます．
角度を取得するプログラム（-∞～+∞）
先程のプログラムでは，求められる角度の範囲が0°～360°でした．このままではマイナスの値が使えないため，制御が思うようにできないことがあります．
例えば，0°にサーボの角度を制御したい場合，オーバーシュートをして0°を超えると360°となってしまうためもう一周して戻ってこなければならなくなってしまいます．
そのようなことが起こらないために，マイナスの表現やサーボが1回転したかどうかの判断ができるようにします．
やり方としては，角度が大きく変化した時を1回転したとして判断して角度を求めるようにします．
プログラムで書くと，以下のようになります．
if(thetaP>270 && theta <90)
{
count++;
}
else if(thetaP<90 && theta>270)
{
count--;
}
theta = 360*count+theta;ここで，thetaPというのは前回の角度を表します．最初のif文の条件は角度が第4象限から第1象限に移った時を表し，このとき，サーボの角度は1回転しているので新たなint型の変数countに1を加えます．このcountは一番上でint型で初期値を0として定義します．
次のelse if文では第1象限から第4象限に移った時を表し，この時はサーボが反対に1回転しているのでcountから1を引きます．
この結果，countという変数には何回転したのかが記録されるので，最後にこれを利用して角度を算出します．
このとき，回転の判定には前回の角度thetaPを用いています．このthetaPの値は0°から360度の範囲でなければなりません．そのためthetaを求める式の前に，単純に”thetaP=theta”としてはいけません．thetaPはthetaの計算をする前に以下のように計算する必要があります．
thetaP = theta-360*count;
まとめ
この記事ではParallax Feedback 360°High-Speed Servoから角度を取得する方法を解説しました．
以上のプログラムを使用することで角度を取得することができます．
データシートを読んだときはタイマーの割り込みを利用して，適切なタイミングで角度の取得を行おうと思ったのですが，うまくいかなかったので上記のようにして角度を取得しました．
最後に角度の算出を行うプログラムを以下に示します．参考にしてみてください．
#define InputPin 3
bool turn = true;
unsigned long tHigh, tLow, tLowS, tLowF;
float duty;
float theta = 0;
float thetaP = 0;
int count = 0;
void setup()
{
Serial.begin(9600);
pinMode(InputPin, INPUT);
tLowS = micros();
tLowF = micros();
tHigh = 0;
tLow = 0;
}
void loop()
{
if(digitalRead(InputPin)==LOW && turn)
{
tLowS = micros();
tHigh = tLowS-tLowF;
turn = false;
}
else if(digitalRead(InputPin) && turn==false)
{
tLowF = micros();
tLow = tLowF-tLowS;
unsigned long t = tHigh+tLow;
if(t>1000 && t<1200)
{
thetaP = theta-360*count;
duty = 100*tHigh/(tHigh+tLow);
theta = (duty-2.9)/(97.1-2.9)*360;
if(thetaP>270 && theta <90)
{
count++;
}
else if(thetaP<90 && theta>270)
{
count--;
}
theta = 360*count+theta;
Serial.println(theta);
}
theta = theta;
turn = true;
}
}
続けて読む
以上で角度を取得することができたので，次はサーボの角度を制御します．
以下の記事ではサーボの角度を制御する方法を解説しているので，参考にしてください．
Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．
それでは最後まで読んでいただきありがとうございました．
コメント
[…] ，この記事では制御の部分のみ公開します．角度の取得から全体を知りたい方はParallax Feedback 360°High-Speed Servoから角度を取得するプログラムを参照して，そちらと組み合わせてください． […]