2013年12月23日月曜日

LEGO Mindostorms EV3 と Arduino を接続してみた

 EV3のコア(コントロール)ブロックにはUSB host (Aコネクタ)端子があって、ARM CPU で Angstrom Linux が動作しています。であれば、Raspberry Pi のときのようにArduinoを接続できれば、Arduinoのアナログ入力を使って色々なセンサを利用したり、EV3モータ以外のモータやラジコンサーボをPWM制御することもできて、可能性が広まります。
 そこで、EV3のUSBにArduinoを接続してみたのですが認識されません("/dev/ttyACM0"ができません)でした。どうやら、EV3のAngstromでは、USB-Serialのドライバ(CDC-ACM)がデフォルトでは入っていないようです。なので、CDC-ACMドライバを組み込んでのカーネル再構築を試してみることにしました。

 まずは、カーネルを含めて、EV3用のCプログラムをコンパイルできる開発環境を用意する必要があります。これに関しては、Mindstorms教育版の日本の代理店のAfrelさんが「カメラとの接続方法」「ネイティブLinuxプログラミング方法」等を公開してくれていますので、そちらを参考にして準備して下さい。AfrelさんではEV3教育版やNXT,EV3の拡張キット、センサ・モータ類も扱っているので何度かお世話になっていますが、こういった、貴重な情報を公開してくれるのでありがたい存在です。

今回のArduino接続用のCDC-ACMドライバの組み込みも、基本的には「カメラとの接続方法」でのカーネルの再構築方法に沿って行います。

カメラとの接続方法」にしたがって、途中まで準備、あるいは既にカーネルを構築できる環境がある前提です。「カメラとの接続方法」の1.1の(10)のところを以下で読み替え、あるいは、平行して見ていただければと思います。

---------------------------------------------------


カーネルソースコードのルートディレクトリに移動します。

   cd ~/projects/extra/linux-03.20.00.13/

カーネルの設定を変更するために以下のオプションでmake menuconfigを実行します。

   make menuconfig ARCH=arm CROSS_COMPILE=arm-none-linux-gnueabi-

 Device Driversを選択してEnterキーを押して下さい。


 次にUSB support でEnter


 USB Modem (CDC ACM) supportを選択してspaceキーを2回押して、<*>状態にします。


 ESCキーを何回か押して、以下の確認画面でYesを選択して、保存して終了します。


 configファイルに書き込まれた以降の操作は、また、「カメラとの接続方法」に準じます。

---------------------------------------------------


 こうして再構築したカーネルをmicroSDで起動したEV3に改めてArduinoを接続したところ、どうやら、"/dev/ttyACM0"として認識してくれるようになりました。

 そこで、簡単な動作確認として、EV3とUSB接続したArduino UnoのAnalog入力にPSDセンサを接続して、PSDセンサの出力電圧をAD変換したものをEV3側で受信するテスト動作をさせてみました。


 Arduino側で一定周期でADを読み込んで、シリアル通信で0x05を受信したタイミングで保持しているAD測定結果値をシリアル通信で送信します。
 EV3側では0.5秒おきくらいに10回0x05をArduinoに送信して、返ってきた受信バイナリからch0の測定値をcmに変換して、コンソールに表示しています。

 以下、Arduino側のソースです。
#include <Arduino.h>
#include <MsTimer2.h>

// global variables.
int adVal;
byte sendData[16];

// Callback function of MsTimer2's interruption.
void CallbackMsTimer2()
{
 for (int i = 0; i <= 5; i++)
 {
  // Read from analog pin.
  adVal = analogRead(i);
  // Configure send data.
  sendData[2 * i + 1] = (byte)((0xff00 & adVal) >> 8);
  sendData[2 * i + 2] = (byte)(0x00ff & adVal);
 }
}

void setup()
{
 // initialize global variables.
 adVal = 0;
 for (int i = 0; i < 16; i++)
  sendData[i] = 0x00;
 sendData[0] = 0x02;
 sendData[15] = 0x03;

 // initialize Serial IF.
 Serial.begin(9600);

 // Set MsTimer2 interval to 100ms.
 MsTimer2::set(100, CallbackMsTimer2);

 // Start MsTimer2.
 MsTimer2::start();
}

void loop()
{
 if (Serial.available() > 0)
 {
  if (Serial.read() == 0x05)
  {
   // Send data on received ENQ.
   for (int i = 0; i < 16; i++)
   {
    Serial.write(sendData[i]);
   }
  }
 }
}

 EV3側のソースです。
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <termios.h>
#include <sys/wait.h>
#include <sys/ioctl.h>

// Definition of Serial communication.
#define BAUDRATE 9600
#define MODEMDEVICE "/dev/ttyACM0"

// global variables.
struct termios oldTermio; // for keep original setting.

// Prototype of function.
int Serial_begin(int brate, char* devicePath);
void Serial_end(int fd);
int Serial_write(int fd, uint8_t byte);
uint8_t Serial_read(int fd);
int analogToCentimeter(int analogValue);


int main(int argc, char *argv[])
{
 int fd = -1;
 uint8_t rcvByte[16];
 int data;
 int i, j;

 // Initialize serial port.
 fd = Serial_begin(BAUDRATE, MODEMDEVICE);
 if (fd < 0)
 {
  printf("Serial port initialize Error.\n");
  exit(-1);
 }

 for (i = 0; i < 10; i++)
 {
  Serial_write(fd, 0x05);

  usleep(500);

  for (j = 0; j < 16; j++)
  {
   rcvByte[j] = Serial_read(fd);
  }

  data = (int)( (((int)rcvByte[1]) << 8) | ((int)rcvByte[2]) );
  data = analogToCentimeter(data);

  printf("%d\n", data);

  usleep(500000);
 }

 return 0;
}


int Serial_begin(int brate, char* devicePath)
{
 int fd;
 struct termios newTermio;

 // open the device.
 fd = open(devicePath, O_RDWR | O_NOCTTY);
 if (fd < 0)
 {
  // failed to open device.
  perror(devicePath);
  exit(-1);
 }

 // get original setting and keep it.
 tcgetattr(fd, &oldTermio);

  // Initialize new_term_io' control code.
 newTermio.c_iflag = 0;
 newTermio.c_oflag = 0;
 newTermio.c_cflag = 0;
 newTermio.c_lflag = 0;
 newTermio.c_line = 0;

 memset(newTermio.c_cc, '\0', sizeof(newTermio.c_cc));
 
 //  Configure port.
 //  B9600 = 9600bps(default)
 //  CS8 = 8bit, no parity, 1 stopbit
 //  CLOCAL = local(non modem control)
 //  CREAD = enable read charactor
 switch (brate)
 {
 case 300:
  newTermio.c_cflag = B300 | CS8 | CLOCAL | CREAD;
  break;
 case 1200:
  newTermio.c_cflag = B1200 | CS8 | CLOCAL | CREAD;
  break;
 case 2400:
  newTermio.c_cflag = B2400 | CS8 | CLOCAL | CREAD;
  break;
 case 4800:
  newTermio.c_cflag = B4800 | CS8 | CLOCAL | CREAD;
  break;
 case 9600:
  newTermio.c_cflag = B9600 | CS8 | CLOCAL | CREAD;
  break;
 case 19200:
  newTermio.c_cflag = B19200 | CS8 | CLOCAL | CREAD;
  break;
 case 38400:
  newTermio.c_cflag = B38400 | CS8 | CLOCAL | CREAD;
  break;
 case 57600:
  newTermio.c_cflag = B57600 | CS8 | CLOCAL | CREAD;
  break;
 case 115200:
  newTermio.c_cflag = B115200 | CS8 | CLOCAL | CREAD;
  break;
 default:
  newTermio.c_cflag = B9600 | CS8 | CLOCAL | CREAD;
  break;
 }


 // Setting parity error is ignore.
 newTermio.c_iflag = IGNPAR;

 // Raw mode output.
 newTermio.c_oflag = 0;
 // Setting input mode. non-canonical, no echo,
 newTermio.c_lflag = 0;
 // inter-character timer
 newTermio.c_cc[VTIME] = 0;
 // Read block still received one char.
 newTermio.c_cc[VMIN] = 1;
 // Clear modem line.
 tcflush(fd, TCIFLUSH);
 
 // Apply new configure.
 tcsetattr(fd, TCSANOW, &newTermio);

 // Wait.
 sleep(3);

 // Return file descriptor.
 return (fd);
}

void Serial_end(int fd)
{
 if (fd >= 0)
 {
  tcsetattr(fd, TCSANOW, &oldTermio);
  close(fd);
 }
}

int Serial_write(int fd, uint8_t byte)
{
 ssize_t res;

 res = write(fd, &byte, 1);

 return (int)res;
}

uint8_t Serial_read(int fd)
{
 uint8_t c;

 read(fd, (char *)&c, 1);

 return c;
}

int analogToCentimeter(int analogValue)
{
    return (int)((6787 / (analogValue - 3)) - 4);
}

 EV3の実行結果をターミナルで見たところ。


 PSDセンサの前で手や板を前後させて、値がそれっぽく変化するのを確認しました。