Shuichi Machida's Weblog

« 前の日(May月 28日, 2008年) | 日付別メイン | 次の日(May月 30日, 2008年) »

2008年 5月 30日金曜日

[Sun SPOT Demo(9)] サーボ(Servo)を使ってパンとティルト(Pan and Tilt)を制御してみる。

Sun SPOTとServoを使って、カメラなどのパン（左右）とティルト（上下）の動きを制御する機構を作成してみます。今回は実装しませんが、Sun SPOTを２台使えばリモコン制御もできますね。

パン＆ティルト制御機構は、以下のパーツを使って作成しました：

品名	参考価格	数量
GWSサーボ S03T/2BB	1600	2
サーボブラケット　AGBL-S03T	1785	1

# サーボブラケットとGWSサーボ

組み立ては簡単です！

# 違うアングルから

配線は、ブレッドボードを使って行います。単三 x4(6V)の電池ボックスをブレッドボードの下に取り付けています。配線はサーボカーの時と基本的には同じです。３ピンのピンヘッダが２本並んでいますが、１番上のピンの横ラインがGND、真ん中の横ラインが電源（6V）、そして１番下のそれぞれのピンがサーボの制御信号ラインです。

また下図のように、GNDラインからケーブル（黒）をのばしてSun SPOTのGNDに、電源ラインからSun SPOTのVHに（赤）、そしてそれぞれのサーボの信号ラインからケーブル（白）を延ばして、下側のサーボ（パン）を H0、上側のサーボ（ティルト）をH1 に接続します。

# パン＆ティルト制御機構を超強力接着剤で取り付けて完成！

おっと、デュークを忘れていました

# 今度こそ完成！

今回は、デュークを左右および上下に一通り動かしてみる簡単なプログラムを作成しました。後半で、パンとティルトを同時に制御するプログラムも試しています。

# メインのプログラム部分

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 javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

/**
* PanTiltControllerSpot:
* パンおよびティルト制御用アプリケーション
*/
public class PanTiltControllerSpot extends MIDlet {
    private PanTiltController controller;      // パン・ティルト制御用クラス
    private ITriColorLED[] leds;               // LED

    /** ここからスタート */
    protected void startApp() throws MIDletStateChangeException {
        new BootloaderListener().start();   // monitor the USB (if connected) and recognize commands from host
        Spot.getInstance().getSleepManager().disableDeepSleep();

        // パン・ティルト制御用クラスの初期化
        // パン用のサーボをH0, ティルト用のサーボをH1に接続
        controller = new PanTiltController(EDemoBoard.H0, EDemoBoard.H1);

        // LEDの初期化
        leds = EDemoBoard.getInstance().getLEDs();
        initLeds(LEDColor.RED);

        // パンおよびティルトの位置の初期化
        controller.center();
        Utils.sleep(1000L);

        while (true) {
            blinkLeds();

            // 右端、左端、中央の順にパン制御します
            testPan();
            // 一番上、一番下、中央の順にティルト制御します
            testTilt();

            blinkLeds();
            Utils.sleep(3000L);

            // パンとティルトを同時に制御します
            testPanTilt();

            blinkLeds();
            Utils.sleep(10000L);
        }
    }

    /** 右端、左端、中央の順にパン制御します */
    private void testPan() {
        controller.panRightmost();
        Utils.sleep(1000L);
        controller.panLeftmost();
        Utils.sleep(1000L);
        controller.panCenter();
        Utils.sleep(1000L);
    }

    /** 一番上、一番下、中央の順にティルト制御します */
    private void testTilt() {
        controller.tiltUpmost();
        Utils.sleep(1000L);
        controller.tiltDownmost();
        Utils.sleep(1000L);
        controller.tiltCenter();
        Utils.sleep(1000L);
    }

    /**
     * パンとティルトを同時に制御します
     */
    private void testPanTilt() {
        Thread[] threads = new Thread[2];
        threads[0] = new PanWorker();
        threads[1] = new TiltWorker();
        for (int i = 0; i < threads.length; i++) {
            threads[i].start();
        }
        for (int i = 0; i < threads.length; i++) {
            try {
                threads[i].join();
            } catch (InterruptedException ex) {
                ex.printStackTrace();
            } finally {
                threads[i] = null;
            }
        }
    }

    class PanWorker extends Thread {
        public void run() {
            testPan();
        }
    }

    class TiltWorker extends Thread {
        public void run() {
            testTilt();
        }
    }

    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.IDemoBoard;
import com.sun.spot.sensorboard.peripheral.IServo;
import com.sun.spot.sensorboard.peripheral.Servo;
import com.sun.spot.util.Utils;

public class PanTiltController {
    private final int MIN_VALUE = 800;
    private final int MID_VALUE = 1500;
    private final int MAX_VALUE = 2200;
    private final int STEP = 100;
    private final int WAIT = 10;

    private IServo panServo;   // 左右の動き
    private IServo tiltServo; // 上下の動き

    public PanTiltController(int panPinId, int tiltPinId) {
        IDemoBoard demo = EDemoBoard.getInstance();
        /** 左右の動きを制御 */
        panServo = new Servo(demo.getOutputPins()[panPinId]);
        /** 上下の動きを制御 */
        tiltServo = new Servo(demo.getOutputPins()[tiltPinId]);

        center();
    }

    /** パンおよびティルトを中央まで動かします */
    public void center() {
        pan(MID_VALUE);
        tilt(MID_VALUE);
    }

    /** パンを右端まで動かします */
    public void panRightmost() {
        pan(MIN_VALUE);
    }

    /** パンを中央まで動かします */
    public void panCenter() {
        pan(MID_VALUE);
    }

    /** パンを左端まで動かします */
    public void panLeftmost() {
        pan(MAX_VALUE);
    }

    /** ターゲットで指定した値まで、パン(左右)方向にサーボを制御します */
    public void pan(int target) {
        moveServo(target, panServo);
    }

    /** ティルトを一番上まで動かします */
    public void tiltUpmost() {
        tilt(MIN_VALUE);
    }

    /** ティルトを中央まで動かします */
    public void tiltCenter() {
        tilt(MID_VALUE);
    }

    /** ティルトを一番下まで動かします */
    public void tiltDownmost() {
        tilt(MAX_VALUE);
    }

    /** ターゲットで指定した値まで、ティルト(上下)方向にサーボを制御します */
    public void tilt(int target) {
        moveServo(target, tiltServo);
    }

    /** ターゲットの位置にサーボを動かします */
    private void moveServo(int target, IServo servo) {
        int oldValue = servo.getValue();
        if ((target - oldValue) > 0) {
            for (int value = oldValue + STEP; value < target; value += STEP) {
                setValue(value, servo);
            }
        } else {
            for (int value = oldValue - STEP; value > target; value -= STEP) {
                setValue(value, servo);
            }
        }
        setValue(target, servo);
    }

    /** サーボに値を設定します */
    private void setValue(int target, IServo servo) {
        servo.setValue(target);
        Utils.sleep(WAIT);
    }
}

それでは実行してみます。。。

。。。。。

成功です