簡単なライントレースロボットを作る

サーボモーターの改造

普通の直流モーターをコントロールするにはギヤとかモータードライバーが必要になります。これを簡単化するにはサーボモーターを改造して回転でき、さらにスピードコントロールできるようにします。 ここでは、安価に購入できるサーボモーターを用いてみます。

サーボモーターの改造

ユニバーサル金具(写真上部)を切って写真のように、穴2つの長さのものと、1.5(本当は1.75ぐらいのがいいかも)のものを作る
長さ2のものを直角に折り曲げ、1.5のものと写真のようにユニバーサルプレートにネジとナットでとめる。ここにサーボモーターがはまる予定。
反対側もネジ止めします。
サーボモーターを取り付けます。
タイヤセットを用意します
サーボモーター付属のパーツを用意します。
タイヤに十字のものをはめて、サーボモーターにネジ止めします。
ホイールを取り付けます。
ボールキャスターと電池ボックスをネジ止めするための金具を取り付けます。
電池ボックスを取り付けます。
ラインセンサーを取り付ける部分を取り付けます。
CPUとの配線を行うブレッドボードを取り付け、サーボモーターの配線をします。
左のモーターはp21、右のモーターはp22に接続します。この写真の上側の+ラインは電池からの電源で約5Vが供給されます。これがCPUボードのp2に入ります。また、下側の+ラインは、CPUボードで3.3Vに定電圧化された電圧がでているのでこれを使う場合はこちらを用います。電源のつなぎ間違いが起こると火を噴くこともあるので十分気を付けてください。
後ろの配線は結束バンドなどで固定しておくと良いでしょう。
車輪の回転スピードなどを検出するために光センサーとタイヤに光を反射させる銀紙を貼り付けます。
これらは必須ではありませんが、スピードを正しくコントロールするときに必要になります。
反射型フォトインターラプタの回路図はここをクリックしてください。
Vccは3.3Vに繋いでください。また、信号は左側のセンサーはp20、右側はp19に繋ぎます。


モーターのスピード特性を計測する



下記のプログラムは右のモーターの特性を調べています。同様に左のモーターの特性も調べる必要があります。

モーターのスピード特性を計測する
#include "mbed.h"

AnalogIn Encoder(p19);
//AnalogIn LeftEncoder(p20);
Serial pc(USBTX, USBRX); // tx, rx
PwmOut LeftMotor(p21);
PwmOut Motor(p22);
Timer tm;

const int Center=1525;

bool waitMark(AnalogIn Encoder){
  float Enc;
  int i;
  do {
    Enc = Encoder.read();
    if(i++ >400000) return true;
  } while(Enc<0.4);
  do {
    Enc =  Encoder.read();
  } while(Enc>0.1);
  return false;
}

int main()
{
  bool bad;
  int count, loop=5;
  int increment=10;
  int direction=-1;
  const int start=-300;
  const int end=300;
  pc.baud(115200);
  pc.printf("\n\n");
  Motor.period_ms(15);
  Motor.pulsewidth_us(Center+start);
  waitMark(Encoder);
  tm.start();
  for(int speed=Center+start; speed<Center+end; speed+=increment )
  {
    Motor.pulsewidth_us(speed);
    tm.reset();
    for(count=0; count<loop; count++) {
      if(bad=waitMark(Encoder)) break;
      if(tm.read()>120.0 && loop==5) break;
    }
    if(count!=0)
    {
      float span=tm.read()/count*direction;
      if(!bad) {
        loop = (span>6) ? 1 : 5;
        pc.printf("%d %8.4f\n", speed, 1/span);
      }
      else
      {
        loop=1;
        do
        {
          Motor.pulsewidth_us(speed+=increment);
        } while(waitMark(Encoder));
        speed-=increment;
        direction=1;
      }
    }
    else {
      loop=1;
      do
      {
        Motor.pulsewidth_us(speed+=increment);
      } while(waitMark(Encoder));
      speed-=increment;
      direction=1;
    }
  }
  Motor.pulsewidth_us(Center);
  pc.printf("stop\n");
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 止まる場所。モーターによって異なる
11 
12 銀紙を見つけてそれが去るまで監視する
13 
14 
15 
16 
17 止まっているか途轍もなく遅い場合は諦める。
18 
19 
20 
21 
22 
23 
24 
25 
26 
27 計測異常か？
28 loop:標準的に5回の平均スピードを求める
29 10μs毎に計測する
30 
31 停止点から300μs狭いパルスから開始
32 停止点から300μs広いパルスで終了
33 
34 
35 
36 開始スピードに設定
37 タイヤを開始地点に移動
38 タイマー計測開始
39 開始地点から終了地点まで10μsずつスピードを変更して計測する
40 
41 
42 
43 
44 
45 遅すぎたら計測中止
46 
47 計測を一回でも行った場合
48 
49 一回当たりの時間間隔
50 遅くて途中で断念しなかった場合
51 ちょっと遅いときは5回はやめて1回の回転に変更する
52 スピードの表示
53 
54 遅くて途中で断念した場合
55 
56 1回の回転に変更する
57 計測可能になるまでスピードを変更する
58 
59 
60 
61 
62 多分、回転方向が変わったはずだからdirectionを1にする
63 
64 
65 一度も計測しないで断念した場合
66 
67 
68 
69 
70 
71 
72 
73 
74 
75 モーター停止
76 
77

スピードの適正化

上記の実験により、モーターのスピードは指定スピードとは正比例しないことが分かります。 これを出来るだけ正比例するように補間を行います。
下のプログラムを参考にして上記のプログラムに組み込んでみましょう。

補間する
#define N 19

float data[2][N] ={
  {-1.0093,-0.9648,-0.9087,-0.8435,-0.7666,-0.6739,-0.5584,-0.3713,
   -0.1429,0.1585,0.3348,0.4792,0.5905,0.6866,0.7706,
   0.8436,0.9004,0.9661,1.0309},
  {1445,1455,1465,1475,1485,1495,1505,1515,1525,1545,
   1555,1565,1575,1585,1595,1605,1615,1625,1635}
};


//線形補間 data[y][x]:データ:　x:補間する値
float interpolation(const float x, float data[][N],const int w)
{
  int i;
  float yi,yi1,xi,xi1;
  //xの値が範囲外の場合はxが最大最小値の値を返す
  if(x<=data[0][0]) return data[1][0];
  else if(x>=data[0][w-1]) return data[1][w-1];
  for(i=1;i<w;i++)
    if(data[0][i]>=x) break;
  xi = data[0][i-1];
  xi1= data[0][i];
  yi = data[1][i-1];
  yi1= data[1][i];
  return yi + ((( yi1 - yi ) * ( x - xi )) / ( xi1 - xi ));
}

 1 データの個数
 2 
 3 「モーターのスピード特性を計測する」で計測した
 4 データを切り出す
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
13 補間するメソッド
14 
15 
16 
17 
18 
19 
20 
21 
22 
23 
24 
25 
26 
27




赤外線センサーは機能すれば自作でも何でもいいですが、ここでは補修用のセンサーを用いています。

上部から眺めると下の写真のようになります。

電池は4.5Vから9Vまでの範囲でMBEDは使えます(VIN(p2)に繋げます)が、サーボモーターの動作電圧範囲が4.8～6Vらしいのでニッケル水素充電池を4本(1.2×4=4.8V)を用いています。



サーボモーターをブレッドボードに取り付けるヘッダを作ります。
通常販売されているものは写真の一番左側のように金属部分の長さが異なります。これを流用してサーボモーターをブレッドボードに繋げるためには左から二番目のように、金属部分をほぼ等しくします。これを、サーボモーターのコネクタに挿したのが写真の一番右側です。これだと上から挿すのでちょっと邪魔になります。ですから、少し写真右から二番目のように曲げます。この場合はブレッドボードから抜けやすくなるので、曲げる方を少し長くする必要があります。直角に曲げてもいいですが、斜め上から挿す方ように工作した方が抜けにくくなります。また、始めから直角に曲がっているのも販売されていますが、似たような工作が必要になります。
真上から差し込むのが一番安定的です。



サーボモーターはP21とp22。
赤外線センサーはp19とP20につなげています
次のプログラムはライントレースできるかどうかのチェックプログラムです。これが最終的な形ではありません。また、中に入っている数字は普遍的な数字ではないので、ロボットの個性で変わります。

ライントレースさせるプログラム
#include "mbed.h"
DigitalOut myled(LED1);
AnalogIn LeftSensor(p20);
AnalogIn RightSensor(p19);
PwmOut LeftMotor(p22);
PwmOut RightMotor(p21);

int main()
{
  LeftMotor.period_ms(20);
  RightMotor.period_ms(20);
  bool Left, Right;
  LeftMotor.pulsewidth_us(1400);
  RightMotor.pulsewidth_us(1600);
  while(true)
  {
    Left  = LeftSensor.read() < 0.2;
    Right = RightSensor.read() < 0.2;
    if(Left) {
      LeftMotor.pulsewidth_us(1450);
      wait_ms(400);
      LeftMotor.pulsewidth_us(1400);
    }
    if(Right) {
      RightMotor.pulsewidth_us(1580);
      wait_ms(400);
      RightMotor.pulsewidth_us(1600);
    }
  }
}

 1 
 2 ここでは使っていないが、動作チェック用
 3 赤外線センサー
 4 
 5 改造済サーボモーター
 6 
 7 
 8 
 9 
10 周期は20msで行う。
11 
12 
13 最高スピードではなく遅めに設定
14 
15 
16 
17 黒のラインを見つけたらtrueになる
18 
19 右に寄ってしまって左のセンサーが黒を検出
20 左に行くために左のサーボを遅くする。
21 ある程度元に戻したら
22 元のスピードに戻す
23 
24 
25 
26 
27


ライントレースが出来ているかのチェックする。