2014年6月27日金曜日

レゴでステアリング

 今回は、レゴのテクニックパーツでステアリングを組んで見ました。過去に組み立てたキットやネットの情報を元に現物合わせでも付け外ししながら動きを確かめつつ組み上げ、結果をLEGO Digital Designer で記録していきました。



 まずは、シンプルなステアリング。十字ギアとリンクで軸の回転を舵に変換します。本当は厳密にはステアリングを切った際には曲がる内側の車輪のほうがより強く舵を切らないといけないのだと思いますが、そのへんは簡略化しています。



 組立図は http://www.cyber-robotics.jp/lego/steering1/ にあります。

 もう一つは、デフを使って、動力も伝えられるようにした版です。



 組立図は http://www.cyber-robotics.jp/lego/steering2/ にあります。
 その他の組立図も http://www.cyber-robotics.jp/lego/ にあります。


 これを使って、何を作るか?・・・は、まだ考えていません。


2014年6月22日日曜日

BP-AT (BrickPi - Armed Transport)

 今回は、また、BrickPi ネタです。LEGO STARWARS の新作 75054 AT-AT を組み立てた のですが、そしたら、何か、4脚的なものを作ってみたくなりました。ただ、歩行機構をリンクなどを使って一から考えるのは、大変(というか、自分には無理そう)なので、EV3拡張キットで組み立てられるモデルの一つの「象」の歩行機構を流用することにしました。


 この「象」のモデルでは、Lモータ1つで四脚を駆動して歩行出来るようになっている他、Mモータで鼻を動かしたりできます。今回はこの「象」の四脚部分をそのまま流用しつつ、左右に分割して、真ん中の鼻を動かしたりする機構を取り除き、パワーを上げるため、左右それぞれを別のLモータで駆動するように改造しました。また、左右を別々のモータにしたために、左右脚の位置(位相)が合わなくなるので、タッチスイッチを使って簡易型の原点検出機構を組み込みました。
 モータとタッチスイッチをBrickPi(シールド)のMindstormsコネクタに接続して、武装として「USBスカッとミサイルランチャー」をRaspberry Pi のUSBコネクタに接続しています。ミサイルランチャーのLEGOへの固定はマジックテープです。

 AT-AT と並べてみました。Pi-AT (BrickPi -Armed Transport : BrickPi 武装トランスポータ)です。



 頭?の部分は以前に組み立てたDark Side Developers Kit の組み立てマニュアルの情報で作りました。


 スカッとミサイルランチャー はマジックテープで固定してあります。



 ミサイルランチャーを取り外すと、BrickPi と電源の エネループ × 8本 の電池ボックスがあります。





 電池ボックスの下あたりに、タッチスイッチを使った原点検出機構を仕込みました。


 ひっくり返して、おなか側から。



 BrickPiのプログラムはこんな感じです。例によって、Dualshock 3 をBluetooth接続して、コントローラとして使っています。一応、左アナログコントローラで前後歩行と左右旋回、右アナログコントローラでミサイルランチャー操作、右アナログボタンでミサイル発射のつもりで作ったんですが、残念ながら左右旋回は(足の動きを上手く制御できておらず)まだ不可です。歩行は今のところ前後のみ。それと、ミサイルランチャーのカメラのOpenCVからの制御も相変わらずできてません。この辺は今後の課題・・・。

#include <stdio.h>
#include <math.h>
#include <time.h>
#include <linux/i2c-dev.h>
#include <fcntl.h>

#include <stdlib.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/input.h>
#include <linux/joystick.h>

#include "tick.h"
#include <wiringPi.h>
#include "BrickPi.h"

// USBミサイルランチャ関連ヘッダ
#include <ctype.h>
#include <unistd.h>
#include <usb.h>
#include <opencv/highgui.h>


#define HID_REPORT_SET (0x09)
#define BULK_OUT (0x02)

#define MISSILE_VENDOR_ID (0x2123)
#define MISSILE_PRODUCT_ID (0x1010)
#define MISSILE_CMD_HEADER (0x02)
#define MISSILE_CMD_DOWN (0x01)
#define MISSILE_CMD_UP (0x02)
#define MISSILE_CMD_LEFT (0x04)
#define MISSILE_CMD_RIGHT (0x08)
#define MISSILE_CMD_FIRE (0x10)
#define MISSILE_CMD_STOP (0x20)

#define MAX_ANALOG_RANGE (32767)
#define MIN_ANALOG_RANGE (-32767)
#define ANALOG_RANGE (MAX_ANALOG_RANGE - MIN_ANALOG_RANGE)
#define MAX_MOTOR_SPEED (100)

#define WALK_CMD_STOP (0x00)
#define WALK_CMD_FORWARD (0x01)
#define WALK_CMD_BACK (0x02)
#define WALK_CMD_TURN_LEFT (0x04)
#define WALK_CMD_TURN_RIGHT (0x08)

const int USB_TIMEOUT = 3000;
const int BUF_SIZE = 8;

int main(int argc, char *argv[])
{
 int result;
 int motor[2];
 int sensor[2];
 int i, j;
 int speedL, speedR;
 int missileCmd;
 int walkCmd;
 int lastWalkCmd = (int)(WALK_CMD_STOP);
 int legInit = 0;

 // USB関連変数。
 struct usb_bus *bus;
 struct usb_device *dev;
 usb_dev_handle *udh;
 struct usb_config_descriptor *config;
 struct usb_interface *interface;
 struct usb_interface_descriptor *altsetting;
 unsigned char usbSendData[8];

 // 画像処理関連変数。
 CvCapture *capture = 0;
 IplImage *frame = 0;
 //double width = 160, height = 120;
 double width = 320, height = 240;

 // ジョイスティック変数。
 int *axis;
 char *button;
 int numOfAxis;
 int numOfButton;
 int fd_j;
 struct js_event js;
 
 // -- Joystick 初期化 --
 // デバイスオープン
 if ((fd_j = open("/dev/input/js0", O_RDONLY)) < 0)
 {
  printf("Error in Opening Joystick.\n");
 }
 // 軸・ボタン数取得。
 ioctl(fd_j, JSIOCGAXES, &numOfAxis);
 ioctl(fd_j, JSIOCGBUTTONS, &numOfButton);
 // 今回は軸数・ボタン数の取得がうまく行かなかったので、固定で。
 numOfAxis = 19;
 numOfButton =16;
 axis = (int*)calloc(numOfAxis, sizeof(int));
 for (i = 0; i < numOfAxis; i++)
  axis[i] = 0;
 button = (char*)calloc(numOfButton, sizeof(char));
 for (i = 0; i < numOfButton; i++)
  button[i] = 0;
 // ノンブロッキングモード。
 fcntl(fd_j, F_SETFL, O_NONBLOCK);


 // -- BrickPi 初期化 --
 // タイマクリア。
 ClearTick();
 // ポートオープン。
 result = BrickPiSetup();
 if (result)
 {
  printf("BrickPi Initializing Error.");
  return -1;
 }
 // アドレス設定。
 BrickPi.Address[0] = 1;
 BrickPi.Address[1] = 2;
 
 // モータポート設定。
 motor[0] = PORT_B; // PORT_BをLモータとして使用。
 motor[1] = PORT_C; // PORT_CをRモータとして使用。
 // モータ有効化。
 for (i = 0; i < 2; i++)
  BrickPi.MotorEnable[motor[i]] = 1;
 // タッチセンサ利用の設定。PORT_2,3 を使用。
 sensor[0] = PORT_2;
 sensor[1] = PORT_3;
 for (i = 0; i < 2; i++)
  BrickPi.SensorType[sensor[i]] = TYPE_SENSOR_TOUCH;
 // BrickPiの設定を更新。
 result = BrickPiSetupSensors();
 if (result)
 {
  printf("BrickPi Setup Error.");
  return -1;
 }

 // 左右脚の初期位置への移動。
 for (i = 0; i < 2; i++)
 {
  BrickPi.MotorSpeed[motor[i]] = 100;
  while (1)
  {
   BrickPiUpdateValues();
   usleep(10000);
   if (BrickPi.Sensor[sensor[i]] > 0)
   {
    BrickPi.MotorSpeed[motor[i]] = 0;
    BrickPi.Encoder[motor[i]] = 0;
    break;
   }
  }
 }
 

 // --- USBミサイルランチャ初期化 ---
 // USB初期化。
 usb_init();
 usb_find_busses();
 usb_find_devices();
 for (i = 0; i < 8; i++)
  usbSendData[i] = 0x00;
 usbSendData[0] = MISSILE_CMD_HEADER;
 
 // ミサイルランチャーのデバイスチェック。
 for(bus = usb_get_busses(); bus; bus = bus->next)
 {
  for(dev = bus->devices; dev; dev = dev->next)
  {
   if(dev->descriptor.idVendor == MISSILE_VENDOR_ID &&
    dev->descriptor.idProduct == MISSILE_PRODUCT_ID)
   {
    // デバイスオープン。
    if((udh = usb_open(dev)) == NULL)
    {
     // オープンに失敗。
     printf("usb_open Error.(%s)\n", usb_strerror());
     exit(-1);
    }
    
    printf("found usb missile launcher\n");

    config = &dev->config[0];
    interface = &config->interface[0];
    altsetting = &interface->altsetting[0];
    
    if (usb_set_configuration(udh, config->bConfigurationValue) < 0)
    {
     if (usb_detach_kernel_driver_np(udh,
              altsetting->bInterfaceNumber) < 0)
     {
      printf("usb_set_configration() error.\n");
      usb_close(udh);
      exit(-1);
     }
    }
    
    // インタフェースの使用をシステムに通知。
    if (usb_claim_interface(udh, altsetting->bInterfaceNumber) < 0)
    {
     // インタフェース使用要求でエラー。
     printf("claiming interface error\n");
     exit(-1);
    }
   }
  }
 }
 /*
 // カメラに対するキャプチャ構造体を作成。
 if (argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
  capture = cvCreateCameraCapture(argc == 2 ? argv[1][0] - '0' : 0);
 
 // キャプチャサイズの設定。
 cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, width);
 cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, height);
 
 // ウィンドウ作成。
 cvNamedWindow("Capture", CV_WINDOW_AUTOSIZE);
 */

 // --- メインループ ---
 while(1)
 {
  // Joystick読み出し。
  if (read(fd_j, &js, sizeof(struct js_event)) == sizeof(struct js_event))
  {
   switch (js.type & ~JS_EVENT_INIT)
   {
   case JS_EVENT_AXIS:  // スティック操作イベント
    axis[js.number] = js.value;
    break;
   case JS_EVENT_BUTTON: // ボタン操作イベント
    button[js.number] = js.value;
    break;
   }
  }

  // --- Joystick値から動作情報生成 ---
  // axis[0]=左アナログX軸, axis[1]=左アナログY軸, axis[2]=右アナログX軸, axis[3]=右アナログY軸
  // 移動情報生成。
  walkCmd = WALK_CMD_STOP;
  if (axis[1] < (int)(MIN_ANALOG_RANGE /3))
  {
   walkCmd |= WALK_CMD_FORWARD;
  }
  else if (axis[1] > (int)(MAX_ANALOG_RANGE / 3))
  { 
   walkCmd |= WALK_CMD_BACK;
  }
  if (axis[0] < (int)(MIN_ANALOG_RANGE /3))
  {
   walkCmd |= WALK_CMD_TURN_LEFT;
  }
  else if (axis[0] > (int)(MAX_ANALOG_RANGE /3))
  {
   walkCmd |= WALK_CMD_TURN_RIGHT;
  }
  switch (walkCmd)
  {
  case WALK_CMD_FORWARD:
   BrickPi.MotorSpeed[motor[0]] = -200;
   BrickPi.MotorSpeed[motor[1]] = -200;
   break;
  case WALK_CMD_BACK:
   BrickPi.MotorSpeed[motor[0]] = 200;
   BrickPi.MotorSpeed[motor[1]] = 200;
   break;
  case WALK_CMD_TURN_LEFT:
   BrickPi.MotorSpeed[motor[0]] = -200;
   BrickPi.MotorSpeed[motor[1]] = 200;
   break;
  case WALK_CMD_TURN_RIGHT:
   BrickPi.MotorSpeed[motor[0]] = 200;
   BrickPi.MotorSpeed[motor[1]] = -200;
   break;
  case WALK_CMD_STOP:
   if ((legInit == 0) && (lastWalkCmd != WALK_CMD_STOP)) legInit = 1;
   BrickPi.MotorSpeed[motor[0]] = 0;
   BrickPi.MotorSpeed[motor[1]] = 0;
   break;
  default:
   break;
  }

  // ミサイルコマンド生成。
  missileCmd = 0x00;
  if (axis[2] < (int)(MIN_ANALOG_RANGE / 3))
  {
   missileCmd |= MISSILE_CMD_LEFT;
  }
  else if (axis[2] > (int)(MAX_ANALOG_RANGE / 3))
  {
   missileCmd |= MISSILE_CMD_RIGHT;
  }
  if (axis[3] < (int)(MIN_ANALOG_RANGE / 3))
  {
   missileCmd |= MISSILE_CMD_UP;
  }
  else if (axis[3] > (int)(MAX_ANALOG_RANGE / 3))
  {
   missileCmd |= MISSILE_CMD_DOWN;
  }
  if (axis[13] > (int)(MAX_ANALOG_RANGE / 3))
  {
   missileCmd |= MISSILE_CMD_FIRE;
  }
  // --- 動作処理 ---
  BrickPiUpdateValues();
  usbSendData[1] = missileCmd;
  // USBへ送信。
  if (usb_control_msg(
   udh,
   USB_ENDPOINT_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
   HID_REPORT_SET,
   0,
   0,
   (char*)usbSendData,
   sizeof(usbSendData),
   USB_TIMEOUT) < 0)
  {
   // USB送信エラー。
   printf("usb write error.\n");
  } 
  usleep(10000);
  if (legInit == 1)
  {
   legInit = 0;
   // 左右脚の初期位置への移動。
   for (i = 0; i < 2; i++)
   {
    BrickPi.MotorSpeed[motor[i]] = 100;
    while (1)
    {
     BrickPiUpdateValues();
     usleep(10000);
     if (BrickPi.Sensor[sensor[i]] > 0)
     {
      BrickPi.MotorSpeed[motor[i]] = 0;
      BrickPi.Encoder[motor[i]] = 0;
      break;
     }
    }
   }
  }
  lastWalkCmd = walkCmd;
 }
 /*
 // キャプチャ解放。
 cvReleaseCapture(&capture);
 // ウィンドウ破棄。
 cvDestroyWindow("Capture");
 */
 // USBデバイスクローズ。
 usb_close(udh);

 return 0;
}

 それと、もう一つ問題があって、ミサイル発射のときや、歩行とランチャー駆動等の負荷が重なった時にシステム(Raspberry Pi)がかなりの頻度で落ちます。BrickPi ではBrickPi側で電源を生成して、Raspberry Pi の5VラインにGPIO端子経由で直接供給するので、Raspberry Pi の電源コンデンサが弱い問題もある程度はクリアされると思うんだけど・・・。(ちなみにBrickPiは1000μくらいの電解コンデンサを電源に積んでいます)

 とりあえず、動かしてみたところです。




iPega PG-9071 Android用Bluetooth GamePad と Dualshock, 3 の電池交換

 自分は、(ゼルダシリーズ以外は)あまりゲームはやらない方ですが、Android端末用のBluetoothゲームコントローラで、iPega PG-9071 とうのをアマゾンで購入しました。ちょっと空いた時間に遊んでみるのもいいかなと思って買ったんですが、このゲームコントローラが初期不良でひどいものでした。まあ、返品なり交換してもらえばいいのですが、安物ですし、せっかくなのでいつもの「分解」で楽しませてもらうことにしました。


 タッピングネジを6本ほど外すと、裏蓋が外れました。


 この基板上のハンダ付けに何箇所か怪しいところがあり、ハンダゴテで再加熱したり、ハンダを付け足したりして、流し込みました。


 上の写真は、充電池からの電源コードの拡大です。

 ハンダ付けに怪しい部分がある以外にも、「ハンダくず」がやたらくっついています。P板のパターンはレジストというか皮膜されているので大丈夫なのかもしれませんが、表面実装の抵抗のランドやスルーホール部分にも「これショートしてねぇ?」っていう感じでくっついています。古歯ブラシで表面をひと通りブラッシングして、ブロアで吹いて、再組み立てを行いました。

 どうにか使えるようになりましたが、これはお勧めできません。もちろん、不良品だったのは偶然というか確率の問題だと思いますが、中を見るとその不良の確率が高いに違いないことは容易に想像できます。

 ちなみに今回、合わせて、いつもロボットのコントロールなどに使っている(ちなみにプレステは持ってません・・・) Dualshock 3 が充電してもすぐに電池切れになるので、充電池の交換を行ってみました。


 交換用の電池パック(amazonで入手)とDualshock 3。もちろん、自己責任で。
 何本か裏側のネジを外して、上下の「爪」の引っ掛かりを外すと外れます。LRのアナログボタンが外れやすいので慎重に裏蓋を外します。(まあ、外れても取り付けできますので大丈夫です。ただし、スプリングを正しく噛ませないと機能しなくなるので、はじめの状態を記憶または撮影しておくといいかも)


 先ほどのゲームパッドでは充電池は汚いハンダ付けで固定されていましたが、Dualshock 3 はコネクタで接続されています。(もちろん、ハンダくずはありません・・・・)


 コネクタを抜いて(ラジオペンチなどで摘んで静かに外すといいでしょう)、交換用の電池パックのコネクタを挿します。


 (LRアナログボタンに注意して)裏蓋を閉じて、ネジを締めれば、交換完了。電池の持ちが復活しました。

 やっぱり、あやしい中国製?ゲームパッドと(Made in China なんだろうけど)ソニー製のDualshock 3 では「つくり」が違いますね。


LEGO STARWARS 8089 Hoth Wampa Cave

 久々のブログ更新です。ネタはいくつか溜まっているんですが、更新が滞っていました・・・。さて、今回は、「LEGO STARWARS 8089 Hoth Wampa Cave (ワンパの洞窟)」です。



まずは、パッケージ。



 開梱。


 部品数はたいしたことありません。わりと大きめのプレートやらスロープやら四角柱状ブリックが多いのと、なんといっても、「ワンパ」が目立ちます。

 では、組み立て開始。まずは、スノースピーダからです。









 スノースピーダ完成。





 次は、ワンパの洞窟。ミニフィグから。






 洞窟完成。ルークをぶら下げて・・・。


 ライトセーバをフォースで手元に引き寄せるところ。


 ひくひく・・・。


 ズバァ~ッ。


 旧三部作を久しぶりに見たくなっちゃったなぁ。