2014年5月28日水曜日

BrickPi で EV3 Sensor Adaptor for NXT のお試し その2

 以前に、BrickPi で EV3 Senser Adaptor for NXT を使ってEV3用のジャイロと超音波センサを試しましたが、今回は、BrickPi で EV3の赤外線センサを試してみます。EV3の赤外線センサには「近接モード」「ビーコンモード」「リモコンモード」の3つの動作モードがあります。

 「近接モード」は赤外線センサ単体(自分で赤外線を送受信して反射を使って)で近接物との距離を測定するモードです。超音波センサでは、だいたいcm単位での距離の数値がそのまま出力されましたが、赤外線センサの場合は、10cm~50cmの距離の間が10~70の値でほぼ比例的に出力され、それ以上距離がある(何も存在しない)と80~100の値になります。

 「ビーコンモード」は赤外線リモコンが発信する赤外線を受光して、赤外線リモコンとの距離を測定するモードです。ビーコンモードはリモコンとの距離が10cm~2mくらいの間で10~100の値が比較的直線的に出力されます。

 「リモコンモード」では、赤外線リモコンの操作ボタンの押下によって、押したボタンの種類ごとに違う番号が出力されます。

 「ビーコンモード」と「リモコンモード」では赤外線リモコン側のチャネルが4チャネルあり、この切り替えにより値が返されるレジスタが変わります。

 赤外線センサの特性などに関しては、アフレルさんのサイトに詳しい技術情報が掲載されています。

 EV3 Sensor Adaptor for NXT には向き(Host側とセンサ側コネクタ)があるので、注意が必要です。っていうか、はじめ間違えて値が取れなかった・・・。



ソースはこんな感じ。ここにもおいておきます。

// S1ポートにEV3 Sensor Adaptor + 赤外線センサ を接続。

#include <stdio.h>
#include <math.h>
#include <time.h>
#include <linux/i2c-dev.h>
#include <fcntl.h>

#include <stdlib.h>
#include <unistd.h>
#include <stdint.h>

#include "wiringPi.h"

#include "tick.h"
#include "BrickPi.h"

#define ESA_I2C_ADDRESS    0x32 // EV3 Sensor Adaptor のデフォルトI2Cアドレス
#define ESA_I2C_DEVICE_NO   0x00 // EV3 Sensor Adaptor の I2Cバス上の番号
#define ESA_I2C_REGISTER_COMMAND 0x41 // コマンドレジスタ
#define ESA_I2C_REGISTER_MODE  0x52 // モードレジスタ
#define ESA_I2C_REGESTER_CHANNEL0 0x54 // 0チャネルセンサ値格納
#define ESA_I2C_REGESTER_CHANNEL1 0x56 // 1チャネルセンサ値格納
#define ESA_I2C_REGESTER_CHANNEL2 0x58 // 2チャネルセンサ値格納
#define ESA_I2C_REGESTER_CHANNEL3 0x5A // 3チャネルセンサ値格納

#define MODE_Color_ReflectedLight 0x00 // カラーセンサ反射光モード
#define MODE_Color_AmbientLight  0x01 // カラーセンサ周囲明るさモード
#define MODE_Color_MeasureColor  0x02 // カラーセンサ色モード
#define MODE_Gyro_Angle    0x00 // ジャイロセンサ角度モード
#define MODE_Gyro_Rate    0x01 // ジャイロセンサ角速度モード
#define MODE_InfraRed_Proximity  0x00 // 赤外線センサ近接度モード
#define MODE_InfraRed_Beacon  0x01 // 赤外線センサビーコンモード
#define MODE_InfraRed_Remote  0x02 // 赤外線センサ遠隔モード
#define MODE_Sonar_CM    0x00 // 超音波センサcmモード
#define MODE_Sonar_Inches   0x01 // 超音波センサインチモード
#define MODE_Sonar_Presence   0x02 // 超音波センサ存在検出モード

int main(int argc, char *argv[])
{
 int result;
 uint8_t mode, p0, p1, p2, p3, v0, v1, v2, v3;
 char h0, h1, h2, h3;
 int value;
 unsigned int tmp;

 // モード選択。
 //mode = MODE_InfraRed_Proximity; // 近接
 //mode = MODE_InfraRed_Beacon;  // ビーコン
 mode = MODE_InfraRed_Remote;  // リモート

 // -- BrickPiの初期化。
 // タイマのクリア。
 ClearTick();
 
 // BricPi初期化(ポートオープン)。
 result = BrickPiSetup();
 if (result) return 0;

 // 通信アドレス設定。
 BrickPi.Address[0] = 1;
 BrickPi.Address[1] = 2;

 // -- センサポートの設定。
 // S1ポートをI2Cセンサに設定。
 BrickPi.SensorType[PORT_1] = TYPE_SENSOR_I2C;
 // S1ポートのI2Cスピードを100kbpsに設定。
 BrickPi.SensorI2CSpeed[PORT_1] = 0;
 // S1ポートのI2C接続デバイス数を1に設定。
 BrickPi.SensorI2CDevices[PORT_1] = 1;

 // アドレス設定。
 BrickPi.SensorSettings[PORT_1][ESA_I2C_DEVICE_NO] = 0;
 BrickPi.SensorI2CAddr[PORT_1][ESA_I2C_DEVICE_NO] = ESA_I2C_ADDRESS;

 if (BrickPiSetupSensors())
  return 0;

 // 赤外線センサのモード設定。
 BrickPi.SensorI2CWrite[PORT_1][ESA_I2C_DEVICE_NO] = 2; // 書込バイト数
 BrickPi.SensorI2CRead[PORT_1][ESA_I2C_DEVICE_NO] = 1; // 読込バイト数
 BrickPi.SensorI2COut[PORT_1][ESA_I2C_DEVICE_NO][0] = ESA_I2C_REGISTER_MODE; 
 BrickPi.SensorI2COut[PORT_1][ESA_I2C_DEVICE_NO][1] = mode;
 
 result = BrickPiUpdateValues();
 if (!result)
 {
  while (1)
  {
   switch (mode)
   {
   case MODE_InfraRed_Proximity: // 近接センサモード
    // センサー値取得
    BrickPi.SensorI2CWrite[PORT_1][ESA_I2C_DEVICE_NO] = 1; // 書込バイト数
    BrickPi.SensorI2CRead[PORT_1][ESA_I2C_DEVICE_NO] = 2; // 読込バイト数
    BrickPi.SensorI2COut[PORT_1][ESA_I2C_DEVICE_NO][0] = ESA_I2C_REGESTER_CHANNEL0; // Ch0
    result = BrickPiUpdateValues();
    if (!result)
    {
     // int値取得算出。
     tmp = (int)BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][0];
     tmp += ((int)BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][1]) << 8;
     if (tmp > 32767)
     {
      value = (int)tmp - 65536;
     }
     else
     {
      value = (int)tmp;
     }

     printf("IR: %d\n", value);
    }
    else
    {
     printf ("IR: Get result Error!!\n");
    }

    break;
   case MODE_InfraRed_Beacon:  // ビーコンモード
    h0 = 0; h1 = 0; h2 = 0; h3 = 0; p0 = 0; p1 = 0; p2 = 0; p3 = 0;

    // センサー値取得
    BrickPi.SensorI2CWrite[PORT_1][ESA_I2C_DEVICE_NO] = 1; // 書込バイト数
    BrickPi.SensorI2CRead[PORT_1][ESA_I2C_DEVICE_NO] = 2; // 読込バイト数
    BrickPi.SensorI2COut[PORT_1][ESA_I2C_DEVICE_NO][0] = ESA_I2C_REGESTER_CHANNEL0; // Ch0
    result = BrickPiUpdateValues();
    if (!result)
    {
     // 方向(1バイト目)と近接(2バイト目)値取得。
     h0 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][0];
     p0 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][1];

    }

    // センサー値取得
    BrickPi.SensorI2CWrite[PORT_1][ESA_I2C_DEVICE_NO] = 1; // 書込バイト数
    BrickPi.SensorI2CRead[PORT_1][ESA_I2C_DEVICE_NO] = 2; // 読込バイト数
    BrickPi.SensorI2COut[PORT_1][ESA_I2C_DEVICE_NO][0] = ESA_I2C_REGESTER_CHANNEL1; // Ch1
    result = BrickPiUpdateValues();
    if (!result)
    {
     // 方向(1バイト目)と近接(2バイト目)値取得。
     h1 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][0];
     p1 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][1];

    }

    // センサー値取得
    BrickPi.SensorI2CWrite[PORT_1][ESA_I2C_DEVICE_NO] = 1; // 書込バイト数
    BrickPi.SensorI2CRead[PORT_1][ESA_I2C_DEVICE_NO] = 2; // 読込バイト数
    BrickPi.SensorI2COut[PORT_1][ESA_I2C_DEVICE_NO][0] = ESA_I2C_REGESTER_CHANNEL2; // Ch2
    result = BrickPiUpdateValues();
    if (!result)
    {
     // 方向(1バイト目)と近接(2バイト目)値取得。
     h2 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][0];
     p2 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][1];

    }

    // センサー値取得
    BrickPi.SensorI2CWrite[PORT_1][ESA_I2C_DEVICE_NO] = 1; // 書込バイト数
    BrickPi.SensorI2CRead[PORT_1][ESA_I2C_DEVICE_NO] = 2; // 読込バイト数
    BrickPi.SensorI2COut[PORT_1][ESA_I2C_DEVICE_NO][0] = ESA_I2C_REGESTER_CHANNEL3; // Ch3
    result = BrickPiUpdateValues();
    if (!result)
    {
     // 方向(1バイト目)と近接(2バイト目)値取得。
     h3 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][0];
     p3 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][1];

    }

    //printf("h0=%d, h1=%d, h2=%d, h3=%d, p0=%d, p1=%d, p2=%d, p3=%d\n", h0, h1, h2, h3, p0, p1, p2, p3);
    printf("p0 = %d, p1=%d, p2=%d, p3=%d\n", p0, p1, p2, p3);

    break;
   case MODE_InfraRed_Remote:  // リモートモード
    v0 = 0; v1 = 0; v2 = 0; v3 = 0;

    // センサー値取得
    BrickPi.SensorI2CWrite[PORT_1][ESA_I2C_DEVICE_NO] = 1; // 書込バイト数
    BrickPi.SensorI2CRead[PORT_1][ESA_I2C_DEVICE_NO] = 2; // 読込バイト数
    BrickPi.SensorI2COut[PORT_1][ESA_I2C_DEVICE_NO][0] = ESA_I2C_REGESTER_CHANNEL0; // Ch0
    result = BrickPiUpdateValues();
    if (!result)
    {
     // 方向(1バイト目)と近接(2バイト目)値取得。
     v0 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][0];
     v1 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][1];

    }

    // センサー値取得
    BrickPi.SensorI2CWrite[PORT_1][ESA_I2C_DEVICE_NO] = 1; // 書込バイト数
    BrickPi.SensorI2CRead[PORT_1][ESA_I2C_DEVICE_NO] = 2; // 読込バイト数
    BrickPi.SensorI2COut[PORT_1][ESA_I2C_DEVICE_NO][0] = ESA_I2C_REGESTER_CHANNEL1; // Ch1
    result = BrickPiUpdateValues();
    if (!result)
    {
     // 方向(1バイト目)と近接(2バイト目)値取得。
     v2 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][0];
     v3 = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][1];

    }

    printf("v0=%d, v1=%d, v2=%d, v3=%d\n", v0, v1, v2, v3);

    break;
   default:
    break;
   }

   usleep(100000);
  }
 }

 return 0;
}