2014年5月18日日曜日

BrickPi で EV3 Sensor Adaptor for NXT のお試し

 本日のがらくたは、Mindsensors社の「EV3 Sensor Adaptor for NXT」です。いつもこの手のものをお世話いただいている ROBO Product さんより購入しました。

EV3 Sensor Adapter for NXT or Arduino

 このアダプタは、EV3用のセンサをNXT コントローラで利用できるようにするための変換アダプタです。中身としては、EV3のUART(シリアル)信号をNXT コントローラで利用できる100kbpsのI2C通信と変換しているのだと思われます。Mindstorms社のサイトを見ると、EV3のセンサをNXTコントローラだけでなく、NXShieldやBrickPiでも使えるようになると書いてありますが、ライブラリやサンプルが提示されているのは、2014年05月中旬現在では、NXTブロックでのグラフィックプログラミングとNXCだけのようです。

 そこで、EV3 のセンサをこのアダプタを使って、BrickPi で利用できるか試してみようと思います。BrickPi は基本NXTコントローラ互換のセンサポートなので、EV3のセンサも使えるようになれば、手持ちのEV3センサが活用できるようになります。今回は、NXTのセットにはなくEV3(の教育版)で追加された「ジャイロ」とNXT版よりも距離精度が上がっている「超音波センサ」をターゲットにしてみます。


 ジャイロセンサをBrickPi の S1ポートに接続しました。


 超音波センサはBrickPi の S2ポートに接続しました。


 参考にした資料は、Mindsensors社のサイトに有るマニュアルNXC用のライブラリのソースです。

 いきなりですが、動かしてみた結果です。ジャイロは「角度モード」で水平に回転させたときに回転した角度が直接読み取れます。初期位置から相対的に時計回りに+、反時計回りに-の角度が出力されます。
 超音波センサは「距離cmモード」にするとmm単位で対象との距離が出力されます。


 ジャイロ、超音波センサともに角度や距離がそのまま割りとリニアリティもよく出力されるので、非常に使いやすいです。ただ、BrickPiからアダプタを使ってのアクセスでは、I2Cデバイスとしてのアクセスなので、手順は一手間あります。今回は、関数化などをしていない状態ですが、ソースをこちらに載せておきます。

// S1ポートにEV3 Sensor Adaptor + EV3 ジャイロを接続。
// S3ポートにEV3 Sensor Adaptor + EV3 超音波センサを接続。

#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 // 0チャネルセンサ値格納
#define ESA_I2C_REGESTER_CHANNEL2 0x58 // 0チャネルセンサ値格納
#define ESA_I2C_REGESTER_CHANNEL3 0x5A // 0チャネルセンサ値格納

#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;
 int value;
 unsigned int tmp;

 // -- 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;
 // S3ポートをI2Cセンサに設定。
 BrickPi.SensorType[PORT_3] = TYPE_SENSOR_I2C;
 // S3ポートのI2Cスピードを100kbpsに設定。
 BrickPi.SensorI2CSpeed[PORT_3] = 0;
 // S3ポートのI2C接続デバイス数を1に設定。
 BrickPi.SensorI2CDevices[PORT_3] = 1;

 BrickPi.SensorSettings[PORT_1][ESA_I2C_DEVICE_NO] = 0;
 BrickPi.SensorI2CAddr[PORT_1][ESA_I2C_DEVICE_NO] = ESA_I2C_ADDRESS;
 BrickPi.SensorSettings[PORT_3][ESA_I2C_DEVICE_NO] = 0;
 BrickPi.SensorI2CAddr[PORT_3][ESA_I2C_DEVICE_NO] = ESA_I2C_ADDRESS;

 if (BrickPiSetupSensors())
  return 0;

 // S1ジャイロセンサのモード設定。
 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_Gyro_Angle;
 //BrickPi.SensorI2COut[PORT_1][ESA_I2C_DEVICE_NO][1] = MODE_Gyro_Rate;
 // S3超音波センサのモード設定。
 BrickPi.SensorI2CWrite[PORT_3][ESA_I2C_DEVICE_NO] = 2; // 書込バイト数
 BrickPi.SensorI2CRead[PORT_3][ESA_I2C_DEVICE_NO] = 1; // 読込バイト数
 BrickPi.SensorI2COut[PORT_3][ESA_I2C_DEVICE_NO][0] = ESA_I2C_REGISTER_MODE; 
 BrickPi.SensorI2COut[PORT_3][ESA_I2C_DEVICE_NO][1] = MODE_Sonar_CM;
 result = BrickPiUpdateValues();
 if (!result)
 {
  mode = BrickPi.SensorI2CIn[PORT_1][ESA_I2C_DEVICE_NO][0];

  while (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_CHANNEL0; // Ch0
   BrickPi.SensorI2CWrite[PORT_3][ESA_I2C_DEVICE_NO] = 1; // 書込バイト数
   BrickPi.SensorI2CRead[PORT_3][ESA_I2C_DEVICE_NO] = 2; // 読込バイト数
   BrickPi.SensorI2COut[PORT_3][ESA_I2C_DEVICE_NO][0] = ESA_I2C_REGESTER_CHANNEL0; // Ch0
   result = BrickPiUpdateValues();
   if (!result)
   {
    // S1ポート、ジャイロ値算出。
    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;
    }

    switch (mode)
    {
    case 0:
     printf("Gyro angle: %d\n", value);
     break;
    case 1:
     printf("Gyro rate: %d\n", value);
     break;
    case 2:
     printf("Gyro N/A\n");
     break;
    default:
     break;
    }

    // S3ポート、超音波センサ値算出。
    value = (int)BrickPi.SensorI2CIn[PORT_3][ESA_I2C_DEVICE_NO][0];
    value += ((int)BrickPi.SensorI2CIn[PORT_3][ESA_I2C_DEVICE_NO][1]) << 8;

    printf("Ultrasonic distance: %d\n", value);
   }

   usleep(100000);
  }
 }

 return 0;
}


 関数化やクラス化をすれば、NXTのセンサとおなじ感覚で、もっと楽に使えるようになると思いますが、それは、また今度。