LEGO Mindstorms NXT で Arduino + R1350Nジャイロシールドを使ってみる

　前回、LEGO Mindstorms NXT と Arduino 間でRS485の通信をするところまで試してみました。今回は、このArduinoとの通信を使って、ArsuinoにスタックしたR1350Nジャイロシールドの値を取得してみたいと思います。
  NXTとArduino間の接続は前回と同じ感じで、NXTのIN_4ポートにRS485-UARTコンバータを入れて接続しているのですが、前回のようにソフトウェアシリアルではなく、今回はArduino Megaのハードウェアシリアル1を使用して、115200bpsで通信するようにしました。

　かなりいい加減なつくりですが、Arduinoを置くための台座をその場で簡単に組めちゃうところが、便利です。

以前に試したR1350Nジャイロシールドをスタックしています。
　ジャイロの値を取得して、RS485経由でNXTに渡して、NXTの液晶に表示（0.01度を1とした数値）しています。

約90度旋回して、停止動作。

以下、NXCのコード。

// RS485通信によるArduino+R1350N Gyro Shield連携

#include "NXCDefs.h"

inline void WaitForMessageToBeSent()
{
  bool sending, avail;

  RS485Status(sending, avail);

  while(sending)
  {
    Wait(MS_1);
    RS485Status(sending, avail);
  }
}

task main()
{
  string rcvStr;
  int gyro;
  int status;
  
  const int STATUS_STOP = 0;
  const int STATUS_TURN_LEFT = 1;
  const int STATUS_TURN_RIGHT = 2;

  // S4ポートをRS485用に初期化。
  UseRS485();
  RS485Enable();
  // RS485の通信条件を設定。
  RS485Uart(HS_BAUD_115200, HS_MODE_8_DATA|HS_MODE_N_PARITY|HS_MODE_10_STOP);

  Wait(MS_10);
  
  // 画面消去。
  ClearScreen();
  
  // タイトル表示。
  TextOut(0, LCD_LINE1, "R1350N Gyro" );

  Wait(MS_10);

  while(true)
  {
    if (ButtonPressed(BTNLEFT, false))
    { // 左ボタンを押されたら、
      if (status == STATUS_STOP)
      {
        // リセットコマンドを送信。
        SendRS485String("r");
        WaitForMessageToBeSent();
        TextOut(0, LCD_LINE2, "Send Reset.");
        Wait(MS_500);
        // 左旋回開始。
        status = STATUS_TURN_LEFT;
        OnFwd(OUT_A, 30);
        OnRev(OUT_C, 30);
      }
      Wait(MS_500);
    }
    
    if (ButtonPressed(BTNRIGHT, false))
    { // 右ボタンを押されたら、
      if (status == STATUS_STOP)
      {
        // リセットコマンドを送信。
        SendRS485String("r");
        WaitForMessageToBeSent();
        TextOut(0, LCD_LINE2, "Send Reset.");
        Wait(MS_500);
        // 右旋回開始。
        status = STATUS_TURN_RIGHT;
        OnRev(OUT_A, 30);
        OnFwd(OUT_C, 30);
      }
      Wait(MS_500);
    }
    
    if (ButtonPressed(BTNCENTER, false))
    { // 中央ボタンを押されたら、
    }

    SendRS485String("g");
    WaitForMessageToBeSent();
    TextOut(0, LCD_LINE2, "Send Meas.");
    
    if (status == STATUS_TURN_LEFT)
    {
      TextOut(0, LCD_LINE1, "TURN LEFT");
      if (gyro < -8500)
      {
        Off(OUT_AC);
        status = STATUS_STOP;
        TextOut(0, LCD_LINE1, "STOP");
      }
    }
    
    if (status == STATUS_TURN_RIGHT)
    {
      TextOut(0, LCD_LINE1, "TURN RIGHT");
      if (gyro > 8500)
      {
        Off(OUT_AC);
        status = STATUS_STOP;
        TextOut(0, LCD_LINE1, "STOP");
      }
    }
    
    Wait(MS_200);
    
    // 受信処理。
    if (RS485DataAvailable())
    {
      ClearScreen();
      TextOut(10, LCD_LINE2, "Received.");
      RS485Read(rcvStr);
      gyro = StrToNum(rcvStr);
      ClearLine(LCD_LINE3);
      NumOut(20, LCD_LINE3, gyro);
    }
  }
}

以下、Arduino Megaのコード。

#include <Arduino.h>
#include <Wire.h>
#include <R1350i2c.h>

// RS485ピン定義（Mega - Serial1使用）
const int rs485RtsPin = 22;  // 任意のpinを指定
const int rs485RxPin = 19;  // 固定
const int rs485TxPin = 18;  // 固定

// R1350ジャイロシールドオブジェクト
R1350i2c R1350(0x9A,XTAL_7);

// RS485初期化。
void RS485_initialize(long baudrate)
{
  //  Serial1初期化。
  Serial1.begin(baudrate);
  // DTS出力ピン設定。
  pinMode(rs485RtsPin, OUTPUT);
  // 受信(DTS=0)状態に設定。
  digitalWrite(rs485RtsPin, LOW);
}

// RS485送信。
void RS485_write(byte data[], int count)
{ 
  // DTS出力ピンを送信(DTS=1)に設定。
  digitalWrite(rs485RtsPin, HIGH);
  // 送信。
  for (int i = 0; i < count; i++)
 { 
    Serial1.write(data[i]);
 }
  delayMicroseconds(70 * count);
  // 受信(DTS=0)状態に設定。
  digitalWrite(rs485RtsPin, LOW);
}

void setup()
{
  Serial.begin(115200);
  // R1350ジャイロ初期化。
  R1350.begin(115200);
  // RS485初期化。
  RS485_initialize(115200);
  
  delay(500);
  R1350.reset();
  delay(500);
}

void loop()
{
  short data;
  char c;
  byte sendData[2];
  
  R1350.getData();
  //data = R1350.angle() / 100;
  data = R1350.angle();
  
  // RS485受信処理。
  if (Serial1.available())
  {
    c = Serial1.read();
    Serial.println(c);
    if (c == 'r')
    {
      Serial.println("Reset...");
      R1350.reset();
      delay(500);
    }
    if (c == 'g')
    {
      Serial.println(data, DEC);
      /*
      sendData[0] = (byte)((data & 0xff00) >> 8);
      sendData[1] = (byte)(data & 0x00ff);
      sendData[2] = 0x03;
      // 送信。
      RS485_write(sendData, 3);
      */
      // DTS出力ピンを送信(DTS=1)に設定。
      digitalWrite(rs485RtsPin, HIGH);
      // 送信。
      Serial1.print(data, DEC);
      Serial1.write(0x03);
      delayMicroseconds(70 * 7);
      // 受信(DTS=0)状態に設定。
      digitalWrite(rs485RtsPin, LOW);
    }
  }
  
  // シリアルポート受信処理。
  if (Serial.available())
  {
    c = Serial.read();
    if (c == 'r')
    {
      Serial.println("Reset...");
      R1350.reset();
      delay(500);
    }
    if (c == 'g')
    {
      Serial.println(data, DEC);
      /*
      sendData[0] = (byte)((data & 0xff00) >> 8);
      sendData[1] = (byte)(data & 0x00ff);
      sendData[2] = 0x03;
      // 送信。
      RS485_write(sendData, 3);
      */
      // DTS出力ピンを送信(DTS=1)に設定。
      digitalWrite(rs485RtsPin, HIGH);
      // 送信。
      Serial1.print(data, DEC);
      Serial1.write(0x03);
      delayMicroseconds(70 * 7);
      // 受信(DTS=0)状態に設定。
      digitalWrite(rs485RtsPin, LOW);
    }
    if (c == 't')
    {
      Serial.println("Test send.");
      byte testData[] = {'H', 'e', 'l', 'l', 'o', '!', 0x03};
      RS485_write(testData, 7);
    }
  }
}

LEGO Mindstorms NXT はじめました。

 巷では LEGO Mindstorms EV3 の9月リリースのニュースが出ています。自分はLEGO Mindstorms は殆どさわったことがなかったのですが、以前から興味はありました。EV3 を思い切って予約しようかとも思ったのですが、発売直後はまだ情報が少ないだろうと思い、敢えて、ネット上にも豊富に情報がある現行の NXT をはじめてみようと思いました。

　とりあえず、マニュアルに掲載されていたサンプルを組み立ててみました。マニュアルのサンプルに使用する部品はわかりやすいように「Start Here!」という袋に分けてくれてあるので、その袋の部品をトレーに取り出して、マニュアルのとおりに組み合わせていきます。

　説明書のとおりに組み合わせていくだけで、あっという間に、簡単に差動二輪の車輪ロボットベースが出来てしまいました。すごい・・・。でも、サンプルで指示通りに組み上げるだけなので、簡単ですが、自分で設計（デザイン）しようと思ったら、大変そう・・・。自分にとっては、このサンプルの組立例でもう十分目的を果たしそうな・・・。

　で、ギークなレゴビルダーの人たちは、これで色々なものを作っちゃうわけですが、自分には無理そうなので、違う路線で攻めてみようと思います。

　レゴの中でもこのMindstormsの最大の特徴はインテリジェントブロックでのプログラミングということになるなわけですが、標準ではLabVIEWベースと思われるグラフィカルプログラミング環境が付属してきます。自分はコードを書いてしまう方がなじむ気がするので、フリーで入手できる Bricx Command CenterというIDEで NXC というC言語ライク（Not eXactly Cだそうです）なプログラミングが出来る環境があったので、これを使ってみることにしました。このNXCでセンサの値を取得して、モータを動かして・・・というのは多分Googleで検索すれば出てくると思うので、もう少し違うことをしてみます。

　このNTXのインテリジェントブロックというかコントローラと各センサやモータは専用ケーブルで接続されます。このケーブルの両端、あるいはコントローラやセンサ・モータのコネクタ部分はISDNのような6芯のモジュラーに似ているんですが、爪の位置が真ん中からずれている特殊なものなので、自作が難しいです。なので、前回のロビのサーボケーブルのようにちょん切ります・・・。で、ちょん切ってどうするかというと、やっぱり、Arduinoにつなぎます。Wikipediaなどを見ると6本の信号線の情報なども掲載されていました。

　この中で気になるのが、5、6ピンです。I2C（SCL, SDA)やRS485(A, B)が機能として割り当てられています。そこでこれらの5、6ピンを使ったI2CとRS485について調べてみることにします。


　ところで、全然話が違うのですが、今回、（レゴブロックを整理するのも合わせて）、在庫部品の保管方法を少し見なおそうと思い、ホームセンターでプラスチック製の連結できる引き出しを買ってきました。今まではアマゾンの箱に分けて入れた部品を段積みにしていましたが、

下の方のものを取り出したりするときにハノイの塔みたいな状態になって、取り出して元に戻すのが結構面倒でした。引き出しに変えて、

引き出しの中に、今まで使っていた段ボール箱の紙を切って仕切りを作ったりして、部品類も値ごとに取り出しやすくなりました。ロビの置き場所も確保できたし、レゴもパーツ分けして整理する予定です。


　さて、レゴの話しに戻ると、まずは、I2CでのNXT - Arduino通信ですが、NXTがI2Cマスタなので、ArduinoをI2Cスレーブとして動かして上げることで、動作確認できました。ただ、プルアップ抵抗の値が微妙なところがあったような感じで、100kΩくらいが安定していました。2.2kとか10kだとエラー（送受信値が化ける）がありましたが、原因・詳細は調べていません。

　Arduino側が5VでNXT側の電圧が不明（4.4Vくらい？）だったのでI2Cレベルコンバータを間に入れたんですが、これが影響してるのかな？このレベルコンバータ自体にはプルアップ抵抗は入っていなかったはずだけど。
　で、ArduinoでI2C通信できたので、ストリナのI2Cコンパスをつないで値を取ろうとしたんですが、うまくいきませんでした。ロジアナで見ても、コンパスからの応答がない状態でした。ロジアナで見て気がついたんですが、I2C通信のクロックが1kHz前後くらいのように見えます。たしか、I2Cは100kHzとか400kHzとかじゃなかったっけ？クロック同期式なので、遅い分にはマスタが出すクロックに合わせて動く規格なんだろうけど、あんまり想定外の遅さでコンパスが反応しないのかな？う～ん、Mindstorms純正じゃない市販の安いI2C式のセンサを使えないかって思ったんだけど、難しいかな？アナログ入力の複数のセンサをArduinoで束ねて、Mindstormsと接続っていう感じでの使い方はありかもしれない。
以下は、お試しに使った、Mindstorms側のNXCのソース。

#include "NXCDefs.h"

#define I2C_PORT IN_1
#define READBUF_SIZE 2

byte writeBuf[2];
byte slaveAddrs1 = 0x21;
byte requestCmd = 0x41;
byte readBuf[READBUF_SIZE];

task main()
{
  int readCount = 0;
  int returnCount = 0;
  int i;
  byte n_read;
  
  // 受信バッファをクリア。
  for (i = 0; i < READBUF_SIZE; i++)
    readBuf[i] = 0;
  // 入力ポートをI2Cポートとして初期化。
  SetSensorLowspeed(I2C_PORT);
  
  // LCD画面クリア。
  ClearScreen();
  
  // メインループ。
  while(true)
  {
    if (ButtonPressed(BTNLEFT, false))
    { // 左ボタンを押されたら、
      TextOut(0, TEXTLINE_1, " LEFT");
      writeBuf[0] = 0x42;
      writeBuf[1] = 0x42;
      // I2C送信。
      I2CWrite(I2C_PORT, returnCount, writeBuf);
    }
    
    if (ButtonPressed(BTNRIGHT, false))
    { // 右ボタンを押されたら、
      TextOut(0, TEXTLINE_1, " RIGHT" );
      writeBuf[0] = 0x42;
      writeBuf[1] = 0x41;
      // I2C送受信。
      I2CBytes(I2C_PORT, writeBuf, readCount, readBuf);
    }
    
    // 時間待ち。
    Wait(10);
  }
}

以下、Arduino側のソース。

#include <Arduino.h>
#include <Wire.h>

byte myAddress = 0x21;
byte receiveData[10];
int receiveCount = 0;
byte sendData = 0x00;

// I2Cマスタからデータを受信した時のハンドラ。
void onI2cReceiveEvent(int getDataCount)
{
  int i;
  
  for (i = 0; i < getDataCount; i++)
  {
    receiveData[i] = Wire.read();
  }
  receiveCount = getDataCount;
}

// I2Cマスタからデータ要求を受信した時のハンドラ。
void onI2cRequestEvent()
{
  Wire.write(sendData);
}

// setup
void setup()
{
  // シリアルポート初期化。
  Serial.begin(115200);
  
  // I2Cスレーブとして初期化。
  Wire.begin(myAddress);
  // I2Cマスタからのデータ受信ハンドラを登録。
  Wire.onReceive(onI2cReceiveEvent);
  // I2Cマスタからのデータ要求受信ハンドラを登録。
  Wire.onRequest(onI2cRequestEvent);
}

// メインループ
void loop()
{
  int i;
  
  if (receiveCount > 0)
  {
    // ここに受信時の処理を記述。
    for (i = 0; i < receiveCount; i++)
    {
      Serial.print(receiveData[i], HEX);
      Serial.print(", ");
    }
    
    receiveCount = 0;
  }
}

　次に、もう一つのRS485での通信を試します。こちらが実現出来れば、I2Cも含めたセンサなどの処理をArduino側で分担させて、必要な指示なり結果をRS485でインタフェースするようなことができると思います。
　まずはRS485通信のためのArduino側のハードですが、今回は、シリアル-RS485変換モジュールがあったので、これを使うことにしました。（この変換モジュールはMAX485の互換チップが載っていて、~REとDEが結線された、よくある使い方のものです。）

Arduino側は今回はUnoを使ったので、SoftwareSerialで485の通信を行うことにしました。通信速度は38600bpsとしました。Mega(ADK)等を使うのであればPCとのUSB接続とは別にハードウェアのシリアルポートが利用できるので、通信速度も115200bpsまで使えると思います。Arduino側の（ソフトウェア）シリアル通信のRxD、つまりArduino側の受信ポートを485モジュールからの出力Tx-Oと接続、Arduinoの送信ポートTxDを485モジュールへの入力Rx-Iへ接続、Arduino側の送信制御用ポートを485モジュールのRTSへ接続します。Mindstormsの485通信のプロトコルがわからないので、とりあえず、Arduino側はRTSへの出力を通常はLowにしておき、いつでも受信できる状態としておき、Arduino側から送信したい時だけ、RTSをHighにして、送信を行い、また、RTSをLowにするような動作とします。RS485ライン側のMindstormsとの接続ですが、485モジュールのAをMindstormsのモジュラー端子6番のAで、Bを5のBでOKでした。485のA線B線表記はバラバラで統一がない（Aが負論理でBが正論理が正しいと聞いたことがあるが、ドライバICだと逆だったりもする）ので、とりあえず、試してみて、ひっくり返して付け替えるとか、ロジアナで覗いて確認してみるとかしないと、結局どっちなのかわからないことが多い気がします。

　当初、NXT側での受信動作がうまくいきませんでした。色々試したんですが、八方塞がりで諦めかけたんですが、結局、NXT側のファームウェアを別のものに書き換えたら動いてしまいました。ただし、受信したデータ（現段階では一文字のアルファベットを受信する動作）をNXT側の液晶に表示するところがうまく出来ません。使い方を間違っているのか？また、ファームの問題なのか？

　いずれにしても、このファームの問題で受信動作がうまく行かなかったことで、かなり、時間を食ってしまいました。
　以下、NXCのソース。

// RS485 communication test.

#include "NXCDefs.h"

inline void WaitForMessageToBeSent()
{
  bool sending, avail;
  
  RS485Status(sending, avail);
  
  while(sending)
  {
    Wait(MS_1);
    RS485Status(sending, avail);
  }
}

task main()
{
  int i = 0;
  string buffer;
  string sendMsg;
  
  // Configure the S4 port as RS485
  //SetSensorType(IN_4, SENSOR_TYPE_HIGHSPEED);
  UseRS485();
  RS485Enable();
  //RS485Control(HS_CTRL_INIT, HS_BAUD_9600, HS_MODE_DEFAULT);
  RS485Uart(HS_BAUD_38400, HS_MODE_8_DATA|HS_MODE_N_PARITY|HS_MODE_10_STOP);
  
  Wait(MS_10);
  
  SendRS485String("Hello,RS485!");
  TextOut(0, LCD_LINE1, "SendRS485String()");
  
  Wait(MS_10);
  
  while(true)
  {
    if (ButtonPressed(BTNLEFT, false))
    { // 左ボタンを押されたら、
      ClearScreen();
      TextOut(0, LCD_LINE1, " LEFT");
      sendMsg = "Left button pushed.";
      SendRS485String(sendMsg);
      WaitForMessageToBeSent();
    }
    
    if (ButtonPressed(BTNRIGHT, false))
    { // 右ボタンを押されたら、
      ClearScreen();
      TextOut(0, LCD_LINE1, " RIGHT");
      sendMsg = "Right button pushed.";
      SendRS485String(sendMsg);
      WaitForMessageToBeSent();
    }
    
    // 受信処理。
    if (RS485DataAvailable())
    {
      ClearScreen();
      RS485Read(buffer);
      SendRS485String(buffer);
      TextOut(0, LCD_LINE1, buffer);
    }
  }
}

以下、Arduino側のソース。

#include <Arduino.h>
#include <SoftwareSerial.h>

// RS485ピン定義（SoftwareSerial使用）
const int rs485RtsPin = 9;
const int rs485RxPin = 10;
const int rs485TxPin = 11;

SoftwareSerial rs485(rs485RxPin, rs485TxPin);

void RS485_initialize(void)
{
  //  SoftwareSerial初期化。
  rs485.begin(38400);
  // DTS出力ピン設定。
  pinMode(rs485RtsPin, OUTPUT);
  // 受信(DTS=0)状態に設定。
  digitalWrite(rs485RtsPin, LOW);
}

void RS485_write(byte data)
{
  // DTS出力ピンを送信(DTS=1)に設定。
  digitalWrite(rs485RtsPin, HIGH);
  delayMicroseconds(5);
  // 送信。
  rs485.write(data);
  // 受信(DTS=0)状態に設定。
  digitalWrite(rs485RtsPin, LOW);
}

void RS486_write(byte data[], int count)
{ 
  // DTS出力ピンを送信(DTS=1)に設定。
  digitalWrite(rs485RtsPin, HIGH);
  // 送信。
  for (int i = 0; i < count; i++)
 { 
    rs485.write(data[i]);
 }
  // 受信(DTS=0)状態に設定。
  digitalWrite(rs485RtsPin, LOW);
}

// 初期化。
void setup()
{
  // シリアルポート初期化。
  Serial.begin(9600);
  
  // RS485初期化。
  RS485_initialize();
}

void loop()
{
  int count;
  
  // RS485受信処理。
  if (rs485.available())
  {
    Serial.write(rs485.read());
  }
  
  // シリアルポート受信処理。
  if (Serial.available())
  {
    RS485_write(Serial.read());
  }
}

現時点では、とりあえず、動いたような・・・って感じなので、もう少し調査が必要ですが、NXTとArduino等のマイコンのインタフェースの可能性が一応開けたかな？
　あっ、そうそう、このRS485に関してはNXT側は必ずポート4を使う必要があります。（I2CはIN_1からIN_4のどこでもプログラム内で指定して使えますが、RS485はIN_4固定です。）

ロビ のサーボからステータスを取得してみる

　腰をダメにしてから全然運動しなくなってしまったんですが、昨日は久しぶりに軽く体を動かしたら、早速、二の腕が筋肉痛になってしまいました。今日は、おとなしく机に向かって、マイコンいじりです。
　さて、巷でも結構人気があるらしいディアゴのロビですが、現時点では、首部の水平動作と右腕（胴体とは繋がっていない）までが出来上がっている状態です。右腕の肘のサーボは以前にKinectを使って遊んでみました。本格的に動き出すのはまだ先だと思いますので、今回は首部分を使って、なにか遊べないかと考えてみました。ネットなどで色々調べていらっしゃる方々の情報を見させてもらうと、ロビで使われているサーボはROBO XEROに使われているFutabaのロボット用コマンドサーボと同じ中身と思われます。（コネクタは特殊ですが。）そこで、コマンド型のサーボの特長でもあるサーボ側ステータスの取得をしてみようと思います。
　TTLレベルでのシリアルコマンド制御になりますが、特長として、１本の信号線で送受信を行わなければなりません。つまり、半二重です。TTLレベルでのシリアル通信はマイコンの得意分野ですが、通常は全二重で、送信ラインと受信ラインは独立です。そこで、こちら（ディアゴのロボザックとかROBO XEROのときに色々と参考にさせてもらっていたサイトです）に掲載されていた回路を使わせていただいて、ロビのサーボをArduino Mega（互換ボード）で制御しました。

　USB-TTL変換でPCと通信もできるのと、ジャンパでTTLレベルでマイコンに直結できるようにして、今回は、Arduino に接続しています。

　ロビの首サーボをトルクOnにして、定常電流を定期的に取得して、取得した電流値がある閾値を超えた場合は、外部から力を加えられたと判断して、ロビのステータスが一段階変わります。（機嫌が悪くなる。）五段階のステータスで目の色を青から赤へ変化させ、五段階目でブチ切れて、イヤイヤをして、また、元の青色に戻ります。

S.H.A ACT.2

 GW後半の部に入りました。今日は、朝からハンダ付け♪。

　短いUSBケーブルやMILコネクタ・ケーブル、バニラシールドへの配線等を行いました。これらを何に使うかというと、

　前半に製作した熱源追跡台車のブレッドボードとジャンパ配線を置き換えて、スッキリさせました。

　センサの固定はあいかわらず、セロテープですけどね・・・。

　ブレッドボードとジャンパ線に比べれば、ずいぶんスッキリした感じ。ついでに、ランニングエレクトロニクスさんのSBDBTも実装できるようにして（まだ、使ってませんが）、ArduinoもUnoからMega ADKに交換しました。なぜ、Mega ADKに替えたかというと・・・

　台車を２階建てにして、

　USB スカッとミサイルランチャーを載せたかったからです。ミサイルを操作するためにUSBホスト機能が欲しかったわけです。

　熱源追跡台車ACT.2 です。ソフトの方がまだまだなので、とんでもない方向で発射したりします。発射のための熱検出のしきい値を低めに設定したら、色んな物に向かって発射してしまいました。プラズマテレビの排熱とかを検出して攻撃を仕掛けたりしてました。「この爆発は人間じゃねぇ・・・」

以下、2016年03月17日に追記しました。
Added on 03 May 17, 2016.

Arduinoのスケッチです。
Arduino sketch is bellow.
DRV8830のライブラリは自作です。
Library of DRV8830 is self-made.