import stamp.core.*;

/**
 * IR距離測定テスト
 */

 public class IrRangeTest1 {
  static PWM pwmLeft ;
  static DAC dacLeft ;
  static PWM pwmRight ;
  static DAC dacRight ;

  public static int getRange ( DAC dac, PWM pwm, int detectorPin ) {
    int range = 0 ;

    // 仮想周辺装置を開始
    dac.start();
    pwm.start();

    // 光強度をステップアップする
    for (int i = 0; i < 15; i++){
      // 強度レベルを設定する
      dac.update(255 - (13*i));

      // 検出器を安定させる
      CPU.delay(20);

      // 範囲のレベルをカウントする
      if(CPU.readPin(detectorPin)) {
        range++;
      }
    }

    // 仮想周辺装置を停止する
    dac.stop();
    pwm.stop();

    return range ;
  }

  public static void main() {
    pwmLeft  = new PWM(CPU.pin1,1,2);
    pwmLeft.stop();
    dacLeft  = new DAC(CPU.pin7);
    dacLeft.stop() ;
    pwmRight = new PWM(CPU.pin3,1,2);
    pwmRight.stop();
    dacRight = new DAC(CPU.pin8);
    dacRight.stop();

    while(true){
      System.out.print   ( "L " ) ;
      System.out.print   ( getRange ( dacLeft, pwmLeft, CPU.pin0 ));
      System.out.print   ( "   R " ) ;
      System.out.println ( getRange ( dacRight, pwmRight, CPU.pin2 ));
      }
  }

}

package JBot ;

import stamp.core.*;
import java.lang.Math.* ;

/**
 * IR Range Sensor Class
 * 
 * IR range sensors support
 */

public class IrRangeSensor extends BaseSensor {
  protected IrRangeSensorTask sensorTask ;

  protected int direction ;
  protected int distance ;
  protected boolean obstacleDetected = false ;
  protected int deadband ;

  static final int noObstacle = 15 ;

  /**
   * IR range sensorオブジェクトを作り、タスクをサポートする
   *
   * 入力:int leftDacPin: DAC出力に用いるCPU.pin
   * 入力:int leftPwmPin: PWM出力に用いるCPU.pin
   * 入力:int rightDacPin: DAC出力に用いるCPU.pin
   * 入力:int rightPwmPin: PWM出力に用いるCPU.pin
   * 入力:int deadband: deadband制限
   */
  public IrRangeSensor ( int leftDacPin
                       , int leftPwmPin
                       , int leftDetectorPin
                       , int rightDacPin
                       , int rightPwmPin
                       , int rightDetectorPin
                       , int deadband ) {
    sensorTask = new IrRangeSensorTask
                       ( this
                       , leftDacPin
                       , leftPwmPin
                       , leftDetectorPin
                       , rightDacPin
                       , rightPwmPin
                       , rightDetectorPin ) ;
    this.deadband = deadband ;
  }


  /**
   * obstacleDetectedが呼び出し可能かどうか調べる
   *
   * 戻り:obstacleDetectedが呼び出し可能の場合trueを返す
   */
  public boolean ready () {
    sensorTask.checkSensors() ;

    return ready ;
  }

  /**
   * 障害物が検出されたかどうか調べる
   * 通常、イベントを用いるのに対してポーリング時に用いる
   *
   * 戻り:障害物の検出値
   */
  public boolean obstacleDetected () {
    sensorTask.checkSensors() ;
    ready = false ;

    return obstacleDetected ;
  }

  /**
   * 障害物の初期位置を調べる
   * シンプルな検出システムでは左右の物体の検出は前方になります
   *
   * 戻り:障害物の相対方向 (left, right, etc.)
   */
  public int obstacleDirection () {
    return direction ;
  }

  /**
   * 指定された方向における障害物までの距離を得る
   * none値は物体は検出されなかったことを意味する
   *
   * 入力:int direction:距離を求める方向
   *
   * 戻り:指定された方向における障害物までの距離
   */
  public int obstacleDistance ( int direction ) {
    return distance ;
  }

  /**
   * センサー情報に基づいた結果を更新する
   * Called by sensor task when results available.
   *
   * 入力:int: resultLeft: photoresistorの rcTimeの結果
   * 入力:int: resultRight: photoresistorの rcTimeの結果
   */
  protected void saveResults ( int resultLeft, int resultRight ) {
    // 現在の結果が有効
    // 障害物のステータスを保存する
    switch (   (( resultLeft < noObstacle ) ? 1 : 0 )
             + (( resultRight < noObstacle ) ? 2 : 0 )) {
    default:
    case 0:
      obstacleDetected = false ;
      break;

    case 1:
      direction = left ;
      distance = resultLeft ;
      obstacleDetected = true ;
      break;

    case 2:
      direction = right ;
      distance = resultRight ;
      obstacleDetected = true ;
      break;

    case 3:
      // 両方のセンサーが障害物を認識
      if ( Math.abs ( resultLeft - resultRight ) < deadband ) {
        // 両方の距離は非常に近い
        direction = front ;
        distance = ( resultLeft > resultRight ) ? resultLeft : resultRight ;
      } else if ( resultLeft > resultRight ) {
        // 左のセンサーは高い値
        direction = left ;
        distance = resultLeft ;
      } else {
        // 右のセンサーは高い値
        direction = right ;
        distance = resultRight ;
      }
      obstacleDetected = true ;
      break;
    }

    ready = true ;
    notify();
  }
}

package JBot ;

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

/**
 * IR Range Sensor Class
 * 
 * IrRangeSensorをサポートする
 * 他のオブジェクトから直接呼んではいけない
 */

public class IrRangeSensorTask extends Task {
  protected IrRangeSensor sensor ;

  protected PWM leftPwm ;
  protected DAC leftDac ;
  protected int leftDetectorPin ;

  protected PWM rightPwm ;
  protected DAC rightDac ;
  protected int rightDetectorPin ;

  protected PWM pwm ;
  protected DAC dac ;
  protected int detectorPin ;

  protected int iteration ;
  protected int range ;
  protected int leftResult ;

  final static int startChecking = 1 ;
  final static int checkLeftPin = 2 ;
  final static int checkRightPin = 3 ;

  protected IrRangeSensorTask
              ( IrRangeSensor sensor
              , int leftDacPin
              , int leftPwmPin
              , int leftDetectorPin
              , int rightDacPin
              , int rightPwmPin
              , int rightDetectorPin ) {
    this.sensor = sensor ;
    leftPwm  = new PWM(leftPwmPin,1,2);
    leftPwm.stop();
    leftDac  = new DAC(leftDacPin);
    leftDac.stop() ;
    this.leftDetectorPin = leftDetectorPin ;

    rightPwm = new PWM(rightPwmPin,1,2);
    rightPwm.stop();
    rightDac = new DAC(rightDacPin);
    rightDac.stop();
    this.rightDetectorPin = rightDetectorPin ;
  }


  /**
   * 実行中でないかどうかセンサーをチェックする
   */
  protected void checkSensors() {
    if ( state == stopped ) {
      // タスクは終了しているので、再度開始する
      nextState (startChecking) ;
      start () ;
    }
  }

  /**
   * 電圧を変更し、delayを設定する
   */
  protected void changeVoltage () {
    dac.update(255 - (13*iteration));
    sleep(state,5);
  }

  /**
   * IRセンサーの動作を開始させる
   * checkRangeとchangeVoltageで用いるDAC,PWM,IR検出器のピンを保存する
   * 反復と距離カウンターをリセットする
   * PWMとDAC仮想周辺装置を開始する
   * 初期電圧とdelayを設定する
   *
   * 入力:DAC dac: DAC仮想周辺装置
   * 入力:PWM pwm: PWM仮想周辺装置
   * 入力:int detectorPin: IR検出器出力に接続されているピン
   * 入力:int nextState: delayの後の次の状態
   */
  protected void startPulse ( DAC dac, PWM pwm, int detectorPin, int nextState ) {
    iteration = 0 ;
    range = 0 ;

    this.dac = dac ;
    this.pwm = pwm ;
    this.detectorPin = detectorPin ;

    dac.start();
    pwm.start();
    nextState ( nextState ) ;
    changeVoltage () ;
  }

  /**
   * IR検出器をチェックする
   * チェックがされていなかった場合、電圧とdelayを変更する
   *
   * 戻り:チェックがされた場合 true
   */
  protected boolean checkRange () {
    if ( CPU.readPin ( detectorPin )) {
      ++ range ;
    }

    if ( iteration == 15 ) {
      // Done checking. Turn everything off
      pwm.stop();
      dac.stop();
      return true ;
    } else {
      // Update counter, change voltage and set delay
      ++ iteration ;
      changeVoltage () ;
      return false ;
    }
  }

  /**
   * マルチタスク・サポート
   */
  public void execute () {
    switch ( state ) {
    case startChecking:
      // 左のDAC,PWMを開始しdelayする
      startPulse ( leftDac, leftPwm, leftDetectorPin, checkLeftPin );
      break;

    case checkLeftPin:
      if ( checkRange ()) {
        // 結果を保存する
        leftResult = range ;

      // 右のDAC,PWMを開始しdelayする
        startPulse ( rightDac, rightPwm, rightDetectorPin, checkRightPin );
      }
      break;

    case checkRightPin:
      if ( ! checkRange ()) {
        // Delayを設定しメソッドを出る
        break;
      }

      // 結果を保存し、タスクを停止させる
      sensor.saveResults ( leftResult, range ) ;

    default:
    case initialState:
      stop() ;
      break;
    }
  }
}

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

import JBot.*;

/**
 * IR Range Sensor Test
 */

public class IrRangeSensorTest1 extends Task {
  protected IrRangeSensor sensor =
    new IrRangeSensor ( CPU.pin7    // left DAC pin
                      , CPU.pin1    // left PWM pin
                      , CPU.pin0    // left detector pin
                      , CPU.pin8    // right DAC pin
                      , CPU.pin3    // right PWM pin
                      , CPU.pin2    // right detector pin
                      , 2           // deadband
                      ) ;

  public void execute() {
    if ( sensor.obstacleDetected ()) {
      int direction = sensor.obstacleDirection () ;

      System.out.print   ( "Dir=" ) ;
      System.out.print   ( direction ) ;
      System.out.print   ( "   Range=" ) ;
      System.out.println ( sensor.obstacleDistance (direction)) ;
    } else {
      System.out.println ( "none" ) ;
    }

    sleep(state,200);
  }


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

    Task.TaskManager();
    System.out.println ( "Should not have terminated!" ) ;
  }

}

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

/**
 * AvoidObstacleTaskクラスをテストする
 * 
 * 障害物を避けてJ-Botを回転させる
 */

public class AvoidObstacleTaskIrRangeTest1 {
  public static void main () {
    new AvoidObstacleTask
      ( new IrRangeSensor ( CPU.pin7    // left DAC pin
                          , CPU.pin1    // left PWM pin
                          , CPU.pin0    // left detector pin
                          , CPU.pin8    // right DAC pin
                          , CPU.pin3    // right PWM pin
                          , CPU.pin2    // right detector pin
                          , 1           // deadband
                          )
      , new RampingJBot ( new MultitaskingJBot ())) ;

    Task.TaskManager () ;
    System.out.println ( "All done" ) ;
  }
}

package JBot ;

import stamp.core.*;
import java.lang.Math.* ;

/**
 * 落下を検出するIR Range Sensorクラス
 * 
 * IR range sensors support
 */

public class IrRangeDropOffSensor extends IrRangeSensor {
  private int direction ;
  private int threshold ;

  /**
   * IR range sensorオブジェクトを作り、タスクをサポートする
   *
   * 入力:int leftDacPin: DAC出力に用いるCPU.pin
   * 入力:int leftPwmPin: PWM出力に用いるCPU.pin
   * 入力:int rightDacPin: DAC出力に用いるCPU.pin
   * 入力:int rightPwmPin: PWM出力に用いるCPU.pin
   * 入力:int deadband: deadband制限
   */
  public IrRangeDropOffSensor
                       ( int leftDacPin
                       , int leftPwmPin
                       , int leftDetectorPin
                       , int rightDacPin
                       , int rightPwmPin
                       , int rightDetectorPin
                       , int deadband ) {
    super ( leftDacPin
          , leftPwmPin
          , leftDetectorPin
          , rightDacPin
          , rightPwmPin
          , rightDetectorPin
          , deadband ) ;

    // J-Botは落下を見ていないと仮定する

    if ( super.obstacleDetected ()) {
      threshold = 10 +
        super.obstacleDistance ( super.obstacleDirection ()) ;
    } else {
      // Use default otherwise

      threshold = 20 ;
    }
  }


  /**
   * obstacleDetectedが呼び出し可能かどうか調べる
   *
   * 戻り:obstacleDetectedが呼び出し可能の場合trueを返す
   */
  public boolean obstacleDetected () {
    int distance ;

    // 障害物のステータスは普通のセンサーと逆

    if ( super.obstacleDetected ()) {
      direction = super.obstacleDirection () ;
      distance  = super.obstacleDistance ( direction ) ;

      // 落下が検出されない場合、距離は閾値よりも低い
      if ( distance > threshold ) {
        // 障害物の方向は下を見ている時は逆
        switch ( direction ) {
        case left:
          direction = right ;
          break;

        case right:
          direction = left ;
          break;

        default:
          direction = front ;
          break;
        }

        return true ;
      }
    }

    return false ;
  }

  /**
   * 障害物の初期位置を調べる
   * シンプルな検出システムでは左右の物体の検出は前方になります
   *
   * 戻り:障害物の相対方向 (left, right, etc.)
   */
  public int obstacleDirection () {
    return direction ;
  }

  /**
   * 指定された方向における障害物までの距離を得る
   * none値は物体は検出されなかったことを意味する
   *
   * 入力:int direction:距離を求める方向
   *
   * 戻り:指定された方向における障害物までの距離
   */
  public int obstacleDistance ( int direction ) {
    return 0 ;
  }
}

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

/**
 * AvoidObstacleTaskクラスをテストする
 * 
 * 障害物を避けてJ-Botを回転させる
 */

public class AvoidDropOffTaskIrRangeTest1 {
  public static void main () {
    new AvoidObstacleTask
      ( new IrRangeDropOffSensor
                        ( CPU.pin7    // left DAC pin
                        , CPU.pin1    // left PWM pin
                        , CPU.pin0    // left detector pin
                        , CPU.pin8    // right DAC pin
                        , CPU.pin3    // right PWM pin
                        , CPU.pin2    // right detector pin
                        , 1 )           // deadband
      , new RampingJBot ( new MultitaskingJBot ())) ;

    Task.TaskManager () ;
    System.out.println ( "All done" ) ;
  }
}