import stamp.core.*;
import JBot.* ;

/**
 * 車輪サーボモータの距離テストプログラム
 * 
 * 決められた時間J-Botを走らせる。
 * 各々の時間は以前の時間より長い。
 * 測定のためテスト走行の前に遅延を挿入している。
 * 距離測定は各々の動作の間で行う。
 */

public class BasicWheelServoDistanceTest1 {
  public static void main () {
    BasicWheelServo leftWheel =
      new BasicWheelServo (
              CPU.pin13   // pin
            ,  ***          // forward
            ,  ***         // center
            ,  ***         // backward
            , 2000        // low
            ) ;
    BasicWheelServo rightWheel =
      new BasicWheelServo (
              CPU.pin12   // pin
            ,  ***         // forward
            ,  ***         // center
            ,  ***         // backward
            , 2000        // low
            ) ;

    for ( int i = 1 ; i <= 10 ; ++i ) {
      CPU.delay(30000);     // 測定のための初期遅延
      CPU.delay(30000);     // 測定のための初期遅延

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

      for ( int j = 0 ; j < i ; ++ j ) {
        CPU.delay(5000);   // 決められた時間サーボを動かす
      }

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

import stamp.core.*;
import JBot.* ;

/**
 * 車輪サーボモータの距離テストプログラム
 * 
 * 決められた距離だけ進む。
 * 距離測定は各々の動きの間で行える。
 * 5cmあるいは2cm毎に動く。
 */

public class BasicWheelServoDistanceTest2 {
  public static void main () {
    BasicWheelServo leftWheel =
      new BasicWheelServo (
              CPU.pin13   // pin
            ,  ***         // forward
            ,  ***         // center
            ,  ***         // backward
            , 2000        // low
            ) ;
    BasicWheelServo rightWheel =
      new BasicWheelServo (
              CPU.pin12   // pin
            ,  ***         // forward
            ,  ***         // center
            ,  ***         // backward
            , 2000        // low
            ) ;

    for ( int i = 1 ; i <= 10 ; ++i ) {
      CPU.delay(30000);     // 測定のための初期遅延
      CPU.delay(30000);     // 測定のための初期遅延
      leftWheel.move ( 100 ) ;
      rightWheel.move ( 100 ) ;

      for ( int j = 0 ; j < i ; ++ j ) {
        CPU.delay(***);   // 決められた時間サーボを動かす
      }

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

import stamp.core.*;
import JBot.* ;

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

public class BasicWheelServoPivotTest1 {
  public static void main () {
    BasicWheelServo leftWheel =
      new BasicWheelServo (
              CPU.pin13   // pin
            ,  ***         // forward
            ,  ***         // center
            ,  ***         // backward
            , 2000        // low
            ) ;
    BasicWheelServo rightWheel =
      new BasicWheelServo (
              CPU.pin12   // pin
            ,  ***         // forward
            ,  ***         // center
            ,  ***         // backward
            , 2000        // low
            ) ;

    for ( int i = 1 ; i <= 4 ; ++i ) {
      CPU.delay(20000);     // 測定のための初期遅延

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

//      for ( int j = 0 ; j < i ; ++ j ) {
          CPU.delay(1000);   // 決められた時間サーボを動かす
//      }

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

import stamp.core.*;
import JBot.* ;

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

public class BasicWheelServoPivotTest2 {
  public static void main () {
    BasicWheelServo leftWheel =
      new BasicWheelServo (
              CPU.pin13   // pin
            ,  ***         // forward
            ,  ***         // center
            ,  ***         // backward
            , 2000        // low
            ) ;
    BasicWheelServo rightWheel =
      new BasicWheelServo (
              CPU.pin12   // pin
            ,  ***         // forward
            ,  ***         // center
            ,  ***         // backward
            , 2000        // low
            ) ;

    for ( int i = 1 ; i <= 4 ; ++i ) {
      CPU.delay(20000);     // 測定のための初期遅延

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

      for ( int j = 0 ; j < i ; ++ j ) {
        CPU.delay(2000);   // 決められた時間サーボを動かす
      }

      leftWheel.stop () ;   // stop wheels
      rightWheel.stop () ;
    }
  }
}

import stamp.core.*;
import JBot.* ;

/**
 * 回転テストプログラム
 * 
 * 決められた時間J-Botを回転させる
 */

public class BasicWheelServoTurnTest1 {
  public static void main () {
    BasicWheelServo leftWheel =
      new BasicWheelServo (
              CPU.pin13   // pin
            ,  ***         // forward
            ,  ***         // center
            ,  ***         // backward
            , 2000        // low
            ) ;
    BasicWheelServo rightWheel =
      new BasicWheelServo (
              CPU.pin12   // pin
            ,  ***         // forward
            ,  ***         // center
            ,  ***         // backward
            , 2000        // low
            ) ;

    for ( int i = 1 ; i <= 4 ; ++i ) {
      CPU.delay(20000);     // 測定のための初期遅延

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

      for ( int j = 0 ; j < i ; ++ j ) {
        CPU.delay(10000);   // 決められた時間サーボを動かす
      }

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

package JBot;

import stamp.core.*;
import stamp.util.os.*;

/**
 * J-Botの車輪コントロールインターフェース
 * 
 * J-Botの車輪コントロールクラスが備えていなければならない
 * メソッドを定義する
 */

public abstract class JBotInterface {
  protected Event startEvent ;
  protected Event nextEvent = Event.nullEvent ;
  protected Event oneTimeEvent = Event.nullEvent ;

  static final public int continuousForward  = 32760 ;
  static final public int continuousBackward = -32760 ;
  static final public int continuousLeft     = continuousForward ;
  static final public int continuousRight    = continuousBackward ;

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

  /**
   * 動作を終了するために現在のタスクを待機(wait)に設定する。
   *
   * 入力:int state: 次のタスク状態
   */
  public void wait ( int state ) {
    Task waitingTask = Task.getCurrentTask () ;
    waitingTask.nextState ( state ) ;

    if ( waitingTask.taskStatus()==Task.taskRunning) {
      waitingTask.suspend () ;
      oneTimeEvent = waitingTask ;
    }
  }

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

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

    // Prime the pump if necessary
    if ( movementDone ()) {
      causeNextEvent () ;
    }

    return resultEvent ;
  }

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

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

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

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

  /**
   * 動作を止める。
   */
  public 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 ) ;
}

package JBot;

import stamp.core.*;
import stamp.util.os.*;

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

public class BasicJBot extends JBotInterface {
  public BasicWheelServo leftWheel ;
  public BasicWheelServo rightWheel ;
  public Timer timer ;
  public int timeout ;
  public int msecPerCm ;
  public int msecPerPivot ;
  public int msecPerTurn ;

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

  protected int leftMovementSpeed ;
  protected int rightMovementSpeed ;
  protected int msecPerStep ;

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

                       // 前進
                    , ***  // ( 641 * 100us ) / 1000us = 64 msec
                       // 旋回
                    , ***  // ( 1800 * 100us ) / 1000us = 180 msec
                       // 回転
                    , ***  // ( 6500 * 100us ) / 1000us = 650 msec
                    , new BasicWheelServo (
                          CPU.pin13   // pin
                        ,  ***         // forward
                        ,  ***         // center
                        ,  ***         // backward
                        , 2000        // low
                        )
                    , new BasicWheelServo (
                          CPU.pin12   // pin
                        ,  ***         // forward
                        ,  ***         // center
                        ,  ***         // backward
                        , 2000        // low
                        )
                    ) ;
  }

  /**
   * 一般的な動きに対する車輪サーボモータを設定する。
   * 前進はcmを単位にする。
   * 旋回と回転は普通45゜ステップを単位にする。
   *
   * 入力:int msecPerCm:　直進の時の1cmあたりのミリ秒
   * 入力:int msecPerPivot: 旋回単位あたりのミリ秒
   * 入力:int msecPerTurn: 回転単位あたりのミリ秒
   * 入力:BasicWheelServo leftWheel:左車輪に対するBasicWheelServo
   * 入力:BasicWheelServo rightWheel:右車輪に対するBasicWheelServo
   */
  public BasicJBot
    ( Event event
    , int msecPerCm
    , int msecPerPivot
    , int msecPerTurn
    , BasicWheelServo leftWheel
    , BasicWheelServo rightWheel
    ) {
    super(event);
    this.msecPerCm  = msecPerCm ;
    this.msecPerPivot = msecPerPivot ;
    this.msecPerTurn  = msecPerTurn ;
    this.leftWheel    = leftWheel ;
    this.rightWheel   = rightWheel ;
    timer = new Timer () ;
  }

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

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

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

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

  /**
   * 動作に対する車輪スピードを得る。
   * msecPerStep, leftMovementSpeed, rightMovementSpeedを設定する
   *
   * 入力:int movement: 動作のタイプ
   * 入力:int steps: 回転のステップ数
   */
  protected 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  =   30 ;
        rightMovementSpeed =  100 ;
      } else {
        leftMovementSpeed  =  100 ;
        rightMovementSpeed =   10 ;
      }
      break;
    }

    // Compute timeout when appropriate
    if (    ( steps != continuousForward )
         || ( steps != continuousBackward )
         || ( steps != 0 )) {
      timeout = msecPerStep * (( 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 void setSpeed ( int left, int right ) {
    // Set real speed
    leftWheel.move(left);
    rightWheel.move(right);
  }
}

import stamp.core.*;
import JBot.* ;

/**
 * BasicJBotクラスをテストする
 * 
 * BasicJBotクラスメソッドを用いてJ-Botを走らせる。
 */

public class BasicJBotTest1 {
  public static void main () {
    BasicJBot jbot = new BasicJBot (null) ;

    jbot.move ( 10 ) ;
    while ( ! jbot.movementDone ()) ;

    jbot.pivot ( -2 ) ;
    while ( ! jbot.movementDone ()) ;

    jbot.turn ( 2 ) ;
    while ( ! jbot.movementDone ()) ;

    jbot.stop () ;
    while ( ! jbot.movementDone ()) ;
  }
}

/*
 * 協調的マルチタスク状態遷移マシンオペレーティングシステム
 * タスクはEventのサブクラスでタスクはEventによって再開される
 */

package stamp.util.os;

import stamp.util.*;
import stamp.core.*;

/**
 * イベントが生じたことを示すためのインターフェース
 */

 public class Event {
  static final public Event nullEvent = new Event () ;

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

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

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

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

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

/*
 * 協調的マルチタスク状態遷移マシンオペレーティングシステム
 * これはラウンドロビンタスクスケジューリングを用いた非常に
 * 基本的なOSです。
 * タスクリストは効率性の理由から循環的にリンクしています。
 *
 * 注意: もしTaskStatusクラスが用いないで、数バイトを付け加えれば
 *   　　コンストラクタの参照でtaskStatusListとnextTaskStatus
 *　 　　を削除可能になる。
 */

package stamp.util.os;

import stamp.util.*;
import stamp.core.*;

/**
 * フリーサイクルのときはいつでもタスクが走ります。
 * 注意: 外部タスクを終了させる良い方法はtaskToAbort.nextState(abort)
 * を用いることです。代替手段はtaskToAbort.stop()ですが、これはメモリ
 * の掃除をしません。
 */

 public abstract class Task extends Event {
  /**
   * taskState() result
   */
  final public static int taskRunning       = 0 ;

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

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

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

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

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

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

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

  /**
   * 循環リスト上の次のアクティブリスト
   */
  protected 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 String name () {
    return "Task" ;
  }

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

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

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

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

    return false ;
  }

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

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

  /**
   * resume()と同じ
  */
  public 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)*115) ;
  }

  /**
   * 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;

  /**
   * 現在のアクティブタスク
   */
  static protected 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 boolean 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 () ;
      }
    }
  }
}

package JBot;

import stamp.core.*;
import stamp.util.os.*;

/**
 * 固定動作に対する車輪コントロールクラス
 * 
 * 動作コントロールのためにBasicJBotクラスと組み合わせる
 */

public class FixedMovementJBot extends Event {
  protected boolean idle = true ;

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

import stamp.core.*;
import JBot.* ;

/**
 * FixedMovementJBotクラスをテストする
 * 
 * BasicJBotクラスメソッドを用いてJ-Botを走らせる
 */

public class BasicJBotTest2 {
  public static void main () {
    BasicJBot jbot = new BasicJBot ( new FixedMovementJBot ()) ;

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

package JBot;

import stamp.core.*;
import stamp.util.os.*;

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

public class RampingJBot extends BasicJBot {
  static final private int rampTimeout = 35 ; // milliseconds
  static final private int maxRampCount = 3 ;

  protected boolean 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 RampingJBot ( Event event ) {
    super ( event ) ;
  }

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

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

  /**
   * 動作が完了したかどうかチェックする。
   * これは戻り値がtrueを返すまで呼ぶ。
   *
   * 戻り:もし動作が待ち状態を完了した時にtrueを返す。
   */
  public boolean 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 ( super.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 ) {
      super.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 ( boolean ramping ) {
    int timeoutAdjust = 0 ;

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

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

    case movementTurn:
      super.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 void move ( int cm ) {
    setNextMovement ( movementMove, cm ) ;
  }

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

  /**
   * 左回転するように車輪スピードを設定する
   *
   * 入力:int steps: 回転するステップ数
   */
  public 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 void setSpeed ( int left, int right ) {
    // Set real speed
    super.setSpeed(left,right);
    leftSpeed  = left ;
    rightSpeed = right ;
  }

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

import stamp.core.*;
import JBot.* ;

/**
 * RampingJBotクラスをテストする
 * 
 * BasicJBotクラスメソッドを用いてJ-Botを走らせる
 */

public class BasicJBotTest3 {
  public static void main () {
    RampingJBot jbot = new RampingJBot ( new FixedMovementJBot ()) ;

    jbot.move ( -5 ) ;        // 後進
    jbot.stop () ;

    jbot.move ( 10 ) ;
    jbot.pivot ( -2 ) ;
    jbot.turn ( 2 ) ;
    jbot.stop () ;
  }
}

import stamp.core.*;
import JBot.* ;

/**
 * 配列に格納されているコマンドを用いてのJ-Botを動かす
 * 
 * RampingJBotクラスメソッドとマンド表を用いてJ-Botを走らせる
 */

public class BasicJBotTest4 {
  static final byte move = -1 ;
  static final byte pivot = -2 ;
  static final byte turn = -3 ;

  RampingJBot jbot = new RampingJBot (new FixedMovementJBot ()) ;

  static byte movements [] = { move, 10, pivot, -2, move, 20 } ;

  public void performMovements ( byte 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 () ;
  }


  // ----  メインプログラム ----

  public static void main () {
    BasicJBotTest4 myJBot = new BasicJBotTest4 () ;

    myJBot.performMovements ( movements ) ;
  }
}

import stamp.core.*;

/**
 * 触覚スイッチテスト
 * 
 * 触覚スイッチステータスを表示する
 */

public class whisker1 {
  public static void main() {
    CPU.setInput ( CPU.pin4 ) ;    // ピンを入力に指定する
    CPU.setInput ( CPU.pin6 ) ;
    while ( true ) {
      System.out.print   ( "P6=" ) ;
      System.out.print   ( CPU.readPin ( CPU.pin6 ) ? 1 : 0 ) ;
      System.out.print   ( "  P4=" ) ;
      System.out.println ( CPU.readPin ( CPU.pin4 ) ? 1 : 0 ) ;
      CPU.delay ( 5000 ) ;
    }
  }
}

import stamp.core.*;
import JBot.*;

/**
 * 触覚スイッチテスト
 */

public class whisker2 {
  public static void main() {
    JBotInterface jbot = new RampingJBot ( new FixedMovementJBot ()) ;
    CPU.setInput ( CPU.pin4 ) ;    // ピンを入力に指定する
    CPU.setInput ( CPU.pin6 ) ;

    while ( true ) {
      switch (   ( CPU.readPin ( CPU.pin6 ) ? 2 : 0 )
               + ( CPU.readPin ( CPU.pin4 ) ? 1 : 0 )) {
      case 0:   // both low, 後進して右旋回
        jbot.stop () ;
        jbot.move ( -3 );
        jbot.pivot ( -4 ) ;
        break ;

      case 1:   // P6 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 ;
      }
    }
  }
}