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;
}

LEGO TECHNIC 42025 CARGO PLANE

　今回は、「LEGO TECHNIC 42025 CARGO PLANE」です。2014年1月発売の最新のTECHNICシリーズの中では最大のモデルじゃないでしょうか。
　Amazon に予約して、発売日に購入しましたが、半年ほど寝かせてありましたが、来月6月にはゴーストバスターズ、スターウォーズのAT&AT、スター・デストロイヤー、さらには、7月には新トレインシリーズ等新製品が続くので、押し入れが溢れない内に組み立てることにします。

　まずはパッケージ

　あいかわらず、テクニック系の大物は箱がでかい・・・。初めて「ウニモグ」を見た時は、言葉を失いましたが、今はもう慣れてきているので、普通に感じます。ただ、今回は飛行機で羽根が付いているため、パッケージ以上に出来上がりのサイズにはインパクトが有るんじゃないかと予想します。

　開梱。

　15穴のビームが結構入っています。また、羽根や胴体になるだろう板状の特殊パーツが多く入っているのが、何より目を引きます。

　ここからは、組立てですが、結構、写真をいっぱいとったので、組立中に撮影した写真はまとめてこちらにおいてあります。
　以下、ダイジェスト。

　出来上がり。モータで、車輪が格納できたり、貨物ハッチ（後部開閉と運転席跳ね上げ）の開閉、プロペラの回転なんかができます。やはり、でかいです。クレーン車は縦長＆高さ方向ですが、この飛行機は羽根が横に広がっているので幅広です。外装に白いパーツが多く使われています。

LEGO Digital Designer

　今回は、LEGO Digital Designer （LDD） の紹介です。簡単に言うとLEGO用のCADです。しかも、公式LEGO製で、無料で利用できます。詳細とダウンロードはこちらからできます。

　いつも、適当に作って、終わったらバラしてしまうので、あとで同じものを作ろうとしても覚えていないんですが、今回は、ここ最近BrickPi用に組み立てたいくつかのベースをLDD上で作図して、記録・保管することにしました。
　操作は結構クセがありますが、使っていれば慣れてきます。無料でこれだけのものが提供されるのですから、ありがたいです。
　この LDD を使えば、作成したモデルの組立て情報を記録することはもちろん、事前に画面上でモデルを設計することができます。言い換えれば、手持ちのブロックがないパーツも使えますし、もっと言うと、LEGOを持っていなくてもLEGOが楽しめちゃいます。しかも、作ったモデルの組み立て手順書を自動で作成してエクスポートする機能までついています。すごいよレゴさん！

　というわけで、自分の覚え用にCADを入力しつつ、組み立て手順書（HTML形式）をエクスポートしてみました。以下においてあります。

• PiTanK ... BrickPi 用クローラ移動車両。
• メカナムホイール ... BrickPi 用メカナムホイール全方向移動車両。要メカナムホイール。
• オムニホイール ... BrickPi 用おむんホイール全方向移動車両。要オムニホイール。

BrickPi用電源スイッチを作りました。

　さて、今回は、BrickPi用の電源スイッチを作ってみました。既存の購入したキットには、9V電池に使うスナップがついていて、単三電池8本のケースに接続して使うようになっています。OSのシャットダウン自体は、shutdownやhaltコマンドで行いますが、OSシャットダウン後の電源Offは電池ボックスからスナップを外すことで行っていました。また、電源Onは逆に電池ボックスにスナップを取り付けることで行っていましたが、この取付・取り外しの際、中途半端に接触したり切れたを繰り返す事が多いのが気になっていました。なので、スイッチを間に入れようと思いましたが、せっかくなので、LEGOに取り付けられるようなスイッチがいいので、Power Function のスイッチのケーブルをちょん切って加工することにしました。

　Power Function のスイッチは3方向のセンターオフで、左右両側は極性が反転して導通するようになっています。BrickPiへの給電では±が反転されては困るので、物理的に片方向にしか倒れないように、ブロックでブロック（ダジャレじゃないよ）しました。

　これで、気分的には少しはすっきりするかな。

Leap Motion のお試し。 ロボットハンド。

　今回のガラクタは、Leap Motion です。Kinect や Xition 等につぐ期待のNUIデバイスとして1～2年前に話題になりましたが、イマイチ使い道が微妙（まあ、NUIデバイス全般に言えることかもしれませんが）なせいか、最近はアマゾンなどでも品薄ということもなく、容易に入手できるようになりました。
　先日、なんとなく色々なセンサデバイスの情報をWebで収集しているときに、Leap Motion のSDKのv2beta が出ているという情報を見つけて、じゃあ、Leap Motionも一応触っておくかと思い、入手してみました。

　※下の写真は Leap Motion のサイトに合ったものをコピーしてきました。大きさのイメージはこんな感じ。ちなみに、自分の机の上はこんなに片付いておりません・・・。

　まだ、ほんのおさわり程度しか遊んでいないんですが、驚くのは、このLeap MotionのSDKはライブラリというか関数がよく出来ていて、APIリファレンスも（英語ですが）きちんとまとめられたものが公開されています。こういう、KickStarterっぽいものから始まったようなデバイスだとその辺が結構アバウトだったりすることが多いと思うのですが、このLeap Motion は資料がDeveloper Site に割りとよく揃っています。

　で、この Leap Motion ですが、Kinect や Xiton がユーザの体全体の情報を扱うのに対して、ユーザの「手」「指」の情報を特化して扱います。Web上のレポートサイトや分解写真をみると、小さなデバイスの天面方向に2つの赤外線カメラが配置されており、ステレオ視によりユーザの手指の位置情報を解析・取得しているように想像されます。

　パッケージはこんな感じですが、

　明けてみると

　中身はとっても小さい。

　SDカードと並べてみました。

　Developer Site から SDKβ2 をダウンロードしてきて展開すると、SDKと合わせてドライバのインストーラも解凍されるので、それをインストールして、Leap Motion をPCに接続します。「Leap Motion Visualizer」というアプリがドライバと一緒にインストールされているので、これを起動するとデバイスが動作しているかどうかを確認できます。手をかざしたところ。

　今回は、Leap Motion で検出した各指の手のひらに対する方向（角度）をそれぞれ1バイトの大きさのデータにして、USB（シリアル）接続経由でArduino  Micro に送信し、Arduino Micro で ラジコンサーボを動かしてみました。以前に「ぱい揉みハンド」で曲げセンサグローブで動かしたおもちゃのロボットハンドを同じように動かしてみました。

　前回は、Futaba のメタルギアでわりとハイトルク・ハイスピードな無駄に良いサーボを使ったんですが、今回は、Amazonで10個で3千いくらという超小型の格安サーボ（飛行機模型用？）を使ったため、小型化出来ましたが、反応（指の動く早さ）は悪くなっています。

また、Leap Motion の指の誤検出が結構あるので、レスポンス・確実性の両方共曲げセンサグローブ方式に負けますが、それでも、Leap Motion の面白さは十分あると思います。まあ、実用でなんかあるか？っていわれると、Kinect以上に使いドコロは微妙ですが、SF映画に出てくるNUIなコンピュータの世界に近づいてきてはいますね。

　今回のソース類はこちらにおいてあります。今回、Leap Motion アプリは初なので、終了時にアプリケーションエラーとか（たぶんリソース開放忘れとか？）出たりすることもありますが、まあ、ご愛嬌・・・。PC側 Leap Motion のアプリはこちら。Arduino側はこちらがソースです。

BrickPi で EV3 Sensor Adaptor for NXT のお試し

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

 このアダプタは、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のセンサとおなじ感覚で、もっと楽に使えるようになると思いますが、それは、また今度。