namespace BasicWheelServoDistanceTest1
{
    public partial class Program
    {
        public void main()
        {
            BasicWheelServo leftWheel =
                new BasicWheelServo(
                  CPU.P13      // pin
                , ***         // forward
                , ***         // center
                , ***         // backward
                , 20000        // low
                );
            BasicWheelServo rightWheel =
              new BasicWheelServo(
                  CPU.P12      // pin
                , ***         // forward
                , ***         // center
                , ***         // backward
                , 20000        // low
                );

            leftWheel.move(100);
            rightWheel.move(100);

            CPU.delay(5000);   // 5秒間サーボを動かす

            leftWheel.stop();   // 車輪を止める
            rightWheel.stop();
        }
    }
}

/**
 * 旋回テストプログラム
 */

namespace BasicWheelServoPivotTest1
{
    public partial class Program
    {
        public void main()
        {
            BasicWheelServo leftWheel =
                new BasicWheelServo(
                  CPU.P13      // pin
                , ***         // forward
                , ***         // center
                , ***         // backward
                , 20000        // low
                );
            BasicWheelServo rightWheel =
                new BasicWheelServo(
                      CPU.P12   // pin
                    , ***      // forward
                    , ***      // center
                    , ***      // backward
                    , 20000     // low
                    );

            leftWheel.move(100);
            rightWheel.move(-100);
            CPU.delay(***);   // ***(ms)サーボを動かす

            leftWheel.stop();   // 車輪を止める
            rightWheel.stop();
        }
    }
}

/**
 * Boe-Botの車輪コントロールインターフェース
 *
 * Boe-Botの車輪コントロールクラスが備えていなければならない
 * メソッドを定義する
 */
namespace BoeBotLib
{
    public abstract class BoeBotInterface
    {
        protected eEvent startEvent;
        protected eEvent nextEvent = eEvent.nullEvent;
        protected eEvent oneTimeEvent = eEvent.nullEvent;

        public const int continuousForward = 32760;
        public const int continuousBackward = -32760;
        public const int continuousLeft = continuousForward;
        public const int continuousRight = continuousBackward;

        /**
         * 動作を開始した時に通知されるようにイベントを設定する
         *
         * 入力:Event event: イベント。nullに設定できる。
         */
        public BoeBotInterface(eEvent _event)
        {
            startEvent = eEvent.checkEvent(_event);
        }

        /**
         * 動作を終了するために現在のタスクを待機(wait)に設定する。
         *
         * 入力:int state: 次のタスク状態
         */

        public void wait(int state)
        {
            Task waitingTask = Task.getCurrentTask();
            waitingTask.nextState(state);

            if (waitingTask.taskStatus() == Task.taskRunning)
            {
                //System.out.println ( "BoeBotInterface" ) ;
                waitingTask.suspend();
                oneTimeEvent = waitingTask;
            }
        }

        /**
         * 動作を開始した時に通知されるようにイベントを設定する
         * 多分、null
         *
         * 入力:Event event: Event
         *
         * 戻り:直前のイベント
         */
        public eEvent setStartEvent(eEvent _event)
        {
            eEvent resultEvent = startEvent;
            startEvent = eEvent.checkEvent(_event);
            return resultEvent;
        }

        /**
         * 動作を開始した時に通知されるようにイベントを設定する
         * 多分、null
         *
         * 入力:Event event: Event
         *
         * 戻り:直前のイベント
         */
        public eEvent setNextEvent(eEvent _event)
        {
            eEvent resultEvent = nextEvent;
            nextEvent = eEvent.checkEvent(_event);

            if (movementDone())
            {
                causeNextEvent();
            }

            return resultEvent;
        }

        /**
         * 通常マッチング・マルチタスク・オブジェクトによって呼ばれる
         */
        protected void causeNextEvent()
        {
            // 次の動作が開始できるというイベントを通知する
            nextEvent.notify(this);

            // 次の動作が開始できるというイベントを通知する
            oneTimeEvent.notify(this);
            oneTimeEvent = eEvent.nullEvent;
        }

        /**
         * このメソッドは動作メソッドの始めに呼ばれるべきである。
         */
        protected void startMovement()
        {
            startEvent.notify(this);
        }

        /**
         * 動作が終わったかどうかチェックする。
         * trueを返すまで呼ばれるべきです。
         *
         * 戻り:もし動作の待機が終わったらtrueを返す
         */
        public abstract bool movementDone();

        /**
         * 動作を止める。
         */
        public virtual void stop()
        {
            move(0);
        }

        /**
         * 前進/後退(マイナスの値)するためにスピードを設定する。
         *
         * 入力:int cm:何センチ移動するか
         */
        public abstract void move(int cm);

        /**
         * 左旋回(プラス)右旋回(マイナス)するためにスピードを設定する。
         *
         * 入力:int steps:回転するステップの数
         */
        public abstract void pivot(int steps);
        /**
         * 左回転(プラス)右回転(マイナス)
         *
         * 入力:int staps:回転するためのステップ数
         */
        public abstract void turn(int steps);
    }
}

using System;
using Microsoft.SPOT; // Smart Personal Objects Technology 
using System.Threading;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.NetduinoMini;
using BoeBotLib;

namespace BoeBotLib
{
    /**
     * 基本的なBoe-Bot車輪コントロールクラス
     * 
     * 車輪サーボモータの自由走行に対するPWMサポートを扱う
     * move(), pivot() or turn()を用いて動作を開始させる。
     * stop() or move(0)でBoe-Botを停止させる。
     */

    public class BasicBoeBot : BoeBotInterface
    {
        public BasicWheelServo leftWheel;
        public BasicWheelServo rightWheel;
        public _Timer timer;
        public int timeout;
        public int msecPerCm;
        public int msecPerPivot;
        public int msecPerTurn;

        protected const int movementNone = 0;
        protected const int movementMove = 1;
        protected const int movementPivot = 2;
        protected const int movementTurn = 3;

        protected int leftMovementSpeed;
        protected int rightMovementSpeed;
        protected int msecPerStep;
        protected bool Done;
        protected new bool wait;

        /**
         * 一般的な動きに対する車輪サーボモータを設定する。
         * 前進はcmを単位にする。
         * 旋回と回転は普通45゜ステップを単位にする。
         * デフォールトのサーボモータの設定を用いる。
         */

        public BasicBoeBot
            (
              eEvent _event
            , int msecPerCm
            , int msecPerPivot
            , int msecPerTurn
            , BasicWheelServo leftWheel
            , BasicWheelServo rightWheel
            )
            : base(_event)
        {
            BasicBot(msecPerCm, msecPerPivot, msecPerTurn, leftWheel, rightWheel);
        }

        /**
         * 一般的な動きに対する車輪サーボモータを設定する。
         * 前進はcmを単位にする。
         * 旋回と回転は普通45゜ステップを単位にする。
         *
         * 入力:int msecPerCm:　直進の時の1cmあたりのミリ秒
         * 入力:int msecPerPivot: 旋回単位あたりのミリ秒
         * 入力:int msecPerTurn: 回転単位あたりのミリ秒
         * 入力:BasicWheelServo leftWheel:左車輪に対するBasicWheelServo
         * 入力:BasicWheelServo rightWheel:右車輪に対するBasicWheelServo
         */

        public void BasicBot
            (
              int msecPerCm
            , int msecPerPivot
            , int msecPerTurn
            , BasicWheelServo leftWheel
            , BasicWheelServo rightWheel
            )
        {
            this.msecPerCm = msecPerCm;
            this.msecPerPivot = msecPerPivot;
            this.msecPerTurn = msecPerTurn;
            this.leftWheel = leftWheel;
            this.rightWheel = rightWheel;
            timer = new _Timer();
        }

        public BasicBoeBot(eEvent _event)
            : base(_event)
        {
            BasicBot(
                       ***   // 前進
                     , ***   // 旋回
                     , ***   // 回転
                     , new BasicWheelServo(
                           CPU.P13   // pin
                         , ***         // forward
                         , ***         // center
                         , ***         // backward
                         , 20000        // low
                         )
                     , new BasicWheelServo(
                           CPU.P12   // pin
                         , ***         // forward
                         , ***         // center
                         , ***         // backward
                         , 20000        // low
                         )
                     );
        }

        /**
         * 動作が完了したかどうかチェックする。
         * これは戻り値がtrueを返すまで呼ぶ。
         *
         * 戻り:もし動作が待ち状態を完了した時にtrueを返す。
         * BoeBotInterface に　public abstract bool movementDone () ;　がある
         */
        public override bool movementDone()
        {
            return timer.timeout ( timeout ) ;
            //return Done;
        }

        /**
         * 前進する車輪スピードを設定する。
         *
         * 入力:int steps: 何センチ進むか
         */
        public override void move(int steps)
        {
            movement(movementMove, steps);
        }

        /**
         * 左旋回(正数)あるいは右旋回(負数)に対する車輪スピードを設定する。
         *
         * 入力:int steps: 回転のステップ数
         */
        public override void pivot(int steps)
        {
            movement(movementPivot, steps);
        }

        /**
         * 左回転(正数)あるいは右回転(負数)に対する車輪スピードを設定する。
         *
         * 入力:int steps: 回転のステップ数
         */
        public override void turn(int steps)
        {
            movement(movementTurn, steps);
        }

        /**
         * 動作に対する車輪スピードを得る。
         * msecPerStep, leftMovementSpeed, rightMovementSpeedを設定する
         *
         * 入力:int movement: 動作のタイプ
         * 入力:int steps: 回転のステップ数
         */
        protected virtual void getMovementSpeed(int movement, int steps)
        {
            switch (movement)
            {
                default:
                case movementMove:
                    msecPerStep = msecPerCm;
// 注意：前進、後退、旋回、回転のスピードは固定になっています。
                    if (steps > 0)
                    {
                        leftMovementSpeed = 100;
                        rightMovementSpeed = 100;
                    }
                    else
                    {
                        leftMovementSpeed = -100;
                        rightMovementSpeed = -100;
                    }
                    break;

                case movementPivot:
                    msecPerStep = msecPerPivot;

                    if (steps > 0)
                    {
                        leftMovementSpeed = -100;
                        rightMovementSpeed = 100;
                    }
                    else
                    {
                        leftMovementSpeed = 100;
                        rightMovementSpeed = -100;
                    }
                    break;

                case movementTurn:
                    msecPerStep = msecPerTurn;

                    if (steps > 0)
                    {
                        leftMovementSpeed = 25;
                        rightMovementSpeed = 100;
                    }
                    else
                    {
                        leftMovementSpeed = 100;
                        rightMovementSpeed = 25;
                    }
                    break;
            }

            // Compute timeout when appropriate
            if ((steps != continuousForward)
                 || (steps != continuousBackward)
                 || (steps != 0))
            {
                timeout = msecPerStep * 10000* ((steps > 0) ? steps : -steps);
            }
        }

        /**
         * 動作に対する車輪スピードを設定する
         * 正数に対しては動作１を用いる
         * 負数に対しては動作２を用いる
         *
         * 入力:int movement: 動作タイプ
         * 入力:int steps: 動くステップ数
         */
        protected void movement(int movement, int steps)
        {
            // 引数を設定する
            getMovementSpeed(movement, steps);

            switch (steps)
            {
                case 0:
                    setSpeed(0, 0);
                    break;

                case continuousForward:
                case continuousBackward:
                    setSpeed(leftMovementSpeed, rightMovementSpeed);
                    break;

                default:
                    timer.mark () ;
                    setSpeed(leftMovementSpeed, rightMovementSpeed);
                    startMovement();
                    break;
            }
        }

        /**
         * 両輪に対するスピードを設定する。
         * 設定はパーセントである。
         * 正数は前進回転
         * 負数は後進回転
         *
         * 入力:int left: 左車輪に対するスピード
         * 入力:int right: 右車輪に対するスピード
         */
        protected virtual void setSpeed(int left, int right)
        {
            // Set real speed
            leftWheel.move(left);
            rightWheel.move(right);
        }
        public override void stop()
        {
            // Set real speed
            leftWheel.move(0);
            rightWheel.move(0);
        }
    }
}

using System;
/**
 * イベントが生じたことを示すためのインターフェース
 */
namespace BoeBotLib
{
    public class eEvent
    {
        public static readonly eEvent nullEvent = new eEvent();

        /**
         * null eventを作る。eEvent.nullEventを用いること。
         */
        protected eEvent()
        {

        }

        /**
         * イベント参照がnullかどうかチェックする。
         *
         * event:  チェックするevent
         *
         * returns:  もしnullだったらnullEventを返し、それ以外はeventを返す。
         */
        static public eEvent checkEvent(eEvent _event)
        {
            return (_event == null) ? nullEvent : _event;
        }

        /**
         * イベントを生じさせる。
         */
        public virtual void notify()
        {
            notify(null);
        }

        /**
         * イベントを生じさせる。
         */
        public virtual void notify(Object _object)
        {
        }

        /**
         * イベントの名前を得る。
         *
         * return: イベント名("Event")
         */
        public virtual String name()
        {
            return "Event";
        }
    }
}

/*
 * 協調的マルチタスク状態遷移マシンオペレーティングシステム
 * これはラウンドロビンタスクスケジューリングを用いた非常に
 * 基本的なOSです。
 * タスクリストは効率性の理由から循環的にリンクしています。
 *
 * 注意: もしTaskStatusクラスが用いないで、数バイトを付け加えれば
 *   　　コンストラクタの参照でtaskStatusListとnextTaskStatus
 *　 　　を削除可能になる。
 */
using System;
using Microsoft.SPOT;
/**
 * フリーサイクルのときはいつでもタスクが走ります。
 * 注意: 外部タスクを終了させる良い方法はtaskToAbort.nextState(abort)
 * を用いることです。代替手段はtaskToAbort.stop()ですが、これはメモリ
 * の掃除をしません。
 */
namespace BoeBotLib
{
    public abstract class Task : eEvent
    {
        /**
         * taskState() result
         */
        public const int taskRunning = 0;

        /**
         * taskState() result
         */
        public const int taskSuspended = 1;

        /**
         * taskState() result
         */
        public const int taskSemaphoreWait = 2;

        /**
         * Initial value of state in execute()
         */
        public const int initialState = 0;
        public const int checkTimer = -1; // sleep()のサポート用
        public const int _interrupt = -2; // InterruptTaskを見よ
        public const int terminate = -3;
        public const int stopped = -4;

        /**
         * sleep()要求によるタスクを扱うためのTimerオブジェクト
         */
        public _Timer timer = new _Timer();

        /**
         * 次のアクティブタスクは循環リストの中
         */
        protected static Task taskStatusList = null;

        /**
         * taskStatusListに対するリンクされたリスト
         */
        protected Task nextTaskStatus = null;

        /**
         * taskState()によって戻される値
         */
        public int _taskStatus;

        /**
         * 循環リスト上の次のアクティブリスト
         */
        public Task nextTask = null;

        /**
         * execute()の中で用いられる状態変数
         */
        protected int state = initialState;

        /**
         * タイムアウトが起こった後の次の状態
         */
        protected int sleepState = checkTimer;

        /**
         * Timout値
         */
        protected int hi, lo;

        /**
         * タスクのセットアップ(設定)と開始
         */
        public Task()
        {
            addTask(this);

            // タスクをステータスリストに加える
            nextTaskStatus = taskStatusList;
            taskStatusList = this;
        }

        /**
         * タスク名を得る。
         *
         * 戻り: String の"Task"
         */
        public override String name()
        {
            return "Task";
        }

        /**
         * タスクのステータスを得る
         *
         * 戻り:　タスクのステータス
         *        (taskRunning, taskSuspended, taskSemaphoreWait)
         */
        public int taskStatus()
        {
            return _taskStatus;
        }

        /**
         * 現在のタスクの状態を得る
         *
         * 戻り:　int タスクの状態
         */
        public int getState()
        {
            return state;
        }

        /**
         * もし、タスクが実行中ならばタスクマネージャーのタスクリスト
         * からタスクを削除する。
         * 任意のタスクから呼ぶことが出来る。
         *
         * 戻り:　boolean タスクがリストから削除されたときにtrue
         */
        public bool suspend()
        {
            if (_taskStatus == taskRunning)
            {
                // 注意: nextTaskがnullでなかったらマネージャーはnull
                // であってはいけない
                _taskStatus = taskSuspended;
                return removeTask(this);
            }
            return false;
        }

        /**
         * 停止するためにタスクを削除しstopの状態に設定する
         * 戻り:　boolean  停止に成功したらtrue
         */
        public bool stop()
        {
            if (suspend())
            {
                state = stopped;
                return true;
            }

            return false;
        }

        /**
         * もしタスクが実行していなかったならば、タスクマネージャーの
         * リストにタスクを戻して加える。
         * 任意のタスクから呼ぶことができるが、そのタスクはマネージャーを
         * 持っていなければならない
         *
         */
        public bool resume()
        {
            if (_taskStatus == taskSuspended)
            {
                // 注意: マネージャーはnextTaskがnullでなかったならばnullで
                //       であってはいけない
                addTask(this);
                return true;
            }
            return false;
        }

        /**
         * resume()と同じ
         */
        public bool start()
        {
            return resume();
        }

        /**
         * resume()と同じ
        */
        public override void notify(Object _object)
        {
            resume();
        }

        /**
         * 次のexecute()呼び出しに対して状態を設定する
         *
         */
        public void nextState(int state)
        {
            this.state = state;
        }

        /**
         * 次のexecute()呼び出しに対して状態を設定する
         * タスクをSuspendする。他のタスクによってresumeされる
         */
        public void sleep(int state)
        {
            this.state = state;
            suspend();
        }

        /**
         * poll timerを仕掛けて設定単位時間を経過後に次の状態を続ける
         *
         * 入力： int nextState タイムアウトが起こったときに入る状態
         * 入力： int hi　タイムアウト値。詳細はTimer.timeout(hi,lo)を見よ     * 入力： int lo  タイムアウト値
         */
        public void sleep(int nextState, int hi, int lo)
        {
            this.hi = hi;
            this.lo = lo;
            timer.mark();
            sleepState = nextState;
            state = checkTimer;
        }

        /**
         * poll timerを仕掛けて設定単位時間を経過後に次の状態を続ける
         * 入力： int nextState 　タイムアウトが起こったときに入る状態
         * 入力： int msec ミリ秒単位でのタイムアウト値
         */
        public void sleep(int nextState, int timeMS)
        {
            //sleep ( nextState, timeMS/569, (timeMS%569)*119) ;
            this.hi = timeMS*10000; // tickの単位は0.1μ秒
            timer.mark();
            sleepState = nextState;
            state = checkTimer;
        }

        /**
         * poll timerを仕掛けて設定単位時間を経過後に次の状態を続ける
         * 入力： int nextState 　タイムアウトが起こったときに入る状態
         * 入力： int timeS 秒単位でのタイムアウト値
         */
        public void sleepSec(int nextState, int timeS)
        {
            sleep(nextState, (timeS * 18) / 10, 0);
        }


        /**
         * 1期間タスクを実行する。全てのサブクラスはこのメソッドで
         * 実装されなければいけない
         * GUIがアイドルのときはいつでもアイドルタスクが実行される。
         * execute()の実装はインターフェースが応答可能にするために
         * 100ms以上かかってはいけない。
         */
        protected abstract void execute();


        // タスクマネージャーサポート

        /**
         * クラス変数: タスクリストの現在のタスク
         */
        static protected Task taskList = null;

        /**
         * 現在のアクティブタスク
         */
        public static Task currentTask = null;

        /**
         * 現在実行しているタスクオブジェクトを得る
         *
         * 戻り：Task 現在実行しているタスク
         */
        static public Task getCurrentTask()
        {
            return currentTask;
        }

        /**
         * タスクリストにタスクを加える
         *
         * 入力：Task aNewTask: タスクリストに加えるタスク
         */
        public static void addTask(Task aNewTask)
        {
            if (taskList == null)
            {
                aNewTask.nextTask = aNewTask;
            }
            else
            {
                aNewTask.nextTask = taskList.nextTask;
                taskList.nextTask = aNewTask;
            }

            aNewTask._taskStatus = taskRunning;
            taskList = aNewTask;
        }

        /**
         * タスクリストからタスクを削除する
         *
         * 入力：aTaskToRemove：タスクリストから削除するタスク
         *
         * 戻り：boolean リストからタスクが削除できたらtrue
         */
        public static bool removeTask(Task aTaskToRemove)
        {
            // 空のリストを始めに処理する

            if (taskList != null)
            {
                // 現在のタスクからリストを走査し見つかったらそれを削除する

                Task scanTask = taskList;

                do
                {
                    if (scanTask.nextTask == aTaskToRemove)
                    {
                        // 削除するタスクが見つかった

                        if (scanTask == aTaskToRemove)
                        {
                            // リストの中の一つだけのタスクを削除する
                            taskList = null;
                        }
                        else
                        {
                            // 現在のタスクが削除されたかどうかチェックする

                            if (taskList == aTaskToRemove)
                            {
                                taskList = aTaskToRemove.nextTask;
                            }

                            scanTask.nextTask = aTaskToRemove.nextTask;
                        }

                        aTaskToRemove.nextTask = null;
                        return true;
                    }

                    // リストの中の次のタスクをチェックする
                    scanTask = scanTask.nextTask;
                }
                while (scanTask != taskList); //全てのリストが走査されたら脱出
            }

            return false;
        }

        /**
         * ラウンドロビン法でタスクリストの中の各々のタスクを
         * 永久に実行する。実行するタスクが無くなった時に戻る
         */
        public static void TaskManager()
        {
            while (taskList != null)
            {
                // execute()がremoveTaskを通して変えることができるように
                // taskListを変える
                currentTask = taskList;
                taskList = taskList.nextTask;
                if (currentTask.state == checkTimer)
                {
                    // タスクがタイムアウトを待機している
                    if (currentTask.timer.timeout(currentTask.hi, currentTask.lo))
                    {
                        // タイムアウトが起こったら、次の状態を設定する
                        currentTask.state = currentTask.sleepState;

                        // もし、次の状態が不適切に設定されていたらば抜け出す
                        if (currentTask.state == checkTimer)
                        {
                            currentTask.state = terminate;
                        }
                    }
                }
                else
                {
                    // 次の状態を呼び出す
                    currentTask.execute();
                }
            }
        }
    }
}

using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.NetduinoMini;
using BoeBotLib;

namespace BasicBoeBotTest2
{
    public partial class Program
    {
        public static void Main()
        {
            new Program().main();
        }
    }
}

using System;

/**
 * 固定動作に対する車輪コントロールクラス
 *
 * 動作コントロールのためにBasicBoeBotクラスと組み合わせる
 */
namespace BoeBotLib
{
    public class FixedMovementBoeBot : eEvent
    {
        protected bool idle = true;

        /**
         * 動作が開始された時に呼ばれる
         */
        public override void notify(Object jbot)
        {
            if (idle && (jbot != null))
            {
                idle = false;
                while (!((BoeBotInterface)jbot).movementDone())
                {
                }
                idle = true;
            }
        }
    }
}

using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.NetduinoMini;
using BoeBotLib;

namespace BasicBoeBotTest2
{
    public partial class Program
    {
        public void main()
        {
            BasicBoeBot jbot = new BasicBoeBot(new FixedMovementBoeBot());

            jbot.move(10);  // forward
            jbot.pivot(-2); // pivot right
            jbot.turn(2);   // turn left
            jbot.stop();
        }
    }
}

using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.NetduinoMini;
using BoeBotLib;

namespace BasicBoeBotTest3
{
    public partial class Program
    {
        public static void Main()
        {
            new Program().main();
        }
    }
}

using BoeBotLib;
/**
 * Rampingを用いて固定動作をする車輪コントロールクラス
 * 
 * 動作コントロールに対するBasicBoeBotクラスを用いる
 */

public class RampingBoeBot : BasicBoeBot {
  private const int rampTimeout = 5 ; // milliseconds
  private const int maxRampCount = 3 ;

  protected bool decelerate = false ;
  protected int rampCount = 0 ;
  protected int rampLeftStep ;
  protected int rampRightStep ;

  protected int leftSpeed = 0 ;
  protected int rightSpeed = 0 ;

  protected int currentMovement = movementNone ;
  protected int currentSteps ;
  protected int nextMovement    = movementNone ;
  protected int nextSteps ;

  /**
   * 一般的な動作のために車輪サーボを設定する
   * 前進はcmを単位にする。
   * 旋回と回転は普通45゜ステップを単位にする。
   * デフォールトのサーボモータの設定を用いる。
   *
   * 入力:Event event:動作が開始した時通知するイベント。nullにできる
   */
  public RampingBoeBot ( eEvent _event ) : base(_event)
  {
  }

  /**
   * 一般的な動きに対する車輪サーボモータを設定する。
   * 前進はcmを単位にする。
   * 旋回と回転は普通45゜ステップを単位にする。
   * デフォールトのサーボモータの設定を用いる。
   *
   * 入力:Event event:動作が開始した時通知するイベント。nullにできる
   * 入力:int msecPerCm:　直進の時の1cmあたりのミリ秒
   * 入力:int msecPerPivot: 旋回単位あたりのミリ秒
   * 入力:int msecPerTurn: 回転単位あたりのミリ秒
   * 入力:BasicWheelServo leftWheel:左車輪に対するBasicWheelServo
   * 入力:BasicWheelServo rightWheel:右車輪に対するBasicWheelServo
   */
  public RampingBoeBot
    ( eEvent _event
    , int msecPerCm
    , int msecPerPivot
    , int msecPerTurn
    , BasicWheelServo leftWheel
    , BasicWheelServo rightWheel
    ) :
    base ( _event, msecPerCm, msecPerPivot, msecPerTurn, leftWheel, rightWheel ) 
  {
  }

  /**
   * 次の動作が開始できるかチェックする。
   *
   * 戻り:もし次の動作メソッドが呼び出し可能のときtrue
   */
  public bool ready() {
    return nextMovement == movementNone ;
  }

  /**
   * 動作が完了したかどうかチェックする。
   * これは戻り値がtrueを返すまで呼ぶ。
   *
   * 戻り:もし動作が待ち状態を完了した時にtrueを返す。
   */
  public override bool movementDone () {
    if ( currentMovement == movementNone ) {
      if ( nextMovement == movementNone ) {
        // nothing left to do
        return true;
      } else {
        // 停止から新しい動作を開始する
        currentMovement = nextMovement ;
        currentSteps    = nextSteps ;
        nextMovement    = movementNone ;

        // Rampingを開始する
        startRamping () ;
      }
    } else {
      // currentMovement is active
      if ( rampCount > 0 ) {
        if ( timer.timeout ( rampTimeout )) {
          // Ramp timeout occurred. Adjust speed.
          -- rampCount ;

          if ( rampCount == 0 ) {
            if ( decelerate ) {
              // should be stopped, setup to accelerate
              decelerate = false ;
              startRamping () ;
            } else {
              // should be up to speed
              startFullSpeedMovement(true) ;
            }
          } else {
            // setup next ramp movement
            updateRampSpeed() ;
          }
        }
      } else {
        // Rampingはしない。動作ステータスをチェックする
        if ( base.movementDone ()) {
          getNextMovement ();
        }
      }
    }
    return false ;
  }

  /**
   * 次の動作の設定を得る
   */
  protected void getNextMovement () {
    // Get nextMovement
    if ( nextMovement == movementNone ) {
      // check if anything more to do
      if ( currentMovement == movementNone ) {
        return ;
      }

      if (    ( currentMovement == movementMove )
           && ( currentSteps == 0 )) {
        // Already stopped. Do nothing more
        currentMovement = movementNone ;
        return ;
      }

      // setup to stop if no movement acquired
      nextMovement = movementMove ;
      nextSteps    = 0 ;

      // Get next movement
      causeNextEvent () ;
    }

    // update steps, currentMovement must be updated later
    currentSteps = nextSteps ;

    // check if next movement or if need to decelerate
    if ( nextMovement == currentMovement ) {
      startFullSpeedMovement(false) ;
    } else {
      // decelerate first
      currentMovement = nextMovement ;
      rampDown();
    }
  }

  /**
   * currentMovementに対してRampingを開始する
   */
  protected void startRamping () {
    if ( currentSteps == 0 ) {
      base.move(0);  // stop servos
    } else {
      getMovementSpeed ( currentMovement, currentSteps ) ;

      rampTo ( leftMovementSpeed, rightMovementSpeed ) ;
    }
  }

  /**
   * 減速に対するRampingを開始する
   */
  protected void rampDown () {
    decelerate = true ;
    rampTo ( 0, 0 ) ;
  }

  /**
   * 動作を開始する。Rampingはすでに完了
   *
   * 入力:boolean ramping:Ramping時間が引かれるべき時にtrue
   */
  protected void startFullSpeedMovement ( bool ramping ) {
    int timeoutAdjust = 0 ;

    switch ( currentMovement ) {
    case movementMove:
      base.move ( currentSteps ) ;
      timeoutAdjust = msecPerStep - 2 ;
      break;

    case movementPivot:
      base.pivot ( currentSteps ) ;
      timeoutAdjust = msecPerStep - 2 ;
      break;

    case movementTurn:
      base.turn ( currentSteps ) ;
      timeoutAdjust =   ( currentSteps == 1 )
                      ? (( 3 * msecPerStep ) / 8 )
                      : ( msecPerStep / 2 ) ;
      break;
    }

    // adjust timeout for ramping
    if ( ramping ) {
      timeout -= timeoutAdjust ;
    }

    // Reset next movement
    nextMovement = movementNone ;
  }

  /**
   * 目的のスピードにRampingするための設定
   *
   * 入力:int left:最終的な左のスピード
   * 入力:int right:最終的な右のスピード
   */
  protected void rampTo ( int left, int right ) {
    rampCount      = maxRampCount - 1 ; // ramp down count less current step
    rampLeftStep   = ( left  - leftSpeed )  / maxRampCount ;
    rampRightStep  = ( right - rightSpeed ) / maxRampCount ;
    updateRampSpeed() ;
  }

  /**
   * rampパラメータを用いてスピードをアップデートする
   */
  protected void updateRampSpeed () {
    // set timeout mark for next ramp speed update
    timer.mark();

    // 新しいスピードに設定する
    setSpeed(leftSpeed+rampLeftStep,rightSpeed+rampRightStep) ;
  }

  /**
   * 前進するため車輪のスピードを設定する
   *
   * 入力:int cm: 移動する直線距離
   */
  public override void move ( int cm ) {
    setNextMovement ( movementMove, cm ) ;
  }

  /**
   * 左旋回するように車輪スピードを設定する
   *
   * 入力:int steps: 回転するステップ数
   */
  public override void pivot ( int steps ) {
    setNextMovement ( movementPivot, steps ) ;
  }

  /**
   * 左回転するように車輪スピードを設定する
   *
   * 入力:int steps: 回転するステップ数
   */
  public override void turn(int steps)
  {
    setNextMovement ( movementTurn, steps ) ;
  }

  /**
   * 次の動作が起こるための設定
   * 動作はmovementDone()を呼ぶことを通して開始される
   *
   * 入力:int movement: 行う次の動作
   * 入力:int steps: 回転するためのステップ
   */
  protected void setNextMovement ( int movement, int steps ) {
    nextMovement = movement ;
    nextSteps    = steps ;

    // 現在停止中ならば動作を開始する
    if ( currentMovement == movementNone ) {
      startMovement();
    }
  }

  /**
   * 両輪のスピードを設定する
   * 設定はパーセントである
   * 正の値は前進
   * 負の値は後進
   *
   * 入力:int left:　左車輪のスピード
   * 入力:int right: 右車輪のスピード
   */
  protected override void setSpeed(int left, int right)
  {
    // Set real speed
    base.setSpeed(left,right);
    leftSpeed  = left ;
    rightSpeed = right ;
  }

  /**
   * 両輪のスピードを調整する
   * 設定はパーセントである
   * 正の値は前進
   * 負の値は後進
   *
   * 入力:int left:　左車輪のスピード補正
   * 入力:int right: 右車輪のスピード補正
   */
  protected void adjustSpeed ( int left, int right ) {
    base.setSpeed
      ( leftSpeed  + (( leftSpeed  * -left  ) / 100 )
      , rightSpeed + (( rightSpeed * -right ) / 100 ));
  }
}

using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.NetduinoMini;
using BoeBotLib;

namespace BasicBoeBotTest3
{
    public partial class Program
    {
        public void main()
        {
            RampingBoeBot jbot = new RampingBoeBot(new FixedMovementBoeBot());

            jbot.move(10);  // forward
            jbot.pivot(-2); // pivot right
            jbot.turn(2);   // turn left
            jbot.stop();
        }
    }
}

using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.NetduinoMini;
using BoeBotLib;

namespace BasicBoeBotTest4
{
    public partial class Program
    {
        /**
         * 配列に格納されているコマンドを用いてのBoe-Botを動かす
         * 
         * RampingBoeBotクラスメソッドとマンド表を用いてBoe-Botを走らせる
         */
        const int move  = -1;
        const int pivot = -2;
        const int turn  = -3;

        RampingBoeBot jbot = new RampingBoeBot(new FixedMovementBoeBot());

        static int[] movements = { move, 10, pivot, -2, move, 20 };
        public void main()
        {
            performMovements(movements);
        }

        public void performMovements(int[] movements)
        {
            for (int i = 0; i < movements.Length; i += 2)
            {
                switch (movements[i])
                {
                    case move:
                        jbot.move(movements[i + 1]);
                        break;

                    case pivot:
                        jbot.pivot(movements[i + 1]);
                        break;

                    case turn:
                        jbot.turn(movements[i + 1]);
                        break;
                }
            }

            jbot.stop();
        }
    }
}

/**
 * 距離センサーテスト
 * 
 * 距離センサーステータスを表示する
 */

namespace distanceTest1
{
    public class Program
    {
        public static void Main()
        {
            ADcon right = new ADcon(CPU.P0);
            ADcon left = new ADcon(CPU.P1);
            while (true)
            {
                Debug.Print("R:"+ right.Value + " L:"+left.Value);
                CPU.delay(500);
            }
        }
    }
}

/**
 * 物体回避テスト
 */
namespace whisker2
{
    public partial class Program
    {
        public void main()
        {
            BoeBotInterface jbot = new RampingBoeBot(new FixedMovementBoeBot());
            ADcon right = new ADcon(CPU.P0);
            ADcon left  = new ADcon(CPU.P1);

            while (true)
            {
                int LeftRight = ((right.GetValue() > 400) ? 0 : 2)+((left.GetValue() > 400) ? 0 : 1);
                switch (LeftRight)
                {
                    case 0:   // both low, 後進して右旋回
                        jbot.stop();
                        jbot.move(-3);
                        jbot.pivot(-4);
                        break;

                    case 1:   // P5 low, 後進して左旋回
                        jbot.stop();
                        jbot.move(-2);
                        jbot.pivot(2);
                        break;

                    case 2:   // P4 low, 後進して右旋回
                        jbot.stop();
                        jbot.move(-2);
                        jbot.pivot(-2);
                        break;

                    case 3:   // ぶつかっていない、直進
                        jbot.move(2);
                        break;
                }
            }
        }
    }
}