2012年9月17日月曜日

Arduino でラジコンサーボ(インチキレンジファインダ)

 Arduino と ストリナのドライバで ステッピングモータを回せた ので、次はラジコンサーボを動かしてみようと思います。といっても、Arduinoにはラジコンサーボ用のライブラリがあるので、単純に動かすだけなら線をつないで簡単にできてしまいます。以前、H8やSHを使って、サーボモータを動かしたときは、駆動用のパルスから自分で生成しないといけないので面倒でした(まあ、パルスの周期などでトルクなどを細かく変えたり自由度は高いですが)。
 ただ、サーボを回すだけだと面白く無いので、サーボホーンに(とりあえず両面テープでだけど)PSD距離センサを貼り付けて動かしてみました。


 当初、動かすのが速すぎて、距離がうまく取れませんでした。GP2D12のデータシートを見たら、ちゃんと、最大50msほど測定にかかると書いてありました。そこで、一点で停止→100ms待ち→測定→次の点へを繰り返すようにしました。


 正面20cmくらいのところに壁がある状態で、こんな感じの測定結果。


 壁は正面に垂直に対していますが、距離センサが軸を中心に回転して測定するので、両端は当然距離が長くなります。なんとなく良い感じでとれてるかな。

 次に、こんな感じで正面に障害物を置いてみると




 真ん中の凹んで(距離が短くなって)いる部分が障害物。

 さらに、正面だけ開いている状態だと




 真ん中だけ、距離が大きくなってます。思ったよりもいい感じ。
これを簡易レンジファインダとして移動ロボットに載せたりできないかな?あいかわらず、具体的な使い道を考えずに、突発的にやっているだけなんだけど。

#include <Servo.h>

// PSDセンサのアナログ信号入力ピン。
const int PIN_PSD_IN1 = 5;

// センサの近接側測定不可領域閾値。
const int PSD_DEADZONE = 80;

// 待ち時間。
const int WAIT_TIME = 100;

Servo servo1;

// 電圧をcmに変換。
int analogToCentimeter(int analogValue) {
  return (int)((6787 / (analogValue -3)) -4 + 5);
}

void setup()
{
  Serial.begin(9600);
  Serial.println("Servo set center");
  servo1.attach(9);
  servo1.write(90);
  delay(2000);
}

void loop()
{
  int i;
  
  for (i = 30; i <= 150; i += 10)
  {
    servo1.write(i);
    delay(WAIT_TIME);
    // センサのアナログ値を読み取り。
    int value1 = analogRead(PIN_PSD_IN1);
    
    // 読み取り値が閾値を超えていれば、距離に変換。
    if (value1 > PSD_DEADZONE) {
      int range1 = analogToCentimeter(value1);
      
      // シリアルへ出力。
      Serial.print(i, DEC);
      Serial.print(", ");
      Serial.print(range1, DEC);
      Serial.println();
    }
    else {
      Serial.println("RANGE OUT, ");
    }
  }
  for (i = 150; i >= 30; i -= 10)
  {
    servo1.write(i);
    delay(WAIT_TIME);
    // センサのアナログ値を読み取り。
    int value1 = analogRead(PIN_PSD_IN1);
    
    // 読み取り値が閾値を超えていれば、距離に変換。
    if (value1 > PSD_DEADZONE) {
      int range1 = analogToCentimeter(value1);
      
      // シリアルへ出力。
      Serial.print(i, DEC);
      Serial.print(", ");
      Serial.print(range1, DEC);
      Serial.println();
    }
    else {
      Serial.println("RANGE OUT, ");
    }
  }
  
}