2008年 5月 28日 水曜日
[Sun SPOT でサーボカーを作ろう(6)] 距離センサー(Distance Sensor)を使って障害物の検出機能を追加してみる。
「Sun SPOT でリモコンカーサーボカーを作ろう」6回目です。今回は、以前も何度か使った（これや、これ）距離センサー（Distance Sensor）を使って、サーボカーに前方の障害物を検出する機能を追加してみます。
# おなじみの？距離センサー(SHARP GP2D12)
このサーボカーの前方に。。。
このように距離センサーを取り付けます。
Sun SPOTへのつなぎ方はこのエントリにも詳しく書いてありますが、距離センサーの出力をSun SPOTのアナログ入力ピンA0に、GNDをSun SPOTのGNDに、そして5VをSun SPOTの+5にそれぞれ接続します。
今回は、簡単な自動走行プログラムを作成しました。基本的には直進し、前方20cm以内に障害物を検出すると後退、左後ろ回り、右後ろ周り、極地左回り、極地右回りのいずれかの動作をランダムで行い、方向転換します。
# アプリケーションのメイン部分
| package org.sunspotworld; import com.sun.spot.peripheral.Spot; import com.sun.spot.sensorboard.EDemoBoard; import com.sun.spot.sensorboard.peripheral.ITriColorLED; import com.sun.spot.sensorboard.peripheral.LEDColor; import com.sun.spot.util.*; import java.io.IOException; import java.util.Random; import javax.microedition.midlet.MIDlet; import javax.microedition.midlet.MIDletStateChangeException; /** * ServoCarSpot: * 距離センサーを使った障害物回避機能を実装したサーボカーアプリケーション. * */ public class ServoCarSpot extends MIDlet { private ServoCar servoCar; // サーボ制御用クラス private DistanceSensor distanceSensor; // 距離センサー制御用クラス private ITriColorLED[] leds; // LED private Random rand; /** ここからスタート */ protected void startApp() throws MIDletStateChangeException { new BootloaderListener().start(); // monitor the USB (if connected) and recognize commands from host Spot.getInstance().getSleepManager().disableDeepSleep(); // 距離センサー制御用クラスのインスタンスの初期化 // センサーはアナログポートA0に接続 try { distanceSensor = new DistanceSensor(EDemoBoard.A0); } catch (IOException ex) { ex.printStackTrace(); notifyDestroyed(); } // サーボカー制御用クラスの初期化 // 右側のサーボをH0, 左側のサーボをH2に接続 servoCar = new ServoCar(EDemoBoard.H0, EDemoBoard.H2); // LEDの初期化 leds = EDemoBoard.getInstance().getLEDs(); initLeds(LEDColor.RED); rand = new Random(); while (true) { // サーボカーを直進させる servoCar.forward(); if (distanceSensor.foundAnObstacle()) { // 前方20cm以内に障害物を検出したので回避する avoid(); } Utils.sleep(20L); // 少しスリープ } } /** * 障害物を回避します. * 0～4までのランダムな値によって、回避動作が決定します */ private void avoid() { servoCar.stop(); blinkLeds(); int index = rand.nextInt(5); switch (index) { case 0: // 後退 servoCar.backward(); break; case 1: // 左後ろ回り servoCar.leftBackward(); break; case 2: // 右後ろ回り servoCar.rightBackward(); break; case 3: // 極地左回り servoCar.pivotLeft(); break; case 4: // 極地右回り servoCar.pivotRight(); break; default: break; } // 500～2000ms のランダムな間、回避動作を実行します Utils.sleep(getSleepDuration()); } /** 500～2000ms のスリープ時間を返します */ private long getSleepDuration() { return 500 * (1 + rand.nextInt(4)); } private void initLeds(LEDColor color) { for (int i = 0; i < leds.length; i++) { leds[i].setOff(); leds[i].setColor(color); } } private void blinkLeds() { for (int i = 0; i < 3; i++) { for (int j = 0; j < leds.length; j++) { leds[j].setOn(); } Utils.sleep(100L); for (int j = 0; j < leds.length; j++) { leds[j].setOff(); } Utils.sleep(100L); } } protected void pauseApp() { } protected void destroyApp(boolean arg0) throws MIDletStateChangeException { } } |
# 距離センサー制御用クラス
| package org.sunspotworld; import com.sun.spot.sensorboard.EDemoBoard; import com.sun.spot.sensorboard.io.IScalarInput; import java.io.IOException; /** * DistanceSensor: * 距離センサー(SHARP GP2D12) を制御するクラス */ public class DistanceSensor { private static final float MAX_VOLTAGE = 3.0f; private IScalarInput input; // アナログ入力ポート private int range; public DistanceSensor(int pinId) throws IOException { input = EDemoBoard.getInstance().getScalarInputs()[pinId]; range = input.getRange(); System.out.println("DistanceSensor init: range=" + range); } /** * 距離センサーを使って、前方の障害物を検出します. * 20cm以内に物体を検出した場合に true を返します. */ public boolean foundAnObstacle() { try { int distance = getDistance(); System.out.println("distance = " + distance); if (distance >= 10 && distance <= 20) { return true; } } catch (IOException ex) { ex.printStackTrace(); } return false; } /** * 前方の物体と距離センサーの間の距離を cm単位で返します. * 検出可能範囲(10-80cm)外の場合、-1を返します. */ public int getDistance() throws IOException { int value = input.getValue(); float voltage = (float)value / range * MAX_VOLTAGE; System.out.println("A/D value -> " + value + ", voltage -> " + voltage); // calculates the distance int distance = -1; float d = 23.333f / (voltage - 0.236f) - 0.420f; if (d >= 10 && d <= 80) { distance = (int)d; } return distance; } } |
# サーボ制御用クラス
| package org.sunspotworld; import com.sun.spot.sensorboard.EDemoBoard; import com.sun.spot.sensorboard.IDemoBoard; import com.sun.spot.sensorboard.peripheral.Servo; public class ServoCar { private static final int MIN_VALUE = 1000; private static final int MID_VALUE = 1500; private static final int MAX_VALUE = 2000; private Servo rightServo; private Servo leftServo; public ServoCar(int rightPinId, int leftPinId) { IDemoBoard demo = EDemoBoard.getInstance(); /** 右側のタイヤを制御 */ rightServo = new Servo(demo.getOutputPins()[rightPinId]); /** 左側のタイヤを制御 */ leftServo = new Servo(demo.getOutputPins()[leftPinId]); stop(); } /** 停止 */ public void stop() { rightServo.setValue(MID_VALUE); leftServo.setValue(MID_VALUE); } /** 前進 */ public void forward() { rightServo.setValue(MAX_VALUE); leftServo.setValue(MIN_VALUE); } /** 後退 */ public void backward() { rightServo.setValue(MIN_VALUE); leftServo.setValue(MAX_VALUE); } /** 左前回り */ public void leftForward() { rightServo.setValue(MAX_VALUE); } /** 右前回り */ public void rightForward() { leftServo.setValue(MIN_VALUE); } /** 右後ろ回り */ public void rightBackward() { rightServo.setValue(MIN_VALUE); } /** 左後ろ回り */ public void leftBackward() { leftServo.setValue(MAX_VALUE); } /** 極地左回り */ public void pivotLeft() { rightServo.setValue(MAX_VALUE); leftServo.setValue(MAX_VALUE); } /** 極地右回り */ public void pivotRight() { rightServo.setValue(MIN_VALUE); leftServo.setValue(MIN_VALUE); } } |
それではプログラムをSun SPOTに流し込んで、実行してみます。。
。。。
無事、動きました 
Posted at 11:55午後 5 28, 2008 by Shuichi Machida in SunSPOT | 投稿されたコメント[0]
投稿されたコメント:
This is a personal weblog, I do not speak for my employer.