3: 2011-07-15 (金) 12:01:08 yoshida  |
4: 2011-07-15 (金) 12:52:35 yoshida |
| | 赤外線反射センサの値と左右の車輪の動きをグラフにしてみます。 | | 赤外線反射センサの値と左右の車輪の動きをグラフにしてみます。 |
| | 白と黒の地面の中心を70としていますので、ここを中心に左右が最大出力になるようにします。 | | 白と黒の地面の中心を70としていますので、ここを中心に左右が最大出力になるようにします。 |
| - | #ref(line8.png) | + | #ref(graph.png) |
| | グラフの横軸は赤外線反射センサの値、縦軸は車輪のスピードです。 | | グラフの横軸は赤外線反射センサの値、縦軸は車輪のスピードです。 |
| | 赤いラインが右車輪、青いラインが左車輪です。 | | 赤いラインが右車輪、青いラインが左車輪です。 |
| | | | |
| - | 黒と白の真ん中(センサ値70)の時、両車輪とも最大速度1000となります(最大値は1023ですが簡略化のため1000としました)。 | + | 黒と白の真ん中(センサ値70)の時、両車輪とも最大速度1023となります。 |
| | + | センサ値が20で黒い地面となりますので、右へ曲がるため右車輪の速度は0になります。反対にセンサ値が120で白い地面となりますので、左へ曲がるため左車輪の速度は0になります。 |
| | | | |
| - | センサ値が70より小さい時は黒い方へずれているということなので、右車輪のスピードを下げて右へ曲がります。センサ値が小さくなるに従ってスピードを下げ右に曲がる量を増やします。 | + | つまり、右車輪は(20,0)と(70,1023)の2点を通る直線で表すことができます。左車輪は(120,0)と(70,1023)の2点を通る直線で表すことができます。 |
| | | | |
| - | 同様にセンサ値が70より大きい時は白い方へずれているということなので、左車輪のスピードを下げて左へ曲がります。センサ値が大きくなるに従ってスピードを下げ、左に曲がる量を増やします。 | |
| | **計算します [#wd05ab8a] | | **計算します [#wd05ab8a] |
| - | グラフを式にしてみます。~ | + | 2点を通る直線は1次関数ですので、グラフを式にすると以下のようになります。 |
| | ***右車輪 [#m1d1d081] | | ***右車輪 [#m1d1d081] |
| - | 右車輪のスピードが最小値(0)から最大値(1000)へ変化する部分を考えます。~ | + | #ref(math_right.png) |
| - | スピードをY、センサ値をXとした場合、 | + | 小数点数は扱いませんので、整数化します。 |
| - | Y = aX + b | + | #ref(math_right2.png) |
| - | となります。これはXが70であればYは1000となるので、 | + | |
| - | 1000 = 70a + b | + | |
| - | と置き換えます。これを最初の式に当てはめると | + | |
| - | Y = aX + 1000 - 70a | + | |
| - | となります。 | + | |
| - | ここでのaはグラフの傾きです。 | + | |
| - | aを大きくすると、センサ値に合わせて右車輪のスピードが大きく下がるため、急激な右旋回となります。aを小さくすれば、スピードは余り下がらなくなるため、ゆっくりとした右旋回となります。 | + | |
| - | | + | |
| - | aをどのような値にすれば良いかは、実際にライントレースをしながら決めます。 | + | |
| - | | + | |
| - | センサ値が20の時、スピードが0になるよう、aの値を20とします。 | + | |
| - | Y = 20X - 400 | + | |
| | | | |
| | ***左車輪 [#s2cab8e6] | | ***左車輪 [#s2cab8e6] |
| - | 次に左車輪ですが、右車輪と異なりスピードが最大値(1000)から最小値(0)へ変化する部分を考えます。基本的に傾き以外右車輪と同じになります。但し、センサ値が120の時、スピードが0になりますので、aの値は-20となります。 | + | #ref(math_left.png) |
| - | Y = -20X + 2400 | + | 左車輪も同様に整数化します。 |
| - | これでひとまずの計算は終了です。 | + | #ref(math_left2.png) |
| | | | |
| - | **スピードを設定する [#v3af612b] | + | **スピードの補正 [#v3af612b] |
| - | グラフはセンサ値が70を境に反転していますので、70で処理を分割します。 | + | 右車輪と左車輪の動きは数式で求められましたが、実際にはアクチュエータ側の制限がありますので、速度設定時に範囲外の値を補正します。 |
| | #html{{ | | #html{{ |
| | <pre class="brush:c"> | | <pre class="brush:c"> |
| - | DX_ReadByteData (100, 28, &GroundData, 10, NULL); | + | // アクチュエータ操作 |
| - | if (GroundData < 70) { | + | void Direction (short left, short right) { |
| - | Direction (1000, GroundData * 20 - 400); | + | if (left < 0) left = 0; |
| - | } else { | + | else if (left > 1023) left = 1023; |
| - | Direction (GroundData * -20 +2400, 1000); | + | if (right < 0) right = 0; |
| - | } | + | else if (right > 1023) right = 1023; |
| - | </pre> | + | fd_DXSetSpeed (1, -right); |
| - | }} | + | fd_DXSetSpeed (2, left); |
| - | 計算式をそのまま当てはめていますが、センサ値によってマイナスのスピードが作成されてしまいます。ロボット相撲と異なり、ライントレースはその場で旋回することはありませんので、スピードの最小値は0にする必要があります。 | + | } |
| - | #html{{ | + | |
| - | <pre class="brush:c"> | + | |
| - | DX_ReadByteData (100, 28, &GroundData, 10, NULL); | + | |
| - | if (GroundData < 70) { | + | |
| - | if ((GroundData * 20 - 400) < 0) | + | |
| - | Direction (1000, 0); | + | |
| - | else | + | |
| - | Direction (1000, GroundData * 20 - 400); | + | |
| - | } else { | + | |
| - | if ((GroundData * -20 +2400) <; 0) | + | |
| - | Direction (0, 1000); | + | |
| - | else | + | |
| - | Direction (GroundData * -20 +2400, 1000); | + | |
| - | } | + | |
| | </pre> | | </pre> |
| | }} | | }} |
| | + | |
| | **プログラム完成 [#y8b96e69] | | **プログラム完成 [#y8b96e69] |
| | #html{{ | | #html{{ |
| | // アクチュエータ操作 | | // アクチュエータ操作 |
| | void Direction (short left, short right) { | | 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 (1, -right); |
| | fd_DXSetSpeed (2, left); | | fd_DXSetSpeed (2, left); |
| | DX_ReadByteData (100, 28, &GroundData, 10, NULL); | | DX_ReadByteData (100, 28, &GroundData, 10, NULL); |
| | fd_printf ("ground: %3d\r", GroundData); | | fd_printf ("ground: %3d\r", GroundData); |
| - | if (GroundData < 70) { // 黒検知 | + | Direction (-20 * GroundData + 2455, 20 * GroundData - 409); |
| - | if ((GroundData * 20 - 400) < 0) Direction (1000, 0); | + | |
| - | else Direction (1000, GroundData * 20 - 400); | + | |
| - | } else { | + | |
| - | if ((GroundData * -20 +2400) < 0) Direction (0, 1000); | + | |
| - | else Direction (GroundData * -20 +2400, 1000); | + | |
| - | } | + | |
| | fd_Wait (10); | | fd_Wait (10); |
| | } | | } |
ページへ戻る
