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