2014年8月9日土曜日

3Dプリンタで多脚ロボット

 3Dプリンタを導入して2週間が経ちましたが、実際に動かせるのは休みの日曜・祝日くらいなので、なかなか思ったように作業が進まないのですが、どうにか初作品のベース部分がそれっぽい形になってきました。









なんだかごちゃごちゃしてますが、六脚ロボットです。サーボはアマゾンで10個入り三千いくらで買ったSG90という小型のものです。動作精度やトルク・早さはもちろん期待できませんが、足の上げ下げ、左・中・右・・・くらいのおおまかな制御ができればいいだろうということで何よりもリーズナブルな価格で採用しました。

 サーボの制御は例によっていつものAdafruitのPCA9685搭載16chPWMコントローラです。まずは、Ardino Unoと接続してサーボ制御プログラムの作成。


 単純に足を上下前後に繰り返し動かすシーケンスですが、なんとなく前に進むことはできそうな動きをしているのでOKとします。

 さらに、3Dプリンタで作った目玉パーツを取り付けます。目玉の中にはスイッチサイエンスさんで入手した Adafruit の I2C接続 8x8 小型マトリックスLED を入れました。




 また、Arduino Uno をArduino Micro に置き換えて、ロボット上に実装しました。電源は搭載が難しそうなので、外部からの有線供給とし、この外部供給 5Vをサーボと制御系で併用するようにしました。ノイズ等での誤動作が心配ですが、パスコンを入れて試してみます。



 全体を組み立てて、



 背中に Arduino Microと


Adafruit 16ch Servo Driverを載せています。


 動かしてみたところ。



 歩き方が雑でぎこちないですが、どうにか動いています。ただ、はじめは、マトリックスLEDのI2Cアドレスを 0x70と0x71にしていたらサーボドライバがなぜか動きませんでした。マトリックスLEDのアドレスを0x71と0x72に変えたところ動くようになりました。別にサーボドライバ(0x40)とアドレスがかぶっているわけでもないですし、プログラム中で指定しているアドレスが間違っているわけでもなかったのですが、不思議です。原因不明。とりあえず今回のソースはかんな感じ。
#include <Wire.h>
#include <TimerOne.h>
#include <Adafruit_PWMServoDriver.h>
#include "Adafruit_16ch_Servo.h"

#include <Adafruit_GFX.h>
#include <Adafruit_LEDBackpack.h>

double SERVO_CENTER_PULSE_WIDTH = 1520.0; // サーボ信号。センターパルス幅。
double SERVO_MIN_PULSE_WIDTH = 720.0;  // サーボ信号。最小パルス幅。
double SERVO_MAX_PULSE_WIDTH = 2320.0;  // サーボ信号。最大パルス幅。

const int led_pin = LED_BUILTIN; // モニタLED。

// マトリックスLEDのアドレス指定。デフォルトは0x70。
static const uint8_t matrixAddress[] = {0x71, 0x72};

static boolean 
 output = HIGH,    // LED点滅フラグ。
 timerCallbacked = false; // タイマ割り込みフラグ。

static int
 actionSeq = 0,
 keepCount = 0;

uint8_t
 blinkCountdown = 100, // 瞬き用カウント。
 gazeCountdown = 75,  // 目玉向き用カウント。
 gazeFrames = 50;  // 目玉動く早さ。

int8_t
 eyeX = 3, eyeY = 3,  // 現在の目玉位置。
 newX = 3, newY = 3,  // 次の目玉位置。
 dX = 0, dY = 0;   // 位置の差分。

// サーボモータのモーション情報。
static int motion[11][17] = {
//  {ch0, ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, ch9,ch10,ch11,ch12,ch13,ch14,ch15,time(x100ms)}
 {  0,  50,   0,   0,   0,  50,   0,   0,   0, -50,   0,   0,   0,   0,   0,   0, 3},
 { 20,   0, -20,   0,  20,   0,  20,   0, -20,   0,  20,   0,   0,   0,   0,   0, 3},
 {  0,   0,   0,  50,   0,   0,   0, -50,   0,   0,   0, -50,   0,   0,   0,   0, 3},
 {-20,   0,  20,   0, -20,   0, -20,   0,  20,   0, -20,   0,   0,   0,   0,   0, 3},
 {  0,  50,   0,   0,   0,  50,   0,   0,   0, -50,   0,   0,   0,   0,   0,   0, 3},
 { 20,   0, -20,   0,  20,   0,  20,   0, -20,   0,  20,   0,   0,   0,   0,   0, 3},
 {  0,   0,   0,  50,   0,   0,   0, -50,   0,   0,   0, -50,   0,   0,   0,   0, 3},
 {-20,   0,  20,   0, -20,   0, -20,   0,  20,   0, -20,   0,   0,   0,   0,   0, 3},
 {  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0, -8},
 {  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0, 0},
 {  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0, 0}
};

// マトリックスLEDの目玉のアニメーションイメージ。開→閉。
static const uint8_t PROGMEM blinkImg[][8] = 
{
 {
  B00111100,   // 目を完全に開いた状態。
  B01111110,
  B11111111,
  B11111111,
  B11111111,
  B11111111,
  B01111110,
  B00111100
 },
 {
  B00000000,
  B01111110,
  B11111111,
  B11111111,
  B11111111,
  B11111111,
  B01111110,
  B00111100
 },
 { 
  B00000000,
  B00000000,
  B00111100,
  B11111111,
  B11111111,
  B11111111,
  B00111100,
  B00000000 
 },
 {
  B00000000,
  B00000000,
  B00000000,
  B00111100,
  B11111111,
  B01111110,
  B00011000,
  B00000000
 },
 {
  B00000000,         // 目を完全に閉じた状態。
  B00000000,
  B00000000,
  B00000000,
  B10000001,
  B01111110,
  B00000000,
  B00000000
 }
};

// 目をまばたくアニメパターン。全開→閉→全開の間を2段階。
static const uint8_t blinkIndex[] = {1, 2, 3, 4, 3, 2, 1};

// サーボオブジェクト。
Adafruit_16ch_Servo pwm = Adafruit_16ch_Servo();

// マトリックスLEDのオブジェクト。
Adafruit_8x8matrix matrix[2] = {
 Adafruit_8x8matrix(), Adafruit_8x8matrix() };

// タイマ割り込みのコールバック関数。
// フラグ更新を行い、実際の処理はloop()関数内でフラグをチェックして実行。
void timerCallback()
{
 timerCallbacked = true;
}

void setup()
{
 actionSeq = -1;
 keepCount = 0;

 // LED接続ポートを出力に設定。
 pinMode(led_pin, OUTPUT);

 // マトリックスLEDの開始。
 for (int i = 0; i < 2; i++)
 {
  matrix[i].begin(matrixAddress[i]);
 }

 // サーボPWMの開始と初期設定。
 pwm.begin();
 pwm.setPWMFreq(60);
 for (int i = 0; i < 16; i++)
 {
  pwm.setCenterPulseWidth(i, SERVO_CENTER_PULSE_WIDTH);
  pwm.setMinPulseWidth(i, SERVO_MIN_PULSE_WIDTH);
  pwm.setMaxPulseWidth(i, SERVO_MAX_PULSE_WIDTH);
 }
 pwm.setServoPosition(0, 0);
 pwm.setServoPosition(1, 0);
 
 // タイマ割り込みの初期化と開始。
 Timer1.initialize();
 Timer1.attachInterrupt(timerCallback, 100000);
 
}

void loop()
{
 
 if (timerCallbacked)
 {
  timerCallbacked = false;

  digitalWrite(led_pin, output);
  output = !output;

  if (keepCount == 0)
  {
   // 次のシーケンスを実行。
   actionSeq++;

   // シーケンス番号が0で停止。負で数だけステップ戻る。
   if (motion[actionSeq][16] == 0)
    while(true);
   if (motion[actionSeq][16] < 0)
   {
    actionSeq = actionSeq + motion[actionSeq][16];

    if (actionSeq < 0) actionSeq = 0;
   }

   // サーボモータ位置設定。
   for (int i = 0; i < 16; i++)
   {
    pwm.setServoPosition(i, (double)motion[actionSeq][i]);
   }
   keepCount = (int)motion[actionSeq][16];
  }
  
  keepCount--;

  
  for (int eye = 0; eye < 2; eye++)
  {
   // マトリックスLEDのクリア。
   matrix[eye].clear();

   // ウィンク動作。
   matrix[eye].drawBitmap(0, 0,
    blinkImg[
     (blinkCountdown < sizeof(blinkIndex)) ?
      blinkIndex[blinkCountdown] : 0
    ], 8, 8, LED_ON);

   // カウンタ減して、0ならランダムに次の瞬きまでのカウントをセット。
   if (--blinkCountdown == 0) blinkCountdown = random(5, 180);

   // 目玉部分の描画。
   if (--gazeCountdown <= gazeFrames)
   {
    matrix[eye].fillRect(
     newX - (dX * gazeCountdown / gazeFrames),
     newY - (dY * gazeCountdown / gazeFrames),
     2, 2, LED_OFF);

    if (gazeCountdown == 0)
    {
     eyeX = newX; eyeY = newY;
     do
     {
      newX = random(7); newY = random(7);
      dX = newX - 3; dY = newY - 3;
     }
     while ((dX * dX + dY +dY) >= 10);
     dX = newX - eyeX;
     dY = newY - eyeY;
     gazeFrames = random(3, 15);
     gazeCountdown = random(gazeFrames, 120);
    }
   }
   else
   {
    matrix[eye].fillRect(eyeX, eyeY, 2, 2, LED_OFF);
   }
  }

  // マトリックスLEDの表示更新。
  for (int i = 0; i < 2; i++)
  {
   matrix[i].writeDisplay();
  }
 }

}


Visual Microでのプロジェクトはこちらにあります。