[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に流し込んで、実行してみます。。

。。。

無事、動きました ;-)

投稿されたコメント:

コメント
  • HTML文法 不許可
About

machida

Search

Archives
« 4月 2014
  
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
   
       
今日