2012年12月1日土曜日

ライントレーサーをつくってみる


こちらもどうぞ ライントレーサのPID制御に挑戦


以前の投稿(TAMIYA 壁づたいねずみ 組立)の壁づたいねずみにUNO、モーターシールド、フォトリフレクタ、モーター用バッテリー(単3 1.2Vx4)、UNO用電源(100円ショップDCDCコンバータ、単3x2)を積んだところ、重すぎたようで車体の流用をやめます。

(初代ライントレーサー この投稿の最後に動画あり)

車体としてライントレーサーを作るときによく使われるTAMIYAの
ツインモーターギアボックス FA-130モーター2個付き
ユニバーサルプレート(2枚セット)
ボールキャスター(2セット入り)
スリムタイヤセット(36mm、55mm経)
で車体を作りパーツを乗せることにしました。



ギアボックスは2つのギア比(58:1、203:1)から選べます。一度58:1で組みましたが、スピードが早すぎてラインを外れる→モーター出力を下げる→トルク不足となったので、203:1に組み替えました。前輪、後輪の位置や重量バランスによって、前輪のボールキャスターが動きにくくなるようですので、前輪が浮かない程度に重心を後ろ寄りにしてます。タイヤは55mmを使用。
ギアボックスから出ているシャフトは3mm六角なので、ミニ四駆のタイヤ(2mm六角)は基本使えません。
はかりがないので全体の重さは不明です。

モーター用電源は、モータードライバTA7291Pで電圧降下2.5Vあるということなので単3充電池 1.2Vx4。
4.8V - 2.5V = 2.3V とモーター電圧2V~3Vの定格内です。
アルカリ電池4本だと
6V - 2.5V = 3.5V
で超えてしまうので、気になる場合はモータードライバに出すPWM信号を最大80%ぐらいに抑えればいいのではないでしょうか。

追記
PWMを80%にしても、パルス幅は変わるけどPWMの電圧は変わらないですね。

モーターの出力は下がります。

モータードライバに出す指示は関数化しました。
motor_cw(n , cw)    回転方向 n : モーター番号0,1 cw : 0で逆転、1で正転
motor(n , pw)      出力   n : モーター番号0,1 pw : 0~100(%)
motor_stop(n , dur)   停止   n : モーター番号0,1 dur : 停止時間ms(0で時間指定なし)
motor_brake(n , dur)  ブレーキ n : モーター番号0,1 dur : 停止時間ms(0で時間指定なし)
ただTA7261Pに用意されているブレーキは効果があるのかわかりません。
motor_stop()、motor_brake()の後、モーターを再び動作させるには、motor_cwをしてからmotor()で回転させます。
TA7291Pのデータシートに 「入力を切り替えたときに貫通電流が流れます。入力切り替え時  (正転⇔逆転、正転/逆転⇔ブレーキ) に STOP モード(約 100 μs)  を入れてください。」 とあるので、motor_cw()、motor_brake()を呼び出したときは、多めの1msの停止命令を入れています。

センサーはフォトリフレクターTPR-105F(反射タイプ)4個。
赤外線LEDと赤外線フォトトランジスタが対になった電子部品。
赤外線LEDから照射された赤外線が、対象物で反射してきた量に応じてフォトトランジスタが電流を出力。それを抵抗にいれ、そこに掛かった電圧をアナログ入力します。

中央に2個、ラインになるビニールテープの幅に入る間隔で配置。
両端に1個ずつ。カーブでラインから大きく外れたのを検知します。
外側は離しすぎました。

センサーのばらつきと感度の調整のために、フォトトランジスタのエミッタ側に可変抵抗をつけたほうがいいかもしれませんが、今回はつけていません。

センサーを床面5mmぐらいに近づけるため、何かに引っかからないようにわざと抵抗を立てて取り付け、ガードの役割にしてます。

センサーは上から見て左からセンサー0~3番。
センサー1,2がライン上にあれば、2個のモーターを速度fast
センサー1,2の片方がラインから外れたら、外れた側のモーターをfast、反対側をslow
センサー0,3のどちらかがライン上にあれば、そちらの側のモーターを停止、反対側をfast
センサー0,1,2,3が全部ライン上、ライン外、又は条件に当てはまらない 1秒停止
とします。
このままだとラインがクロスするコースでは止まってしまいますね。

電源をいれるときは、床とラインの赤外線反射具合を調べるために、中央2個のセンサーをライン上、両端のセンサーはラインに乗らないように置く。

4つのセンサーの入力値からしきい値を求め、センサー1の値としきい値を比較しmodeを決定。
白っぽい床に黒のライン→mode = 0
黒っぽい床に白のライン→mode = 1

もちろん、赤外線の反射が変化すればいいので、色にこだわらなくていいです。
センサー読み取り値など、デバッグ情報はシリアルで送信。

注意:当投稿、下記スケッチのままでは、モーター用に7.5V以上の電源をつないでも、モーターに掛かる電圧は5Vが上限になります。詳しくはこちらの投稿で。

スケッチ:

// ライントレーサー by cranberry
// http://cranberrytree.blogspot.jp/
//                           2012/11/30

byte pin_pr[4] = {14,15,16,17}; //フォトリフレクタ入力ピン
int pr[4] = {0,0,0,0};          //フォトリフレクタ読み取り値
int threshold;                  //しきい値

boolean cwccw = true;           //false:逆転 true:正転
byte pin_pwm[2] = {10, 5}; //PWM_output motor0:pin10 motor1:pin5
byte pin_sig[2][2] = {     //CW/CCW signal to IN1,IN2
{9,8},                     //motor0:pin9,8
{6,7},                     //motor1:pin6,7
};
byte last_sens = 0;        //前ループでのセンサーの状態
byte sens = 0;             //現在のセンサーの状態
byte fast = 50;           //fast speed(%)
byte slow = 40;            //slow speed(%)
boolean mode = 0;          //自動判別 0:black line mode 1:white line mode

void setup() {
  Serial.begin(9600);
  for(byte pin=14; pin < 18;pin++){   //センサー入力ピン初期化
    pinMode(pin,INPUT);
  }
  for(byte pin = 5; pin < 11; pin++){ //モータードライバ用ピン初期化
    pinMode(pin, OUTPUT);
  }
  delay(3000);
  Serial.println("");
  for(byte j=0;j < 20;j++){         //20回読み込む
    for(byte i=0;i < 4;i++){
      pr[i] = pr[i] + analogRead(pin_pr[i]);
    }
  }
  for(byte i=0;i < 4;i++){        //平均値を出す
    pr[i] = pr[i] / 20;
  Serial.print("sensor[");
  Serial.print(i);
  Serial.print("]");
  Serial.print(pr[i]);
  Serial.print("\t");
  }
  Serial.println("");
  int val_max = 0;
  int val_min = 1023;
  for(byte i=0;i < 4;i++){        //最大値、最小値を出す
    if(pr[i] >= val_max){
      val_max = pr[i];
    }
    if(pr[i] <= val_min){
      val_min = pr[i];
    }
  }
  threshold = (val_max - val_min) /2 + val_min; //しきい値決定
  Serial.print("threshold = ");
  Serial.println(threshold);
  if(pr[1] < threshold){         //白or黒ライン判別
    Serial.println("black line mode");
    mode = 0;
  }
  else{
    Serial.println("white line mode");
    mode = 1;
  }
  delay(3000);
}

void loop() {
  //フォトリフレクタセンサー入力
  sens = 0;
  for(byte i=0;i < 4;i++){
    pr[i] = analogRead(pin_pr[i]);
    if(pr[i] < threshold){
      bitSet(sens,3-i);
    }
    Serial.print(pr[i]);
    Serial.print("\t");
  }
  if(mode == 1){     //白ラインモードなら
    sens = sens ^ B00001111; //xorで下位4ビットを反転
  }
  for(byte j=0;j < 4;j++){
    if(pr[j] <threshold){
      if(mode == 1){Serial.print("0");}
      else{Serial.print("1");}
    }
    else{
      if(mode == 1){Serial.print("1");}
      else{Serial.print("0");}
    }
  }
  Serial.print(" (");
  Serial.print(sens);
  Serial.print(")");
  Serial.print("\t");
  Serial.println(threshold);
 
  switch(sens){           //センサー状態による条件分岐
    case 0:  //ラインから外れた
      Serial.println("No line");
      motor(0,slow);
      motor(1,slow);
//      delay(1000);
      break;
    case 1:  //0001
      motor_stop(0,0);
      motor_cw(1,true);
      motor(1,slow);
      break;
    case 2: //0010
      motor_cw(0,true);
      motor(0,slow);
      motor_cw(1,true);
      motor(1,fast);
      break;
    case 4: //0100
      motor_cw(0,true);
      motor(0,fast);
      motor_cw(1,true);
      motor(1,slow);
      break;
    case 6: //0110
      motor_cw(0,true);
      motor(0,fast);
      motor_cw(1,true);
      motor(1,fast);
      break;
    case 8: //1000
      motor_cw(0,true);
      motor(0,slow);
      motor_stop(1,0);
      break;
    case 15: //1111 全部ライン上
      Serial.println("STOP");
      motor_brake(0,0);
      motor_brake(1,0);
      delay(1000);
    break;
    default:    //条件に当てはまらなかった
      Serial.println("No CASE in SWITCH statement.");
      motor(0,slow);
      motor(1,slow);
//      delay(1000);
  }
  last_sens = sens;
}

void motor(byte num, byte pw){
  pw = map(pw, 0, 100, 0, 255);
  analogWrite(pin_pwm[num], pw);
}

void motor_cw(byte num, boolean cw){
  digitalWrite(pin_sig[num][0],LOW);
  digitalWrite(pin_sig[num][1],LOW);
  delay(1);
  if(cw == true){
    digitalWrite(pin_sig[num][0],HIGH);
    digitalWrite(pin_sig[num][1],LOW);
  }
  else{
    digitalWrite(pin_sig[num][0],LOW);
    digitalWrite(pin_sig[num][1],HIGH);
  }
}

void motor_stop(byte num,int dur){
  digitalWrite(pin_sig[num][0],LOW);
  digitalWrite(pin_sig[num][1],LOW);
  if(dur >0){
    delay(dur);
  }
}

void motor_brake(byte num,int dur){
  digitalWrite(pin_sig[num][0],LOW);
  digitalWrite(pin_sig[num][1],LOW);
  delay(1);
  digitalWrite(pin_sig[num][0],HIGH);
  digitalWrite(pin_sig[num][1],HIGH);
  if(dur >0){
    delay(dur);
  }
}

このスケッチのような条件分岐での制御は簡単に理解できますが、ギクシャクしてしまいます。


機械の制御にPID制御というものがあるようですが、youtubeでその動きを見るとセンサー1個でも大変スムーズですね。
arduino用にライブラリもありますが、自分には無理そう。


環境:ArduinoUNO R3、Arduino1.0.2、win7





0 件のコメント:

コメントを投稿