 |
 |
 |
 |
プログラムの修正 
もっと効率よく進むことができるようプログラムを修正します。
赤外線反射センサの値を再確認
赤外線反射センサの値を確認で作成したIRSensor.cを再度実行し、黒い地面から白い地面に移動する際のセンサの値を確認します。
センサの値は、地面が白くなると急激に変化する訳でなく、徐々に変化しているのが分かります。
これにより、黒と白の中間点では全力で直進、黒の方へ少しずれたら少し右へ、白の方へ大きくずれたら大きく左へと走行することが可能ではないでしょうか。
グラフを作る
赤外線反射センサの値と左右の車輪の動きをグラフにしてみます。 白と黒の地面の閾値を70としていますので、ここを中心に左右が最大出力になるようにします。
グラフの横軸は赤外線反射センサの値、縦軸は車輪のスピードです。 赤いラインが右車輪、青いラインが左車輪です。
センサの値が70の時、両車輪とも最大速度1023となります。 センサ値が20で黒い地面となりますので、右へ曲がるため右車輪の速度は0になります。反対にセンサ値が120で白い地面となりますので、左へ曲がるため左車輪の速度は0になります。
つまり、右車輪は(20,0)と(70,1023)の2点を通る直線で表すことができます。左車輪は(120,0)と(70,1023)の2点を通る直線で表すことができます。
計算します
2点を通る直線は1次関数ですので、グラフを式にすると以下のようになります。
右車輪
y = (1023 / 50) * x - (2046 / 5)
小数点数は扱いませんので、整数化します。
y = 20 * x - 409
左車輪
y = -(1023 / 50) * x + (12276 / 5)
左車輪も同様に整数化します。
y = -20 * x + 2455
スピードの補正
右車輪と左車輪の動きは数式で求められましたが、実際にはアクチュエータ側の制限がありますので、速度設定時に範囲外の値を補正します。
// アクチュエータ操作
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に補正します。
プログラム完成
#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); // 停止
}
完成したプログラムを実行してみましょう。 前章のプログラムと比べ、上手にライントレースができているでしょうか。
中心となるセンサ値やグラフの傾きを変えながら、適合する値を調整してみましょう。
graph.png
line7.png
