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

/**
 * 車輪エンコーダーをテストするクラス
 * 
 * 赤外線反射センサーに対する初期テストプログラム
 * 車輪が穴の時0、穴でない時1
 */

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

    jbot.move ( JBotInterface.continuousForward ) ;

    while ( true ) {
      CPU.writePin(CPU.pin10,true);
      CPU.writePin(CPU.pin11,true);
      CPU.delay(10);
      System.out.print   ((CPU.rcTime(6,CPU.pin10,false)==-1)?0:1) ;
      System.out.print   (" ") ;
      System.out.println ((CPU.rcTime(6,CPU.pin11,false)==-1)?0:1) ;
    }
  }
}

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

/**
 * 車輪エンコーダーをテストするクラス
 * 
 * 赤外線反射センサーを用いた走行距離計テストプログラム
 */

public class WheelEncoderTest2 {
  static final int rightPin = CPU.pin10 ;
  static final int leftPin = CPU.pin11 ;

  public static boolean checkSensor( int pin ) {
    CPU.writePin(pin,true);
    CPU.delay(10);
    return CPU.rcTime(6,pin,false) != -1 ;
  }

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

    int rightCount = 0 ;
    int leftCount = 0 ;
    boolean rightState = checkSensor(rightPin) ;
    boolean leftState = checkSensor(leftPin) ;

    jbot.move ( JBotInterface.continuousForward ) ;

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

      if ( leftState != checkSensor(leftPin)) {
        leftState = ! leftState ;
        ++ leftCount ;
      }

      System.out.print   (rightCount) ;
      System.out.print   (" ") ;
      System.out.println (leftCount) ;
    }
  }
}

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

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

public class WheelEncoderTest3 extends BasicJBot {
  static final int rightPin = CPU.pin10 ;
  static final int leftPin = CPU.pin11 ;

  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 () {
    super ( new FixedMovementJBot ());
  }

  public static boolean checkSensor( int pin ) {
    CPU.writePin(pin,true);
    CPU.delay(10);
    return CPU.rcTime(6,pin,false) != -1 ;
  }


  void runTest () {
    boolean rightState = CPU.readPin(rightPin) ;
    boolean leftState = CPU.readPin(leftPin) ;

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

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

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

      if ( leftState != checkSensor(leftPin)) {
        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 ;
          }
        }

        // テスト時のみ
        System.out.print   (rightOdometer) ;
        System.out.print   (" ") ;
        System.out.print   (rightCount) ;
        System.out.print   (" ") ;
        System.out.print   (rightSpeed-rightAdjust) ;
        System.out.print   (" = ") ;
        System.out.print   (leftOdometer) ;
        System.out.print   (" ") ;
        System.out.print   (leftCount) ;
        System.out.print   (" ") ;
        System.out.print   (leftSpeed-leftAdjust) ;
        System.out.println (" ") ;
      }
    }

    stop();
  }

  public static void main () {
    WheelEncoderTest3 encoder = new WheelEncoderTest3 () ;

    encoder.runTest();
  }
}

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

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

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

package JBot;

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

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

public class WheelEncoderJBot extends RampingJBot {
  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 = 8 ; // = 16 / 21*10

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

  static public int transitionsPer10TurnSm = 10 ;
  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.pin10 ;
  protected int leftPin = CPU.pin11 ;

  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 ;              // 遷移が起こった

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


  /**
   * 車輪エンコーダーJ-Botサーボモーター制御オブジェクトを作る
   *
   * 入力: Event startEvent: 開始時に通知するイベント
   * 入力: int msecPerCm: 直線運動に対する1cmあたりのmsec数
   * 入力: int msecPerPivot:旋回単位あたりのmsec数
   * 入力: int msecPerTurn:回転単位あたりのmsec数
   * 入力: BasicWheelServo leftWheel:左車輪のBasicWheelServo
   * 入力: BasicWheelServo rightWheel:右車輪のBasicWheelServo
   * 入力: int leftInput:左車輪のエンコーダーの入力ピン
   * 入力: int rightInput:右車輪のエンコーダーの入力ピン
   */
  public WheelEncoderJBot
    ( Event event
    , int msecPerCm
    , int msecPerPivot
    , int msecPerTurn
    , BasicWheelServo leftWheel
    , BasicWheelServo rightWheel
    , int leftInput
    , int rightInput ) {
    super ( 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を返す
   */
  static protected boolean checkSensor ( int pin ) {
    // 現在のステータスを得る
    boolean result = CPU.rcTime(6,pin,false)!= -1 ;

    // 次の読みに対してコンデンサを充電する
    CPU.writePin(pin,true);

    // 現在の結果を返す
    return result;
  }

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

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

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

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

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

      // 左車輪のエンコーダー入力をチェックする
      if ( leftState != checkSensor(leftPin)) {
        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 void rampDown () {
    // 減速を設定する
    super.rampDown();

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

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

    // エンコーダーが動作を停止するようにramping動作を設定する
    super.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) ;
    leftState   = CPU.readPin(leftPin) ;
    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 ;
  }
}

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

/**
 * 車輪エンコーダークラスをテストする
 * 
 * WheelEncoderJBotとWheelEncoderTaskクラスをテストする
 */

public class WheelEncoderTest4 extends Event {
  WheelEncoderJBot jbot ;
  int state ;
  int i ;

  static final int moveForward = 0 ;
  static final int pivotLeft = 1 ;
  static final int done = 2 ;

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

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

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

  public static void main () {
    new WheelEncoderTest4 () ;

    Task.TaskManager () ;

    System.out.println("All Done");
  }
}