実験４ 車輪エンコーダ
ここで行うこと。



Boe-Botの車輪サーボモータの制御はフィードバック無しでした。Boe-Botは車輪を動かすためのパルスを発生するためのPWMオブジェクトを設定することができますが、同じパルス幅を異なるサーボモータに送ることは僅かに異なった回転スピードを生じます。これはBoe-Botが右と左の速度が異なる前進動作を表します。つまり、Boe-Botは一方向にそれ続け、半径の大きい回転になります。

もし、Boe-Botが直線で走ったら幸運です。Boe-Botを5,6m確認のために走らせてみてください。Boe-Botはそれて、少なくとも2,3cm直線より外れるでしょう。

だから、プログラマーは何をするか?　それは、閉ループのフィードバックを用います。

実際にある意味、閉ループのフィードバックには既に遭遇しています。様々なセンサーの利用はその周囲の状況についてのフィードバックを得るために用いられ、実験２で行ったようにプログラムはBoe-Botの動作を補正することができます。この実験では車輪の回転に関係したフィードバックについて行います。

エンコーダーという言葉は赤外線反射センサーが車輪の動きをBoe-Botが用いることのできる二進数の形式に情報を符号化(encode)するのに用いられるからです。いくつかの車輪エンコーダーは車輪の絶対位置を測定できますが、この実験ではこのような性能は必要ありません。必要な性能は、相対的な位置を知ることで、車輪の回転スピードを望まれた割合で両輪を動かし続けるように制御できることです。

センサーは車輪の穴を追うことによって車輪の回転情報を連続的に得ます。白い紙が貼ってあるので、赤外線LED光は紙がセンサーの前にあるときに検出器に反射して戻ってきます。しかし、スホークの時には反射しません。

実験４－１　エンコーダーを作りテストする

どのように車輪エンコーダープログラムが動作するのか

車輪エンコーダーをテストするクラス
namespace WheelEncoderTest1
{
    public partial class Program
    {
        public void main()
        {
            AnalogInput RightSpin = new AnalogInput(CPU.P2);
            AnalogInput  LeftSpin = new AnalogInput(CPU.P3);
            const int SpinThreshold = 500;
            BoeBotInterface jbot =
                    new BasicBoeBot(new FixedMovementBoeBot());
            jbot.move(BoeBotInterface.continuousForward);
            Boolean rState = RightSpin.Read() > SpinThreshold;

            for (int count = 1; count < 48; )
            {
                if ((RightSpin.Read() > SpinThreshold) != rState)
                {
                    rState = !rState;
                    count++;
                }
            }
            jbot.stop();
            
        }
    }
}

 1 
 2 
 3 部分クラス
 4 
 5 Mainから呼ばれる
 6 
 7 右の車輪のエンコーダー
 8 左の車輪のエンコーダー
 9 光の閾値を500に設定する
10 固定動作のBasicBoeBotオブジェクトをBoeBotInterface型で作る
11 
12 前進させる
13 次の穴からカウントスタート
14 
15 穴が48回通過するまでループする
16 つまり、48/8=6回転
17 
18 
19 
20 
21 
22
23 停止する
24
25
26
27

forwardメソッドが呼ばれた時にjbotオブジェクトは車輪を回転し始めます。リセットボタンが押されたり電源が抜かれるまで連続的に走ります。

車輪が回転すると、穴とスポークは反射センサーの前を通ります。センサーはスポークがセンサーの前にあって光が検出された時にLOWすなわち論理０を出力します。穴がセンサーの前にあって光が検出されない場合にはHIGHすなわち論理１になります。

センサーとBoe-Botは車輪が動くよりも速く動作します。これはメッセージウィンドウで表示される数字が01と繰り返すのではなく、複数個ずつ繰り返すことを意味します。同様に、左右の車輪は独立で、遷移は多分同期していません。

注意

  1. 車輪が回るが、１つあるいは両方のセンサーが同じ値を出し続ける場合は、センサーの位置、プログラム、回路をチェックしなさい。典型的な間違いは、センサーとの接続です。
  2. 0と1との遷移が一定の間隔で起こるかどうか見なさい。


車輪が直線に動いているか見る

先ほどのプログラムはセンサーと回路が正しく動作しているかを見るものでした。そのプログラムを少し修正して遷移の数を数えてみましょう。もしその値が同期し続けたらBoe-Botは前進します。もし片方が早かったりするとその数は大きくなり、Boe-Botは反対方向にずれて行きます。

次のプログラムは両輪で検出される遷移数を数えるプログラムです。



車輪エンコーダーをテストするクラス
namespace WheelEncoderTest2
{
    public partial class Program
    {
        AnalogInput RightSpin = new AnalogInput(CPU.P2);
        AnalogInput  LeftSpin = new AnalogInput(CPU.P3);
        const int SpinThreshold = 500;
        public Boolean checkSensor(AnalogInput pin)
        {
            int value = pin.Read();
            return (value < SpinThreshold);
        }

        public void main()
        {
            BoeBotInterface jbot = new BasicBoeBot(new FixedMovementBoeBot());

            int rightCount = 0;
            int leftCount = 0;
            Boolean rightState = checkSensor(RightSpin);
            Boolean leftState = checkSensor(LeftSpin);

            //jbot.move(BoeBotInterface.continuousForward);
            jbot.move(BoeBotInterface.continuousBackward);

            while (true)
            {
                if (rightState != checkSensor(RightSpin))
                {
                    rightState = !rightState;
                    ++rightCount;
                }

                if (leftState != checkSensor(LeftSpin))
                {
                    leftState = !leftState;
                    ++leftCount;
                }
                Debug.Print(rightCount + " " + leftCount);
            }
        }
    }
}


各ピンの状態は記録され、ピンに結び付けられたカウンターは変化が検出された時に増加します。

課題4-1-2

  1. 出力は数字を認識できないほど速くスクロールします。メッセージウィンドウの出力は止められますが、プログラムも一緒に止まってしまいます。10カウント毎に表示するように修正しなさい。
  2. 車輪が持続的に動くかチェックしなさい。左のカウンターが100になったらその値を0にリセットするようにしなさい。右のカウンター値はその時同じ値だろうか(100)。そうでなかったらBoe-Botはどちら側にずれているか。

実験４－２　直線に動かす

前の実験では車輪エンコーダーのハードウェアがBoe-Botの動きを追跡するのにいかに用いられるかを示しました。Boe-Botがいかにフィードバック無しの設定で直線かを数値的に調べることができるようになりました。この場合、rightCountとleftCountの値を比較します。これらは基本的に走行距離計の値となります。

ここでは、車輪エンコーダーのハードウェアからの情報は直線にBoe-Botが動き続けるように保つのに用いられます。両輪に付けられている車輪エンコーダによる走行距離を比較することによりなされます。車輪のスピードはどちらかがもう1つより距離が長かったりした場合調整されます。

これと、人間が自動車を直線に動かす方法を比較しましょう。この場合、直進するように車体を回すように車輪の方向を変えます。Boe-Botの車輪は方向を変えませんが、以前の実験で行ったように各々の車輪のスピードを変えることによって同じ事を行っています。従って、距離を走行した変化に応じてBoe-Botの両輪のスピードを変えることは、車輪エンコーダーのおかげで、Boe-Botは非常に微細な回転ができます。

最終的な結果はBoe-Botは比較的に直線で前進します。実際には、左右に波を打って動きますが、その回転を見ることは困難です。もし、Boe-Botが完全に直線で動くようになったとしたら、補正は不必要になり、Boe-Botは波打ちません。実際、Boe-Botは大抵ずれますが、ずれの総量はサーボモータで作られる初期見積もりに基づいて異なります。フィードバックシステムの長所は、Boe-Botは初期値がかなりずれていても比較的直線に動くということです。勿論、直進動作の値の代わりに旋回する値を用いるような極端な違いは、もし、ここに提示したプログラムを用いて直すことが不可能でないとしても、非常に困難になります。

少しずつ改良していく代わりに、比較的完全な実装で始めます。その代わりに様々なパラメータやシステムの動作にそれがいかに影響するかを見るアルゴリズムを無くしたり調節することによりプログラムを変更する実験ができます。



車輪エンコーダーをテストするクラス
namespace WheelEncoderTest3
{
    public partial class Program
    {
        public void main()
        {
            WheelEncoderTest3 encoder = new WheelEncoderTest3();

            encoder.runTest();
        }
    }

/**
 * 車輪エンコーダーをテストするクラス
 * 
 * 直進するための初期車輪エンコーダーテストプログラム
 */

    public class WheelEncoderTest3 : BasicBoeBot
    {
        AnalogInput RightSpin = new AnalogInput(CPU.P2);
        AnalogInput LeftSpin = new AnalogInput(CPU.P3);
        const int SpinThreshold = 500;

        int rightCount = 0;
        int leftCount = 0;
        int rightRatio = 1;
        int leftRatio = 1;
        int rightSpeed = 100;
        int leftSpeed = 100;
        int rightAdjust = 0;
        int leftAdjust = 0;
        int leftStep = 20;
        int rightStep = 20;
        int leftLimit = 40;
        int rightLimit = 40;
        int leftOdometer = 0;
        int rightOdometer = 0;
        Boolean transition = false;

        public WheelEncoderTest3()
            : base(new FixedMovementBoeBot())
        {

        }

        public Boolean checkSensor(AnalogInput pin)
        {
            int value = pin.Read();
            return (value < SpinThreshold);
        }



        public void runTest()
        {
            Boolean rightState = checkSensor(RightSpin);
            Boolean leftState = checkSensor(LeftSpin);

            setSpeed(leftSpeed, rightSpeed);
            leftCount = leftRatio;
            rightCount = rightRatio;

            int i = 0;   // テストのみに対して

            while (true)
            {
                if (rightState != checkSensor(RightSpin))
                {
                    transition = true;
                    rightState = !rightState;
                    --rightCount;
                    ++rightOdometer;
                }

                if (leftState != checkSensor(LeftSpin))
                {
                    transition = true;
                    leftState = !leftState;
                    --leftCount;
                    ++leftOdometer;
                }

                if (transition)
                {
                    // 遷移が起こった。フラグをリセットし、補正する。
                    transition = false;

                    // テストが終わった時に出る
                    i++;
                    if (i > 200)
                    {
                        break;
                    }

                    if (rightCount == 0)
                    {
                        if (leftCount == 0)
                        {
                            // 同期の際、変化無し、カウンターをリセット
                            leftCount = leftRatio;
                            rightCount = rightRatio;
                        }
                        else
                        {
                            // 左側は右より遅い
                            if (leftCount >= leftRatio)
                            {
                                // 調節: 左車輪が遅い
                                if (leftAdjust == 0)
                                {
                                    // 左の調節はない。右側の車輪を遅くする
                                    if (rightAdjust < rightLimit)
                                    {
                                        rightAdjust += rightStep;
                                    }
                                }
                                else
                                {
                                    // 左車輪が調節される。値を減らす
                                    leftAdjust -= leftStep;
                                }
                                // 新しいスピードに設定する
                                setSpeed(leftSpeed - leftAdjust, rightSpeed - rightAdjust);
                            }

                            // カウンターをリセットする
                            leftCount += leftRatio;
                            rightCount = rightRatio;
                        }
                    }
                    else
                    {
                        if (leftCount == 0)
                        {
                            // 右側は左より遅い
                            if (rightCount >= rightRatio)
                            {
                                // 調節: 右車輪は遅い
                                if (rightAdjust == 0)
                                {
                                    // 右の調節は無し、左の補正を変える
                                    if (leftAdjust < leftLimit)
                                    {
                                        leftAdjust += leftStep;
                                    }
                                }
                                else
                                {
                                    // 右車輪を補正する。それを減らす
                                    rightAdjust -= rightStep;
                                }
                                // 新しいスピードに設定する
                                setSpeed(leftSpeed - leftAdjust, rightSpeed - rightAdjust);
                            }

                            // カウンターをリセットする
                            leftCount = leftRatio;
                            rightCount += rightRatio;
                        }
                    }

                    // テスト時のみ
                    String output = "rightOdometer:" + rightOdometer + " ";
                    output += "rc:" + rightCount+" ";
                    output += "rightSpeed - rightAdjust:" + (rightSpeed - rightAdjust);
                    output += " ... ";
                    output += "leftOdometer:" + leftOdometer + " ";
                    output += "lc:" + leftCount+" ";
                    output += "leftSpeed - leftAdjust:" + (leftSpeed - leftAdjust);
                    Debug.Print(output);
                }
            }
            stop();
        }
    }
}


アルゴリズムの話に入る前に、次の表に用いている変数を示します。
変数説明
transition エンコーダーの遷移が起こったことを示す
leftState, rightState 車輪エンコーダーからの最後の入力値
leftCount, rightCount 車輪エンコーダーのカウンター
leftRatio, rightRatio 他の側に関係するステップ数
leftSpeed, rightSpeed スピード・パーセンテージ(-100%～100%)
leftAdjust, rightAdjust スピード調節
LeftStep, rightStep スピード調節に対する増減
leftLimit, rightLimit 最大のスピード調整値
leftOdometer,rightOdometer 距離計のカウンタ


leftCountとrightCount変数の移動総距離の相対差を保持しているのに対して、距離計のカウンターは、実際に移動した距離を保持しています。実際の値はleftRatioとrightRatioそして車輪エンコーダー入力によって制御されます。leftCountとrightCount変数の値は相対的な距離計の読みです。これらの利用については、アルゴリズムの説明のところでさらに分かってきます。

leftSpeedとrightSpeedの値は以前にBoe-Botの車輪コントロールクラスで用いたパーセンテージで表した希望スピードです。leftAdjustとrightAdjust変数はフルスピードの設定の時の任意のパーセント変化を保持します。これらの値は左右どちらかの車輪が遅れる時に変ります。これらの変数の値はleftStepとrightStepの増加で変ります。補正変数の最大値はleftLimitとrightLimit変数です。

アルゴリズム自体はrunTestメソッドに入っています。このメソッドは多くの遷移を追跡し制限(ソースでは200)を超えた時に終了します。これは別の状況で永久に続けるテストの出力を確かめることができます。この遷移の数はいかにアルゴリズムが動作しているかを見るのに十分有効です。より実践的な実装では、外部イベント(例えば物体を検出したとき、あるいはある時間がたった時、あるいは距離計値で示された距離に達した時)が起こった時に動作は止まるか変えるかします。

メインループは下に示すように左右の車輪エンコーダー出力ピンをチェックすることから開始します。 

if ( rightState != CPU.readPin(rightPin)) {
    transition = true ;
    rightState = ! rightState ;
    -- rightCount ;
    ++ rightOdometer;
}


条件(if文)は車輪エンコーダーの出力と前の状態を比較しています。これは車輪のスポークか穴の両方のエッジを検出するのに用いています。これはスポークと穴をチェック比較で検出した遷移の二倍になります。transition変数は両車輪のエンコーダー出力がチェックされるようにセット(true)されます。遷移が両入力で検出されなくなったならばループが続けられます。

状態変数は論理的NOT演算子！を用いてひっくり返されています。rightCountの値は減らされ、距離計変数は増加されます。

rightCountとleftCount変数が各々の車輪が検出している遷移の数の間の相対差を追跡しているという愚行がこのメソッドの裏にあります。距離計変数を使うことができますが、これも欠点になります。第一に、距離計変数は上限を持っている。値は大きいが値と比較する必要がある計算に影響します。二番目に、その計算は複雑で時間がかかります。カウンター変数で行われる実行は単純な比較、代入、減算です。アルゴリズムが動作しているのを見るのも簡単です。

メインループの前では、rightCountとleftCountは各々rightRatioとleftRatioに設定されています。これらは正の値で、二つのうち一つは最後にはゼロまで減らされます。

1つあるいは両方のカウンタがゼロになった時にあることが起こります。コードブロックの終わりで起こりますが、カウントしていたものがリセットされます。本質的には両カウンタは各々retio値が加算されますが、プログラマーは値の1つはゼロなのを知っているので、より効率的な代入がなされています。

終わりで更新が起こる理由は次に示すコードのratio値に対しての比較でその値が必要になるからです。 

            // 左側は右より遅い
            if ( leftCount >= leftRatio ) {
              // 調節: 左車輪が遅い
              if ( leftAdjust == 0 ) {
                // 左の調節はない。右側の車輪を遅くする
                if ( rightAdjust &;t; rightLimit ) {
                  rightAdjust += rightStep ;
                }
              } else {
                // 左車輪が調節される。値を減らす
                leftAdjust -= leftStep ;
              }
              // 新しいスピードに設定する
              setSpeed ( leftSpeed - leftAdjust, rightSpeed - rightAdjust ) ;
            }

            // カウンターをリセットする
            leftCount += leftRatio ;
            rightCount = rightRatio ;


カウンター値がratio値より小さい場合は何も変更されません。これは両カウンタが同時にゼロになった時でも同じです。これはratioの制限以内で車輪が正確である(両値がゼロの時)あるいは同期に極めて近いあるいはほぼ同期していることを意味しています。ratio 1:1 は本質的に 2:2 と同じですが、このアルゴリズムでは 2:2 は小さな変化には鈍感になり、1:1 の設定では変化に即時応答します。

カウンター値が各々のratioより大きいか等しい時は、片方の車輪が他方より速いので、両輪の1つを補正します。遅い車輪は速くし、速い車輪は遅くします。現在の補正変数値(leftAdjustあるいはrightAdjust)に基づいた条件と決定に対してチェックします。サーボモータの最大スピードが100%であることを仮定しているためこのようになります。

上で示したソースリストで、leftAdjust値は左の車輪が遅くなった時にテストされます。もしleftAdjust値がゼロでなかった時にその時点で以前より遅くされます。step値を減算することによってその値を減らすことはsetSpeedメソッドが呼ばれ、サーボモータを速くします。

そうでなければ、右の車輪は補正値を増加させることによって遅くしなければなりません。これは重要な最適化が含まれているところです。

rightAdjust値はrightLimitより小さい時に限って変更されます。これは指定した制限よりサーボモータが遅くなるのを防ぎます。そうしないとスピードはゼロまで行ってしまったり、あるいは車輪を逆転させるマイナスにまで行ってしまいます。

サーボモータを遅くしすぎると他のサーボモータが遅れを取り戻してBoe-Botを急速に回転させます。この過操縦は逆方向の補正をBoe-Botに要求を非常に速く行います。結果としてBoe-Botは凄く左右に波打ちます。

このものすごい動作を防止する方法はdampingと呼ばれます。いかに車輪を遅く制限するかdampingを与えて補正されます。

確認

  1. Boe-Botを車輪を床に接触させないように持ち上げ、シリアルケーブルを付けたままでプログラムを走らせます。メッセージウィンドウは両輪のスピードを含めて様々な変数の状態を表示します。遷移の検出時の差に応じて両輪のスピードが変化するのに注意してください。
  2. ケーブルを取り除き、Boe-Botを電池駆動にします。Boe-Botを床に置き直進するかどうか見ます。僅かに左右に波打つのを思い出してください。


課題4-2

  1. プログラムは直線に前進するようにBoe-Botを走らせるように作られています。プログラムを後進で直線に走るように変更しなさい。ヒント：パーセント変数を変更します。が、遷移カウンター変数を変更してはいけません。スポークと穴の遷移は車輪が回っている方向にかかわらず同じ方法で検出されることを思い出してください。
  2. Boe-Botを左右に旋回させるように同様のことをしなさい。旋回は車輪を逆回転させて行ったことを思い出してください。
  3. テストプログラムは遷移の数を数え、固定値を超えた時に終了します。特定の車輪で検出される遷移数に基づいて終了するように変えなさい。これは基本的に走行する距離を制御します。この方法は始めのテストプログラムとどのように異なるだろうか？


実験４－３　車輪エンコーダクラス

実験４－２ではBoe-Botを直進させるメカニズムについて行いました。追加の実験として後進と旋回の動作をさせました。これらはシングルタスクの環境でBoe-Botを制御するのに用いることのできるクラスに組み込むことができます。マルチタスクシステムで動作するような制御システムを作ることはさらに複雑なタスクになりますが、この実験でできるようになります。

期待しているように、この車輪制御クラスは以前の実験で定義されたほかのクラスよりかなり複雑になります。タスクが車輪エンコーダーをモニターするため、実際に２つのクラスが必要になります。タスクはBoeBotInterfaceからそのinterfaceで継承される車輪エンコーダークラスの後ろに隠れます。

２つのクラスのアーキテクチャーはphotoresistorのサポートや赤外線距離測定で用いたマルチタスクセンサーシステムと似ています。アプリケーションは通常、Taskクラスに基づきバックグラウンドで実行される、メインオブジェクトと２番めのオブジェクトを結び付けます。

車輪エンコーダーシステムのメインクラスはWheelEncoderBoeBotクラスです。もう１つのクラスはWheelEncoderTaskです。両クラスから１つのオブジェクトが作られ、これら２つのオブジェクトはBoe-Botのサーボモーターを制御するために相互作用します。アプリケーションはWheelEncoderBoeBotオブジェクトと結びつきます。WheelEncoderTaskは車輪が動く時のためのみに必要になります。WheelEncoderBoeBotクラスは以前のBoeBotInterfaceに基づいたクラスでできなかった走行距離計メソッドが使えるようにもなります。

僅かに異なったinterfaceがWheelEncoderBoeBotクラスをコントロールするのに与えられました。このinterfaceはEventクラスに基づいています。WheelEncoderBoeBotクラスは動作が完了したときにイベントのnotifyメソッドを呼びます。もし必要ならば、他の動作をすぐに始めるためのイベントも作ります。これは、WheelEncoderBoeBotのステータスをポーリングする他のタスクを必要としないでサーボモーターを連続的に制御するメカニズムです。ポーリングは依然として使用可能ですが、非効率です。

開始点は次のソースで示されるWheelEncoderBoeBotクラスです。

車輪エンコーダーを用いたBoe-Bot車輪制御
using System;
using BoeBotLib;
using SecretLabs.NETMF.Hardware;

namespace BoeBotLib
{
    /**
     * 車輪エンコーダーを用いたBoeBot車輪制御
     * 
     * 閉じたフィードバックループを用いてサーボモータを制御する
     * バックグラウンドタスクはマルチタスクをサポートする
     */

    public class WheelEncoderBoeBot : RampingBoeBot
    {
        protected _Timer sensorTimer = new _Timer();

        protected int rightAdjust = 0;           // パーセント補正
        protected int leftAdjust = 0;


        // 動作定数

        /**
         * 前進/後進動作に対する遷移/cm
         * 21cmの円周で6.67cmの車輪を仮定します。
         * 車輪には8個の穴があります。一回転に16回の遷移があります
         * 0.76 = 16 / 21
         */
        static public int transitionsPer10Cms = 7; // = 16 / 21*10

        /**
         * 左/右旋回に対する遷移/旋回ステップ
         */
        static public int transitionsPer10Pivot = 25;//31

        static public int transitionsPer10TurnSm = 35;
        static public int transitionsPer10TurnLg = 100;

        static public int turnRatioSm = 1;
        static public int turnRatioLg = 10;

        static public int adjustStep = 20;  // percent, ステップを補正
        static public int maxAdjust  = 40;  // percent, 最大補正

        //protected int rightPin = CPU.P10 ;
        //protected int leftPin = CPU.P11 ;
        AnalogInput RightSpin = new AnalogInput(CPU.P2);
        AnalogInput LeftSpin = new AnalogInput(CPU.P3);
        const int SpinThreshold = 500;

        protected int _leftOdometer = 0;            // 距離計のサポート
        protected int _rightOdometer = 0;
        protected int leftStop = 0;                // 距離計停止点
        protected int rightStop = 0;

        protected int rightCount;                  // 車輪エンコーダ
        // フィードバック
        protected int leftCount;
        protected int rightRatio;
        protected int leftRatio;

        protected Boolean rightState;              // 最後に検出した
        // 車輪エンコーダの状態
        protected Boolean leftState;
        protected Boolean transition;              // 遷移が起こった

        /**
         * デフォールトのピンを用いて
         * 車輪エンコーダーBoeBotサーボモーター制御オブジェクトを作る
         *
         * 入力: Event startEvent: 開始時に通知するイベント
         */
        public WheelEncoderBoeBot(eEvent startEvent)
            : base(startEvent)
        {
        }


        /**
         * 車輪エンコーダーBoeBotサーボモーター制御オブジェクトを作る
         *
         * 入力: Event startEvent: 開始時に通知するイベント
         * 入力: int msecPerCm: 直線運動に対する1cmあたりのmsec数
         * 入力: int msecPerPivot:旋回単位あたりのmsec数
         * 入力: int msecPerTurn:回転単位あたりのmsec数
         * 入力: BasicWheelServo leftWheel:左車輪のBasicWheelServo
         * 入力: BasicWheelServo rightWheel:右車輪のBasicWheelServo
         * 入力: int leftInput:左車輪のエンコーダーの入力ピン
         * 入力: int rightInput:右車輪のエンコーダーの入力ピン
         */
        public WheelEncoderBoeBot
          (eEvent _event
          , int msecPerCm
          , int msecPerPivot
          , int msecPerTurn
          , BasicWheelServo leftWheel
          , BasicWheelServo rightWheel
          , int leftInput
          , int rightInput)
            : base(
                 _event, msecPerCm, msecPerPivot, msecPerTurn, leftWheel, rightWheel)
        {
            //rightPin = leftInput ;
            //leftPin = rightInput ;

            // 車輪エンコーダーセンサーのLEDを点灯させる
            //CPU.writePin(rightPin,true);
            //CPU.writePin(leftPin,true);
            sensorTimer.mark();
        }


        /**
         * 車輪エンコーダーセンサーをチェックする
         *
         * 入力:int pin:センサーに繋がっているCPU.pin
         *
         * 戻り:もし穴が検出された場合falseを返す
         */
        public Boolean checkSensor(AnalogInput pin)
        {
            int value = pin.Read();
            return (value < SpinThreshold);
        }

        /**
         * 動作が終わったかどうかチェックする。
         * これはtrueが戻されるまで呼ばれるべきである。
         *
         * 戻り:boolean: 待ち動作完了した時trueを返す
         */
        public override Boolean movementDone()
        {
            // すでに完了しているかどうかチェックする
            if (base.movementDone())
            {
                return true;
            }

            // センサーのコンデンサが充電されているかどうかをチェックする
            if (sensorTimer.timeout(1))
            {
                // 次のタイムアウトに対して設定する
                sensorTimer.mark();

                // 遷移をチェックする
                transition = false;

                // 右車輪のエンコーダー入力をチェックする
                if (rightState != checkSensor(RightSpin))
                {
                    transition = true;
                    rightState = !rightState;
                    --rightCount;
                    ++_rightOdometer;

                    // 遷移が起こった。距離計の停止をチェックする
                    if ((rightStop > 0) && ((--rightStop) == 0))
                    {
                        // 望まれた距離を移動した
                        getNextMovement();
                        return false;
                    }
                }

                // 左車輪のエンコーダー入力をチェックする
                if (leftState != checkSensor(LeftSpin))
                {
                    transition = true;
                    leftState = !leftState;
                    --leftCount;
                    ++_leftOdometer;

                    // Transition occurred. Check for odometer stop
                    if ((leftStop > 0) && ((--leftStop) == 0))
                    {
                        // 望まれた距離を移動した
                        getNextMovement();
                        return false;
                    }
                }

                // 遷移が起こった時にスピードを補正する
                if (transition)
                {
                    // 走り続ける。補正が必要かどうかチェックする
                    if (rightCount == 0)
                    {
                        if (leftCount == 0)
                        {
                            // カウンターをリセットする。
                            leftCount = leftRatio;
                            rightCount = rightRatio;
                        }
                        else
                        {
                            // 左側が右より遅い
                            if (leftCount >= leftRatio)
                            {
                                // 補正: 左車輪が遅い
                                if (leftAdjust == 0)
                                {
                                    // 左車輪の補正は無い。右側を遅くする
                                    if (rightAdjust < maxAdjust)
                                    {
                                        rightAdjust += adjustStep;
                                    }
                                }
                                else
                                {
                                    // 左車輪は補正された。左側をスピードアップする
                                    leftAdjust -= adjustStep;
                                }
                                // 新しいスピードに設定する
                                adjustSpeed(leftAdjust, rightAdjust);
                            }

                            // カウンターをリセットする
                            leftCount += leftRatio;
                            rightCount = rightRatio;
                        }
                    }
                    else
                    {
                        if (leftCount == 0)
                        {
                            // 右側は左より遅い
                            if (rightCount >= rightRatio)
                            {
                                // 補正: 右車輪が遅い
                                if (rightAdjust == 0)
                                {
                                    // 右の補正は無い。左側を遅くする。
                                    if (leftAdjust < maxAdjust)
                                    {
                                        leftAdjust += adjustStep;
                                    }
                                }
                                else
                                {
                                    // 右車輪が補正された。右車輪をスピードアップする。
                                    rightAdjust -= adjustStep;
                                }
                                // 新しいスピードに設定する
                                adjustSpeed(leftAdjust, rightAdjust);
                            }

                            // カウンターをリセットする。
                            leftCount = leftRatio;
                            rightCount += rightRatio;
                        }
                    }
                }
            }

            return false;
        }

        /**
         * 減速に対するrampingを開始する
         */
        protected override void rampDown()
        {
            // 減速を設定する
            base.rampDown();

            // 減速中にカウントダウンのため停止することを防ぐ
            leftStop = 3;
            rightStop = 3;
        }

        /**
         * 車輪動作パラメータを得る
         *
         * 入力:int movement: 動作番号
         * 入力:int steps:　動作するためのステップ数
         */
        protected override void getMovementSpeed(int movement, int steps)
        {
            int leftRatio;
            int rightRatio;
            int leftStop;
            int rightStop;

            // エンコーダーが動作を停止するようにramping動作を設定する
            base.getMovementSpeed
              (movement
              , steps + ((steps == 0) ? 0 : ((steps > 0) ? 2 : -2)));

            switch (movement)
            {
                default:
                case movementMove:
                    leftRatio = 1;
                    rightRatio = 1;
                    leftStop = steps * transitionsPer10Cms;
                    rightStop = leftStop;
                    break;

                case movementPivot:
                    leftRatio = 1;
                    rightRatio = 1;
                    leftStop = steps * transitionsPer10Pivot;
                    rightStop = leftStop;
                    break;

                case movementTurn:
                    leftRatio = turnRatioSm;
                    rightRatio = turnRatioLg;
                    leftStop = steps * transitionsPer10TurnSm;
                    rightStop = steps * transitionsPer10TurnLg;
                    break;
            }

            // パラメータを設定する
            leftStop = (leftStop > 0) ? leftStop : -leftStop;
            rightStop = (rightStop > 0) ? rightStop : -rightStop;
            this.leftStop = (leftStop + 5) / 10;
            this.rightStop = (rightStop + 5) / 10;
            this.leftRatio = leftRatio;
            this.rightRatio = rightRatio;

            // 動作を設定する
            //rightState = CPU.readPin(rightPin);
            rightState = checkSensor(RightSpin);
            //leftState = CPU.readPin(leftPin);
            leftState = checkSensor(LeftSpin);
            rightAdjust = 0;
            leftAdjust = 0;
            leftCount = leftRatio;
            rightCount = rightRatio;
        }


        /**
         * 距離計をリセットする
         */
        public void resetOdometer()
        {
            _leftOdometer = 0;
            _rightOdometer = 0;
        }


        /**
         * 左の距離計の読みを得る
         *
         * 戻り:int: 距離計の値を返す
         */
        public int leftOdometer()
        {
            return _leftOdometer;
        }

        /**
         * 右の距離計の読みを得る
         *
         * 戻り:int: 距離計の値を返す
         */
        public int rightOdometer()
        {
            return _rightOdometer;
        }
    }
}


WheelEncoderBoeBotクラスは多くの定数定義で始まります。これらは車輪構成に基づくサーボモーターの動作を制御します。異なったサイズ、異なった数の穴とスポークを持った車輪を使う時にはこれらの数字を変更します。定数定義を一緒に保持することは変更が必要になった時にそれらを見つけるのに簡単になります。

WheelEncoderBoeBotのコンストラクタは比較的シンプルです。スーパークラスにstartEventを格納します。これは典型的にはFixedMovementJBotあるいはMultitaskingJBotイベントオブジェクトです。

基本的な動作制御メソッド、movementDoneとstopはmove,pivotなどを含んだ動作メソッドでアプリケーションに対して利用できます。これらは基本的には同じです。車輪エンコーダーサポートを設定し、それに応じたスーパークラスサポートを呼びます。スーパークラスに渡すパラメータが増えていることに注意してください。これは車輪エンコーダのハードウェアが正しく動作しない、あるいは車輪がなんらかの理由でスリップしたというような理由でBoe-Botが永久に走るのを防ぎます。

新しいメソッドは距離計メソッドを含みBoeBotInterfaceクラスによって要求されません。距離計の動作は比較的単純です。距離計はゼロにリセットし、その値が得られます。左右の値が独立に利用できます。

仕事の大半はmovementDoneメソッドでなされます。このメソッドはstartEventによって周期的に呼ばれます。車輪エンコーダーをチェックし、必要に応じてスピードを補正しながら検出された遷移の追跡を保持します。setRealSpeedメソッドはsetSpeedメソッドに影響しないでrampingサポートによって変化させるために用いられます。

WheelEncoderBoeBotとWheelEncoderTaskクラスをテストするには次のプログラムを用います。



車輪エンコーダークラスをテストする
namespace WheelEncoderTest4
{
    public partial class Program
    {
        public void main()
        {
            new WheelEncoderTest_4();

            Task.TaskManager();

            Debug.Print("All Done");
        }
    }
/**
 * 車輪エンコーダークラスをテストする
 * 
 * WheelEncoderJBotとWheelEncoderTaskクラスをテストする
 */

    public class WheelEncoderTest_4 : eEvent
    {
        WheelEncoderBoeBot jbot;
        int state;
        int i;

        const int moveForward = 0;
        const int pivotLeft = 1;
        const int done = 2;

        /**
         * テスト(イベント)オブジェクトを作る
         */
        public WheelEncoderTest_4()
        {
            jbot = new WheelEncoderBoeBot(new MultitaskingBoeBot());
            state = moveForward;
            i = 0;
            jbot.setNextEvent(this);
        }

        /**
         * このメソッドはWheelEncoderJBotオブジェクトが動作を完了した
         * ことをこのオブジェクトに通知するために呼ばれます。
         */
        public override void notify(Object _object)
        {
            switch (state)
            {
                case moveForward:
                    if (i < 4)
                    {
                        ++i;                      // 更新カウンター
                        jbot.move(20);            // 正方形の端に沿って動く
                        state = pivotLeft;       // 次の状態は旋回(pivot)
                    }
                    else
                    {
                        jbot.stop();
                        state = done;
                    }
                    break;

                case pivotLeft:
                    jbot.pivot(2);        // 正方形の角で回る
                    state = moveForward; // 次の状態は直進
                    break;
            }
        }
    }
}


WheelEncoderTest4クラスはEventクラスに基づいていて、Taskクラスではないので少々驚くでしょう。これは、1つのみのタスクバックグラウンドのMultitaskingJBotしかこの時点で必要がないからです。

メインメソッドはWheelEncoderTest4イベントオブジェクトを作ることによって開始します。オブジェクト変数jbotはMultitaskingJBotオブジェクトを順々に作る、新しいWheelEncoderBoeBotオブジェクトへの参照として代入されます。これはTask.TaskManagerメソッド呼び出しによって実際に走るタスクです。

うまくいけば、これは絡み合うこともなしにしっかりと追従します。コンストラクタメソッドは今述べた適切なオブジェクトを作ります。次にコンストラクタはjbotのsetEventメソッドを呼び、WheelEncoderTest4イベントオブジェクトへの参照を渡します。タスクは走っていないので(コンストラクタは停止させることを思い出してください)、setEventメソッドの呼び出しはイベントのnotifyメソッドへの続いて起こる呼び出しを生じます。setEvent呼び出しの前にiとstate変数を設定することが重要です。これはこれらの変数がnotifyメソッドが呼ばれる前に初期化されないといけないからです。

notifyメソッドはBoe-Botが正方形を描いて動くのでシンプルです。四角の全ての４辺は同じ動作が実行されます：前進、旋回。この場合、これらの動作を操作する二つの状態があります：moveForwardとpivotLeft。これらの名前はWheelEncoderTest4クラスに固有であるから、他のクラスの中で用いられている名前と衝突することはありません。

jbotのメソッドは各々の動作を初期化するために呼ばれます。状態は動作の初期化の後他の状態に移行します。TaskManagerメソッドによって繰り返し呼ばれるタスクのexecuteメソッドとは異なってnotifyメソッドは動作が完了した時にのみ呼ばれます。

notifyメソッドのmoveForward状態は四角を描くのに４回呼ばれます。２つの四角は、状態に入り、出る回数を保存することによって描かれます。

notifyメソッドが8回呼ばれるとjbot.forwardメソッドを呼ばなくなります。バックグラウンドのタスクは停止し、TaskManagerメソッドが戻り、メインメソッドの中のSystem.out.printlnメソッドを用いて"All Done"テキストを最後にプリントします。

課題4-3

  1. シリアルケーブルを繋ぎ、Boe-Botの車輪が床に触れないように持ち上げ、プログラムを走らせ、車輪の回転が予期通りかどうか見なさい。
  2. シリアルケーブルを取り除き、電池駆動にしてBoe-Botを走らせなさい。Boe-Botを床に置き、二回四角を描くかどうかを見て、僅かに左右に波打つのを確認しなさい。
  3. プログラムはシンプルな四角で動きます。これを長辺が短辺の二倍長い長方形を描くように変更しなさい。
  4. 逆方向に描くようにしなさい。この意味は、後ろに行き右に旋回することです。
  5. TaskクラスはEventのサブクラスで、WheelEncoderBoeBotのsetEventメソッドに渡すことができます。Eventの代わりにTaskを拡張することによってWheelEncoderTest4クラスを実装しなさい。デフォールトのnotifyに対するTaskの動作は、そのタスクを開始することであることを覚えておいてください。これはタスクのexecuteメソッドは動作を開始し、停止することを意味します。stopメソッドが呼ばれる前に状態設定の中で動作が完了した時にリスタートします。
問題4

  1. 赤外線反射センサーはどんな部品ですか？

  2. 赤外線反射センサーはどのように働きますか？

  3. 白い車輪の代わりに黒い車輪が使われたとしたら何が起こるか。

  4. 車輪エンコーダーハードウェアと共に黒い車輪が動作するようにするにはどのようにするか？

  5. 実験４－２でBoe-Botのサーボモーターのフィードバック制御を導入しました。ダンピングとは何か、サンプルプログラム中でそれがどのように用いられているか、また、これが取り除かれた場合何が起こるか？

  6. 実験４－３で用いたマルチタスクシステムの代わりにポーリングを用いてBoeBotInterfaceに基づいたシングルタスクのクラスを作りなさい。CheckForWaitとmovementDoneメソッドを検討しなさい。

  7. 車輪エンコーダーのオブジェクトはBoe-Botを直線に動くように、また望んだように回転するようにするために用いることが出来ますが、これらの操作は最適な値に非常に近い値から開始されれば使い物になります。Boe-Botのサーボモータの制御クラスは以前の実験で手動で補正されました。


  8. イベントメカニズムは動作呼び出しの固定組を用いて単純なパスを移動するのに用いられます。文字列あるいは配列で動作の組を渡すことの出来るイベントクラスを作りなさい。このクラスはイベントへの参照を持つsetEventのようなメソッドも持ち、より有用になるでしょう。このイベントは配列あるいは文字列の中の全ての動作が実行された時に連絡されるべきです。タスクはEventのサブクラスであることに気をつけてください。ですから、典型的な実装はこの新しい動作イベントを用いるタスクを持ちます。そのタスクは動作の連続を初期化した後でスリープします。
実験３へ戻る　　 実験５へ

実験４終わり