Lego Mindstoms NXT - NXC プログラミング (3) センサー

それでは、センサーを使ってみましょう。

NXT には、つぎのセンサーがあります。

・タッチセンサー (ポート 1)
・音センサー (ポート 2)
・光センサー + ライト (ポート 3)
・超音波センサー (ポート 4)
・回転センサー (モーターA、B、C)


■ タッチセンサー

まずは、タッチセンサーを確認するプログラムを作ります。

---

/**
 * TouchSensor01.nxc
 * 「トライボット」使用
 * タッチセンサーを確認する。
 * @author maruno
 * @version 1.0, 2008-01-08
 * @since 1.0
 */
#include "NXCDefs.h"

task main()
{
    SetSensorTouch(S1);  // (1)
    OnFwd(OUT_BC, 75);
    until (SENSOR_1 == 1);  // (2)
    Off(OUT_BC);
}

---

□ SetSensorTouch(S1); // (1)

ポート 1 をタッチセンサーとして設定します。
SENSOR_TOUCH とすることで、タッチセンサーで、その結果はブール値 (0 または 1) で取得できるようになります。


□ SetSensorTouch 関数
---
SetSensorTouch(port) Function

指定されたポート port をタッチセンサーとして構成します。

例:
SetSensorTouch(S1);
---

センサーのポートは、0, 1, 2, 3 の4つあります。これらは、次のように定義されています。Sn または IN_n のどちらを使っても同じです。(★)

IN_1, IN_2, IN_3, IN_4
S1, S2, S3, S4

センサーから取得された値は、つぎの変数に入ります。

SENSOR_1, SENSOR_2, SENSOR_3, SENSOR_4


□ until (SENSOR_1 == 1); // (2)

ポート S1 のセンサーから取得した値は SENSOR_1 に入るので、この値が 1 のときにタッチされたことが検出されます。したがって、ここでは until ループで、1 になるまでループさせます。当然、この間はモーターが動き続けることになります。


■ トライボットの「バンパー」のプログラム

これは、トライボットの 2 番目の工程で作った「バンパー (タッチセンサー)」のプログラムです。ただし、後退する時間は回転数ではなく、0.5 秒という時間で行っています。

---

/**
 * Bumper01.nxc
 * 「トライボット」使用
 * 前進し、そこでタッチしたものをつかみ、後退する。
 * @author maruno
 * @version 1.0, 2008-01-08
 * @since 1.0
 */
#include "NXCDefs.h"

task main()
{
    SetSensorTouch(S1);  // タッチセンサーの設定

    // タッチするまで前進
    OnFwd(OUT_BC, 75);
    until (SENSOR_1 == 1);
    Off(OUT_BC);

    // アームを閉じる
    OnRev(OUT_A, 100);
    Wait(500);
    Off(OUT_A);

    // 0.5秒後退
    OnRev(OUT_BC, 75);
    Wait(500);
    Off(OUT_BC);

}

---



■ 前進した分だけ後退する

今度は、前進した分だけ後退するようにします。
これには、前進した分の時間を計測して、これを後退するときの待ち時間に設定すればいいです。前進した分の時間は、
停止した時刻 - 前進を始めた時刻
で取得できます。この、時刻を取得する関数が、CurrentTick 関数です。

---

/**
 * Bumper02.nxc
 * 「トライボット」使用
 * 前進し、そこでタッチしたものをつかみ、後退する。
 * @author maruno
 * @version 1.0, 2008-01-08
 * @since 1.0
 */
#include "NXCDefs.h"

task main()
{
    SetSensorTouch(S1);  // タッチセンサーの設定

    // タッチするまで前進
    unsigned long start_time = CurrentTick();  // (1)
    OnFwd(OUT_BC, 75);
    until (SENSOR_1 == 1);
    Off(OUT_BC);
    unsigned long forwarding_time = 
                           CurrentTick() - start_time;

    // アームを閉じる
    OnRev(OUT_A, 75);
    Wait(500);
    Off(OUT_A);

    // 前進した時間分後退
    OnRev(OUT_BC, 75);
    Wait(forwarding_time);
    Off(OUT_BC);

}

---

□ unsigned long start_time = CurrentTick(); // (1)

モーターを動かし始めた時刻を取得します。


□ CurrentTick 関数
---
CurrentTick() Value

現在時刻( tick とよぶ)をミリ秒単位の unsigned 32-bit 値として取得します。

例:
x = CurrentTick();
---



■ 各センサーの値を読み取る

---

/**
 * Sensor01.nxc
 * 「トライボット」使用
 * センサーの値を読み取り、表示する。
 * @author maruno
 * @version 1.0, 2008-01-08
 * @since 1.0
 */
#include "NXCDefs.h"

task main()
{
//    SetSensor(S1, SENSOR_TOUCH);  // タッチセンサーの設定 (1)
    SetSensorTouch(S1);
    SetSensorSound(S2);             // 音センサーの設定 (2)
//    SetSensor(S3, SENSOR_LIGHT);  // 光センサーの設定 (3)
    SetSensorLight(S3);
    SetSensorLowspeed(S4);      // 超音波センサーの設定 (4)

    while (true) {
        string num;
        string msg;

        // タッチセンサーの値の表示
        num = NumToStr(SENSOR_1);
        msg = StrCat("Touch    = ", num);
        TextOut(0, LCD_LINE1, msg);

        // 音センサーの値の表示
        num = NumToStr(SENSOR_2);
        msg = StrCat("Sound    = ", num);
        TextOut(0, LCD_LINE2, msg);

        // 光センサーの値の表示
        num = NumToStr(SENSOR_3);
        msg = StrCat("Light    = ", num);
        TextOut(0, LCD_LINE3, msg);

        // 超音波センサーの値の表示
        num = NumToStr(SensorUS(S4));  // (5)
        msg = StrCat("US       = ", num);
        TextOut(0, LCD_LINE4, msg);
        
        // モーター回転速度センサーの値の表示
        num = NumToStr(MotorBlockTachoCount(OUT_A));  // (6)
        msg = StrCat("Tacho    = ", num);
        TextOut(0, LCD_LINE5, msg);

        // モーター回転センサーの値の表示
        num = NumToStr(MotorRotationCount(OUT_A));  // (7)
        msg = StrCat("Rotation = ", num);
        TextOut(0, LCD_LINE6, msg);

    }
}

---

□ SetSensorTouch(S1); // タッチセンサーの設定 (1)

タッチセンサーをポート 1 に設定します。
センサーから取得した値は、SENSOR_1 変数に入ります。


□ SetSensorSound(S2); // 音センサーの設定 (2)

音センサーをポート 2 に設定します。
センサーから取得した値は、SENSOR_2 変数 (0 ~ 100) に入ります。


□ SetSensorSound 関数
---
SetSensorSound(port) Function

指定されたポート port を音センサー (dB 単位)として構成します。

例:
SetSensorSound(S1);
---


□ SetSensorLight(S3); // 光センサーの設定 (3)

光センサーをポート 3 に設定します。
センサーから取得した値は、SENSOR_3 変数に入ります。


□ SetSensorLight 関数
---
SetSensorLight(port) Function

指定されたポート port を光センサーとして構成します。

例:
SetSensorLight(S1);
---


□ SetSensorLowspeed(S4); // 超音波センサーの設定 (4)

超音波センサーをポート 4 に設定します。
センサーから取得した値は、SensorUS 関数を使って取得します。


□ SetSensorLowspeed 関数
---
SetSensorLowspeed(port) Function

指定されたポート port を超音波(I2C デジタル、9V パワー)センサーとして構成します。

例:
SetSensorLowspeed(S1);
---


□ num = NumToStr(SensorUS(S4)); // (5)

超音波センサーの値を取得し、文字列化します。
超音波センサーの値は、直接 SENSOR_n 変数を使って取得するのではなく、SensorUS 関数を使って取得する必要があります。


□ SensorUS 関数
---
SensorUS(n) Value

指定されたポート n に設定されている超音波センサー(I2C デジタル)の値を取得します。

例:
x = SensorUS(S4); // read sensor 4
---


□ num = NumToStr(MotorBlockTachoCount(OUT_A)); // (6)

モーター A の回転速度を取得します。このプログラムでは、自分でアームを開いたり、閉じたりして表示値を確認してみてください。前進が正数、後退が負数になるので、アームを開いているときは正数、閉じているときは負数になっているはずです。



□ MotorBlockTachoCount 関数
---
MotorBlockTachoCount(output) Value

指定されたモーターのブロックに関連した位置カウンターを取得します。
tacho なので、回転速度と思われます。(★)

例:
x = MotorBlockTachoCount(OUT_A);
---


□ num = NumToStr(MotorRotationCount(OUT_A)); // (7)

モーター A の回転角度を取得します。回転速度の取得と同じように、自分でアームをを開いたり、閉じたりして表示値を確認してみてください。前進が正数、後退が負数になるので、アームを開いているときは正数、閉じているときは負数になっているはずです。

ただ、ディスプレイで表示を見ると、MotorBlockTachoCount の値も、MotorRotationCount の値も同じになっています。これらは、別に回転速度と角度ではなく、どの程度動いたか? というものかもしれません。これらについて調べるのは別の機会にしたいと思います。★


□ MotorRotationCount 関数
---
MotorRotationCount(output) Value

指定されたモーターのプログラムに関連した位置カウンターを取得します。
rotation なので、回転角度と思われます。(★)

例:
x = MotorRotationCount(OUT_A);
---


■ トライポッドの動きを NXC プログラムで再現する

---

/**
 * Trybot01.nxc
 * 「トライボット」使用
 * 前進し、そこでタッチしたものをつかみ、後退、回転し、
 * 黒線があるところまで前進し、黒線のところでつかんだものを離す。
 * @author maruno
 * @version 1.0, 2008-01-08
 * @since 1.0
 */
#include "NXCDefs.h"

#define PUT_THRESHOLD 51 // 物を置くしきい値
#define SOUND_THRESHOLD 50 // 音のしきい値
#define LIGHT_THRESHOLD 40 // 光のしきい値
#define TOUCHED 1 // タッチしたときの値

task main()
{
    // センサーの設定
    SetSensorTouch(S1);     // タッチセンサーの設定
    SetSensorSound(S2);     // 音センサーの設定
    SetSensorLight(S3);     // 光センサーの設定
    SetSensorLowspeed(S4);  // 超音波センサーの設定

    // 物が置かれるまで停止
    until (SensorUS(S4) <PUT_THRESHOLD); SOUND_THRESHOLD);

    // アームを閉じる
    OnRev(OUT_A, 75);
    Wait(500);
    Off(OUT_A);

    // 後退
    OnRev(OUT_BC, 75);
    Wait(500);
    Off(OUT_BC);

    // 回転
    OnRev(OUT_C, 75);
    OnFwd(OUT_B, 75);
    Wait(500);
    Off(OUT_BC);

    // 黒線のところまで前進して停止
    OnFwd(OUT_BC, 75);
    until (SENSOR_3 <LIGHT_THRESHOLD);

---



以上