プログラムの修正 anchor.png

もっと効率よく進むことができるようプログラムを修正します。

Page Top

赤外線反射センサの値を再確認 anchor.png

赤外線反射センサの値を確認で作成したIRSensor.cを再度実行し、黒い地面から白い地面に移動する際のセンサの値を確認します。
センサの値は、地面が白くなると急激に変化する訳でなく、徐々に変化しているのが分かります。

line7.png

これにより、黒と白の中間点では全力で直進、黒の方へ少しずれたら少し右へ、白の方へ大きくずれたら大きく左へと走行することが可能ではないでしょうか。

Page Top

グラフを作る anchor.png

赤外線反射センサの値と左右の車輪の動きをグラフにしてみます。 白と黒の地面の閾値を70としていますので、ここを中心に左右が最大出力になるようにします。

graph.png

グラフの横軸は赤外線反射センサの値、縦軸は車輪のスピードです。 赤いラインが右車輪、青いラインが左車輪です。

センサの値が70の時、両車輪とも最大速度1023となります。 センサ値が20で黒い地面となりますので、右へ曲がるため右車輪の速度は0になります。反対にセンサ値が120で白い地面となりますので、左へ曲がるため左車輪の速度は0になります。

つまり、右車輪は(20,0)と(70,1023)の2点を通る直線で表すことができます。左車輪は(120,0)と(70,1023)の2点を通る直線で表すことができます。

Page Top

計算します anchor.png

2点を通る直線は1次関数ですので、グラフを式にすると以下のようになります。

Page Top

右車輪 anchor.png

y = (1023 / 50) * x - (2046 / 5)

小数点数は扱いませんので、整数化します。

y = 20 * x - 409
Page Top

左車輪 anchor.png

y = -(1023 / 50) * x + (12276 / 5)

左車輪も同様に整数化します。

y = -20 * x + 2455
Page Top

スピードの補正 anchor.png

右車輪と左車輪の動きは数式で求められましたが、実際にはアクチュエータ側の制限がありますので、速度設定時に範囲外の値を補正します。

// アクチュエータ操作
void Direction (short left, short right) {
  if (left < 0) left = 0;
  else if (left > 1023) left = 1023;
  if (right < 0) right = 0;
  else if (right > 1023) right = 1023;
  fd_DXSetSpeed (1, -right); 
  fd_DXSetSpeed (2, left);
}

AX-12Aを逆転させるには-(マイナス)値を指定すれば問題ありませんが、後退もその場での旋回も行いませんので、有効範囲を0~1023とします。 範囲外の値については、0以下の場合は0、1023以上は1023に補正します。

Page Top

プログラム完成 anchor.png

#include <fd.h>
// アクチュエータ操作
void Direction (short left, short right) {
  if (left < 0) left = 0;
  else if (left > 1023) left = 1023;
  if (right < 0) right = 0;
  else if (right > 1023) right = 1023;
  fd_DXSetSpeed (1, -right); 
  fd_DXSetSpeed (2, left);
}
// メイン関数
void main (void) {
  uint8_t GroundData;

  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  fd_SetUVThreshold (7.4);
  DX_ChangeBaudrate (1000000);

  fd_DXSetEndlessTurn (1, true);
  fd_DXSetEndlessTurn (2, true);

  while (!fd_GetPB () && !fd_rx_buff ()) {
    DX_ReadByteData (100, 28, &GroundData, 10, NULL);
    fd_printf ("ground: %3d\r", GroundData);
    Direction (-20 * GroundData + 2455, 20 * GroundData - 409);
    fd_Wait (10);
  }
  Direction (0, 0); // 停止
}

完成したプログラムを実行してみましょう。 前章のプログラムと比べ、上手にライントレースができているでしょうか。

中心となるセンサ値やグラフの傾きを変えながら、適合する値を調整してみましょう。

次のチャプターへ

FDIII-HC Starter Kit Guideに戻る


Front page   Diff ReloadPrint View   Page list Search Recent changes   RSS of recent changes (RSS 1.0) RSS of recent changes (RSS 2.0) RSS of recent changes (RSS Atom)
Last-modified: 2013-04-09 (Tue) 23:54:58 (JST) (2728d)