Wiiヌンチャク + Arduino + RCB-1 で8軸ロボットを動かしてみた。

ArduinoはI2CでWiiヌンチャクの状態を取得し、RCB-1の低速シリアルのデータを返す。
加速度センサで移動、ジョイスティックで技が出せる。
http://youtube.com/watch?v=yeGf9P09vBo

左右に寝かすと横移動。
前に寝かすと前進、後ろに寝かすと後進。
ジョイスティックの左右でチョップを出す。

現状のArduinoのスケッチはこちら。
RCB-1の低速シリアルは負論理なので、SoftwareSerialのソースを修正して負論理で使えるようにして動作させた。
識者の良きアドバイスをお待ちしています。

/**
 * Wiiヌンチャク利用の8軸用コントローラ
 * 
 * Wiiヌンチャクの状態から、RCB-1 の低速シリアルをSoftwareSerial2
 * で制御する。
 *
 * SoftwareSerial2 は Arduino付属の SoftwareSerial の出力を正負逆転
 * させたもの。（hardware\libraries\SoftwareSerialフォルダを 
 * SoftwareSerial2フォルダとしてコピーし、SoftwareSerial.cpp を
 * SoftwareSerial2.cpp、SoftwareSerial.h を SoftwareSerial2.h に
 * にリネームし、それぞれのファイル内のSoftwareSerial・HIGH・LOWを
 * SoftwareSerial2・LOW・HIGHに置換しただけ。.oファイルがあれば削除
 * してから Sketch をコンパイルする。）
 *
 * (参考)
 * Wii ヌンチャクと通信してみた(2)CommentsAdd Star
 * http://d.hatena.ne.jp/clayfish/20090405/1238924970
 * Wiimote/Extension Controllers/Nunchuk
 * http://www.wiili.org/index.php/Wiimote/Extension_Controllers/Nunchuk
 */
#include <SoftwareSerial2.h> 
#include <Wire.h>

#define rxPin 2
#define txPin 3
#define ledPin 13

// 新たなシリアルポートを設定
SoftwareSerial2 mySerial =  SoftwareSerial2(rxPin, txPin);

uint8_t outbuf[6];		// array to store arduino output
int cnt = 0;

byte pinState = 0;  // LEDの状態

void setup() {
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  pinMode(ledPin, OUTPUT);
  mySerial.begin(2400);	// 2400bpsでポートを開く
  Serial.begin(9600);	// 9600bpsでポートを開く
  Wire.begin ();    // join i2c bus with address 0x52
  nunchuck_init (); // send the initilization handshake
}

void
nunchuck_init ()
{
  Wire.beginTransmission (0x52);	// transmit to device 0x52
  Wire.send (0x40);		// sends memory address
  Wire.send (0x00);		// sends sent a zero.  
  Wire.endTransmission ();	// stop transmitting
}

void
send_zero ()
{
  Wire.beginTransmission (0x52);	// transmit to device 0x52
  Wire.send (0x00);		// sends one byte
  Wire.endTransmission ();	// stop transmitting
}

void
loop ()
{
  Wire.requestFrom (0x52, 6);	// request data from nunchuck
  while (Wire.available ()) {
    outbuf[cnt] = nunchuk_decode_byte (Wire.receive ());	// receive byte as an integer
    cnt++;
  }
  // If we recieved the 6 bytes, then go print them
  if (cnt >= 5) {
    print ();
  }
  cnt = 0;
  send_zero (); // send the request for next bytes
  delay (50);
}


void
print ()
{
  int threshold = 16;
  int joy_x_axis   = outbuf[0];
  int joy_y_axis   = outbuf[1];
  int accel_x_axis = outbuf[2]; 
  int accel_y_axis = outbuf[3];
  int accel_z_axis = outbuf[4];
  int z_button = 0;
  int c_button = 0;
  
  if ((outbuf[5] >> 0) & 1) z_button = 1;
  if ((outbuf[5] >> 1) & 1) c_button = 1;
  if ((outbuf[5] >> 2) & 1) accel_x_axis += 2;
  if ((outbuf[5] >> 3) & 1) accel_x_axis += 1;
  if ((outbuf[5] >> 4) & 1) accel_y_axis += 2;
  if ((outbuf[5] >> 5) & 1) accel_y_axis += 1;
  if ((outbuf[5] >> 6) & 1) accel_z_axis += 2;
  if ((outbuf[5] >> 7) & 1) accel_z_axis += 1;
  
  // RCB-1 send data
  int b1 = 0xf0;
  int b2 = 0x00;
  
  // 1byte 
  if (c_button == 0) b1 |= 0b00000001;
  if (z_button == 0) b1 |= 0b00000010;
  
  // 2byte 
  if (accel_y_axis > 128 + threshold) b2 |= 0b00000001;
  if (accel_y_axis < 128 - threshold) b2 |= 0b00000010;
  if (accel_x_axis > 128 + threshold) b2 |= 0b00000100;
  if (accel_x_axis < 128 - threshold) b2 |= 0b00001000;
  if (joy_y_axis > 128 + threshold)   b2 |= 0b00010000;
  if (joy_y_axis < 128 - threshold)   b2 |= 0b00100000;
  if (joy_x_axis > 128 + threshold)   b2 |= 0b01000000;
  if (joy_x_axis < 128 - threshold)   b2 |= 0b10000000;

  delay (17);
  mySerial.print(b1, BYTE);        // 文字を送信
  mySerial.print(b2, BYTE);        // 文字を送信
  
  toggle(ledPin); // 動作が見えるよう、受信するたびにLEDを反転

  Serial.print (joy_x_axis, DEC);
  Serial.print (",");
  Serial.print (joy_y_axis, DEC);
  Serial.print (",");
  Serial.print (accel_x_axis, DEC);
  Serial.print (",");
  Serial.print (accel_y_axis, DEC);
  Serial.print (",");
  Serial.print (accel_z_axis, DEC);
  Serial.print (",");
  Serial.print (z_button, DEC);
  Serial.print (",");
  Serial.print (c_button, DEC);
  Serial.print (",");
  Serial.print (b1, DEC);
  Serial.print (",");
  Serial.print (b2, DEC);
  Serial.print("\r\n");
}

char
nunchuk_decode_byte (char x)
{
  x = (x ^ 0x17) + 0x17;
  return x;
}

void toggle(int pinNum) {
  digitalWrite(pinNum, pinState);
  pinState = !pinState;  // pinStateが0なら1、1なら0に反転
}