J-Botの車輪に穴が開いているのが不思議だと思った人もいるでしょう。格好良くしているわけではありません。色と構成は赤外線反射物体センサー(フォトリフレクター)GP2S05を用いて車輪を動作させます。そのセンサーは赤外線センサーと前の実験で用いた検出器に似ているもので作られていますが、センサーはLEDの変調信号は必要ありません。 反射センサーは検出する物体が極めて近くで動作するように設計されており、この実験の場合では車輪です。センサーと車輪の距離は約6mmになります。センサーを近くで眺めてみると、LEDと検出器がお互いの方向に角度をもって向いていることが分かります。検出器とLEDの方向を伸ばして三角形を作った場合、その頂点は約6mmになります。しかしながらこのフォトリフレクターGP2S05は4mm程度の検出器なので感度を高くして使う必要があります。 特性表 |
車輪のスポークが黒であるために調整が難しいと思います。うまくいかない時は、車輪に紙でホイールキャップを作り、はめることにより光が反射し望んだ動作をします。上で説明したことの逆動作になります。つまり、紙で反射し、スポークでは反射しない。 |
車輪エンコーダーをテストするクラス |
---|
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(); } } |
変数 | 説明 |
---|---|
transition | エンコーダーの遷移が起こったことを示す |
leftState, rightState | 車輪エンコーダーからの最後の入力値 |
leftCount, rightCount | 車輪エンコーダーのカウンター |
leftRatio, rightRatio | 他の側に関係するステップ数 |
leftSpeed, rightSpeed | スピード・パーセンテージ(-100%~100%) |
leftAdjust, rightAdjust | スピード調節 |
LeftStep, rightStep | スピード調節に対する増減 |
leftLimit, rightLimit | 最大のスピード調整値 |
leftOdometer,rightOdometer | 距離計のカウンタ |
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 ; |
車輪エンコーダーを用いたJ-Bot車輪制御 |
---|
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"); } } |