Funny Snowman

FunnySnowman > Geek <ギーク>なコトをやってみよっと。

ラピロのファームウェアを改造して動作コマンドを追加する。

ゴール設定(ラピロに新しい動作コマンドを追加する)

ラピロのメイン基盤に書き込んだArduinoスケッチに組み込まれているラピロの動作は[#M0]~[#M9]までの10パターン。このスケッチを改造すれば、新しい動作コマンド[#M10, #M11, #M12…]を追加できるはず。まずはプログラムのフローチャートを理解してから、その次にスケッチを改造して新しい動作コマンドを追加してみよう。

ラピロ(Rapiro)を操作する

やってみる

事前準備(Arduinoスクリプトを理解する)

ふむふむふむ。スクリプトは、Arduino言語というもので記述されている。幸い、C言語にそっくりなのでなんとか読めそう。main()は無いが、初期設定を行うsetup()とメインルーチンのloop()という名前の関数を使ってシーケンスを組み立てるようだ。あとはセンサーのデータを入力検知して、モーターやLEDなどのIO出力をON/OFFするだけなので、出来る事が少ない分ArduinoはRaspberry Piよりも簡単だ。シリアルコマンドの入力[Serial関数]やサーボモーターへの出力[Servo.h ライブラリ]が標準で用意されている。ラピロの場合は、PCやラズパイからのシリアルコマンドを入力受けして、12個のサーボモータや頭部のLEDを操作できる。さて、ラピロの基本コマンドは、motion配列に8つの連続動作を覚えさせる方式であることがわかった。デフォルトでは[#M0]~[#M9]までの10パターンが配列で登録されている。シリアルコマンドを読んで、[#M*]なら動作パターン[NextFrame()]を実行し、[#P*]なら都度12個のモーター角度を指定するポーズ[NextPose()]を実行する。忘れちゃうので、フローチャートで示すと下図の通り。

Arduino日本語リファレンス <http://www.musashinodenpa.com/arduino/ref/>

RAPIROファームウェアのフローチャート(Flowchart of RAPIRO firmware),setup(),loop(),Arduino,RAPIRO_ver0_0.ino

事前準備(サンプルスケッチを改造する)

56行目あたりから始まるmotion配列に下記11個めの動作パターンを追加する。1行が12個のサーボモータの位置を示しており、左からモーター1と2が90度、モーター3が0度の位置、最後から4つめがLEDの色255/R, 0/G, 0/Bを示し、最後が移動時間10。これを8行記述して、ラピロの一連の動作を登録することができる。この例では、初期の[#M0]と同じサーボモーターの位置を8回繰り返すので手足は動かさず、頭部のLEDだけ1秒毎に7色に変色させるレインボーイルミネーションモードを実行してくれるもの。ちなみに、サンプルスケッチの動作パターン[#M0]~[#M9]の配列を置き換えることで、サンプルとは違う動作パターンを登録できるので改造の入門としては面白いと思う。下記では[#M9]コマンドをオタ芸みたいな左右にふりふりする動作に改造してみた。なお、サーボモーターの位置を7回以下だけ動作させるには移動時間部分を 0 (ゼロ)にすればよい。

uint8_t motion[MAXMN][MAXFN][16]={
{ 中略 },
{  // 9 Right and Left 
  { 90, 90,150, 70,110,  0,100, 70, 90, 85, 90, 85,255,151,111,  3},
  {160, 20,180,110,110,  0, 70, 70, 90, 85, 90, 85,255,151,111,  3},
  { 10, 20, 40,130,110,130, 50, 70, 90, 85, 90, 85,255,151,111,  3},
  { 10, 20, 40,130,110,130, 50, 70, 90, 85, 90, 85,255,151,111,  3},
  { 20,160,180, 70,110,  0,100, 70, 90, 85, 90, 85,255,151,111, 10},
  { 20,160, 40, 70,110,130,100, 70, 90, 85, 90, 85,255,151,111,  3},
  {160,160, 40, 70,110,130,100, 70, 90, 85, 90, 85,255,151,111,  3},
  { 90, 90,150, 70,110,  0,100, 70, 90, 85, 90, 85,255,151,111,  3}
},
{  // 10 Illumination
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,255,  0,  0, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,255,  0, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,255, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,255,255,  0, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,255,255, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,255,  0,255, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,255,255,255, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,  0, 10}
}
};
  

このままだとmotion配列が溢れてエラーになるので、MAXMNを10個から11個に変更する。

#define MAXMN 11     // Max Number of Motions
  

コマンド[#M*]で指定する数字は、readOneDigit()関数が1文字だけ認識する処理になっていたので、11個めのコマンドは[#MA]とし、以降、B,C,D,とするつもり(だった)。ところが12個めをmotion配列に追加したところ、Arduinoメモリが不足して、ラピロが「ぷるぷるぷる…」って小刻みに震えだしてしまったので、この方式だと11個が限界なのかも。ということでスマートな処理はあきらめ、むりやり[#MA]を認識してmotionNumberに10をセットする処理を追加する。今回は、『if( buf == 17 ) { motionNumber = 10 }』を追加。新コマンド[#MA]は、buf = readOneDigit()で返ってくる値が17なのです。

      switch(Serial.read()) {
        case 'M':
          buf = readOneDigit();
          if(buf != ERR){
            motionNumber = buf;
            if(buf == 17) {
              motionNumber = 10;
            }
            mode = 'M';
  

readOneDigit()関数は、[#M*]の*印の部分の数字(motionNumberとなる0~9)を返す処理。シリアルコードで読み込んだ数字はAsciiコードで、48が0、49が1、…、なので48を引いた値が戻り値となる。[#MA]のAは、Asciiコードで65なので、48を引くと17となる。0~9および17だけはreturnして、それ以外をERRORとなるように以下、改造。

//Read ASCII One-digit
int readOneDigit() {
  int buf;
  while(!Serial.available()) {}
  buf = Serial.read() - 48;
  if(buf < 0 || 9 < buf){
    if(buf!=17){
      buf = ERR;
    }
  }
  return buf;
}
  

実験(書き換えたコマンドを実行してみる[動画]。)

・コマンド"#M9"を改造してあばれるモードに。
・コマンド"#MA"を追加してイルミネーションモードに。
・コマンド"#X"を追加してサイレントモードに。

改造後のサンプルコード

// by ShotaIshiwatari is licensed under the Creative Commons - Public Domain Dedication license.
// Modified by IchiroKANO; Motion #M9 changed, Additional Motion #MA and Additional #X for sleep servo motors.

#include 

#define SHIFT 7
#define R 0          // Red LED
#define G 1          // Green LED
#define B 2          // Blue LED
#define TIME 15      // Column of Time
#define MAXSN 12     // Max Number of Servos
#define MAXMN 11     // Max Number of Motions
#define MAXFN 8      // Max Number of Frames 
#define POWER 17     // Servo power supply control pin
#define ERR -1       // Error

int i = 0;
int t = 1;
Servo servo[MAXSN];
uint8_t eyes[3] = { 0, 0, 0};

// Fine angle adjustments (degrees)
int trim[MAXSN] = { -5,  // Head yaw
                    8,  // Waist yaw
                    0,  // R Sholder roll
                    -5,  // R Sholder pitch
                    0,  // R Hand grip
                    25,  // L Sholder roll
                    15,  // L Sholder pitch
                    0,  // L Hand grip
                    0,  // R Foot yaw
                    1,  // R Foot pitch
                    -7,  // L Foot yaw
                    7}; // L Foot pitch

int nowAngle[MAXSN] =        { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};  // Initialize array to 0
int targetAngle[MAXSN] =     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};  // Initialize array to 0
int deltaAngle[MAXSN] =      { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};  // Initialize array to 0
uint8_t bufferAngle[MAXSN] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};  // Initialize array to 0
uint8_t tempAngle[MAXSN] =   { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};  // Initialize array to 0

int nowBright[3] =        { 0, 0, 0};  // Initialize array to 0
int targetBright[3] =     { 0, 0, 0};  // Initialize array to 0
int deltaBright[3] =      { 0, 0, 0};  // Initialize array to 0
uint8_t bufferBright[3] = { 0, 0, 0};  // Initialize array to 0
uint8_t tempBright[3] =   { 0, 0, 0};  // Initialize array to 0

double startTime =   0;                // Motion start time(msec)
double endTime =     0;                // Motion end time(msec)
int remainingTime =  0;                // Motion remaining time(msec)
uint8_t bufferTime = 0;                // Motion buffer time (0.1sec)

uint8_t motionNumber = 0;
uint8_t frameNumber = 0;
char mode = 'M';

uint8_t motion[MAXMN][MAXFN][16]={
{  // 0 Stop
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,255, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,  0,  0}
},
{  // 1 Forward
  { 90, 90,  0, 90, 90,180, 90, 90, 80,110, 80,120,  0,  0,  0,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 70, 90, 70, 90,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 70, 70, 70, 80,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90,100, 60, 92, 70,  0,  0,  0,  3},
  { 90, 90,  0, 90, 90,180, 90, 90,110, 90,102, 90,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90,110,100,110,110,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0}
},
{  // 2 Back
  { 90, 90,  0, 90, 90,180, 90, 90,100,110,100,120,  0,  0,  0,  3},
  { 90, 90,  0, 90, 90,180, 90, 90,110, 90,110, 90,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90,110, 70,110, 80,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90,100, 60,100, 70,  0,  0,  0,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 70, 90, 70, 90,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 70,100, 70,110,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0}
},
{  // 3 Right
  { 90, 90,  0, 90, 90,180, 90, 90, 95,110, 85,120,  0,  0,  0,  3},
  { 90, 90,  0, 90, 90,180, 90, 90,100, 90, 80, 90,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90,100, 70, 80, 80,  0,  0,  0,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 85, 60, 95, 70,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 80, 90,100, 90,  0,  0,  0,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 80,100,100,110,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0}
},
{  // 4 Left
  { 90, 90,  0, 90, 90,180, 90, 90, 95, 60, 85, 70,  0,  0,  0,  3},
  { 90, 90,  0, 90, 90,180, 90, 90,100, 90, 80, 90,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90,100,100, 80,110,  0,  0,  0,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 85,110, 95,120,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 80, 90,100, 90,  0,  0,  0,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 80, 70,100, 80,  0,  0,255,  3},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0}
},
{  // 5 Green
  { 90, 90,120, 90, 90, 60, 90, 90, 90, 90, 90, 90,  0,  0,  0, 10},
  {100, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90,  0,255,  0,  5},
  { 90, 90,120, 90, 90, 60, 90, 90, 90, 90, 90, 90,  0,255,  0,  5},
  { 80, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90,  0,  0,  0,  5},
  { 90, 90,120, 90, 90, 60, 90, 90, 90, 90, 90, 90,  0,255,  0, 10},
  { 90, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90,  0,255,  0,  5},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0}
},
{  // 6 Yellow
  { 90,120,120,130, 90,180, 90, 90, 90, 90, 90, 90,255,255,  0,  7},
  { 90,120,120, 90, 90,180, 90, 90, 90, 90, 90, 90,255,255,  0,  7},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0}
},
{  // 7 Blue
  { 90, 90,120,130, 70, 60, 50,110, 90, 90, 90, 90,  0,  0,255, 10},
  { 90, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90,  0,  0,255,  5},
  { 90, 90,120,130, 70, 60, 50,110, 90, 90, 90, 90,  0,  0,255,  5},
  { 90, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90,  0,  0,255,  5},
  { 90, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90,  0,  0,255, 15},
  { 90, 90, 90,130,110, 90, 50, 70, 90, 90, 90, 90,  0,  0,255,  3},
  { 90, 90,120,130,110, 60, 50, 70, 90, 90, 90, 90,  0,  0,255,  3},
  { 90, 90, 90,130,110, 90, 50, 70, 90, 90, 90, 90,  0,  0,255,  3}
},
{  // 8 Red
  { 90, 60,  0, 90, 90, 60, 50, 90, 90, 90, 90, 90,255,  0,  0,  7},
  { 90, 60,  0, 90, 90, 60, 90, 90, 90, 90, 90, 90,255,  0,  0,  7},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0},
  { 90, 90,  0, 90, 90,180, 90, 90, 90, 90, 90, 90,  0,  0,  0,  0}
},
{  // 9 Right and Left ; #M9 Kano modified
  { 90, 90,150, 70,110,  0,100, 70, 90, 85, 90, 85,255,151,111,  3},
  {160, 20,180,110,110,  0, 70, 70, 90, 85, 90, 85,255,151,111,  3},
  { 10, 20, 40,130,110,130, 50, 70, 90, 85, 90, 85,255,151,111,  3},
  { 10, 20, 40,130,110,130, 50, 70, 90, 85, 90, 85,255,151,111,  3},
  { 20,160,180, 70,110,  0,100, 70, 90, 85, 90, 85,255,151,111, 10},
  { 20,160, 40, 70,110,130,100, 70, 90, 85, 90, 85,255,151,111,  3},
  {160,160, 40, 70,110,130,100, 70, 90, 85, 90, 85,255,151,111,  3},
  { 90, 90,150, 70,110,  0,100, 70, 90, 85, 90, 85,255,151,111,  3}
},
{  // 10 Illumination ; #MA Kano modified
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,255,  0,  0, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,255,  0, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,255, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,255,255,  0, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,255,255, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,255,  0,255, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,255,255,255, 10},
  { 90, 90,  0,130, 90,180, 50, 90, 90, 90, 90, 90,  0,  0,  0, 10}
}
};

void setup()  {
  servo[0].attach(10);   // Head yaw
  servo[1].attach(11);   // Waist yaw
  servo[2].attach(9);    // R Sholder roll
  servo[3].attach(8);    // R Sholder pitch
  servo[4].attach(7);    // R Hand grip
  servo[5].attach(12);   // L Sholder roll
  servo[6].attach(13);   // L Sholder pitch
  servo[7].attach(14);   // L Hand grip
  servo[8].attach(4);    // R Foot yaw
  servo[9].attach(2);    // R Foot pitch
  servo[10].attach(15);  // L Foot yaw
  servo[11].attach(16);  // L Foot pitch
  eyes[R] = 6;           // Red LED of eyes
  eyes[G] = 5;           // Green LED of eyes
  eyes[B] = 3;           // Blue LED of eyes
  
  for( i = 0; i < MAXSN; i++) {
    targetAngle[i] = motion[0][0][i] << SHIFT;
    nowAngle[i] = targetAngle[i];
    servo[i].write((nowAngle[i] >> SHIFT) + trim[i]);
  }
  for(i = 0; i < 3; i++) {
    targetBright[i] = 0 << SHIFT;
    nowBright[i] = targetBright[i];
    analogWrite(eyes[i], nowBright[i] >> SHIFT);
  }
  
  Serial.begin(57600);
  
  delay(500);
  
  pinMode(POWER, OUTPUT);
  digitalWrite(POWER, HIGH);
} 

void loop()  {
  int buf = ERR;
  if(Serial.available()) {
    if(Serial.read() == '#') {
      while(!Serial.available()){}
      switch(Serial.read()) {
        case 'M':
          buf = readOneDigit();
          if(buf != ERR){
            motionNumber = buf;
            if(buf == 17) {
              motionNumber = 10;
            }
            mode = 'M';
            digitalWrite(POWER, HIGH);
            Serial.print("#M");
            Serial.print(motionNumber);
          } else {
            Serial.print("#EM");
          }
        break;
        case 'P':
          buf = getPose();
          if(buf != ERR) {
            mode = 'P';
            digitalWrite(POWER, HIGH);
            Serial.print("#PT");
            printThreeDigit(buf);
          } else {
            Serial.print("#EP");
          }
        break;
        case 'Q':
          Serial.print("#Q");
          if(mode == 'M') {
            Serial.print("M");
            Serial.print(motionNumber);
            Serial.print("T");
            buf = (endTime-millis()) /100;
            if(buf < 0) { buf = 0;}
            printThreeDigit(buf);
          }
          if(mode == 'P') {
            Serial.print("PT");
            buf = (endTime-millis()) /100;
            if(buf < 0) { buf = 0;}
            printThreeDigit(buf);
          }
        break;
        case 'C':
          Serial.print("#C");
          if(bufferTime > 0) {
            Serial.print("F");
          } else {
            Serial.print("0");
          }
        break;
        case 'X':
          Serial.print("#X");
          digitalWrite(POWER, LOW);
        break;
        default:
          Serial.print("#E");
        break;
      }
    }
  }
  if(endTime > millis()) {
    remainingTime = (endTime - millis()) / 10;
    for( i = 0; i < MAXSN; i++) {
      nowAngle[i] = targetAngle[i] - (deltaAngle[i] * remainingTime);
      servo[i].write((nowAngle[i] >> SHIFT) + trim[i]);
    }
    for( i = 0; i < 3; i++) {
      nowBright[i] = targetBright[i] - (deltaBright[i] * remainingTime);
      analogWrite(eyes[i], nowBright[i] >> SHIFT);
    }
  } else if(mode == 'M') {
    nextFrame();
  } else if(mode == 'P') {
    if(bufferTime > 0){
      nextPose();
    } else if(endTime + 500 < millis()){
      //digitalWrite(POWER, LOW);
    }
  }
}

//Motion Play
void nextFrame() {
  frameNumber++;
  if(frameNumber >= MAXFN) {
    frameNumber = 0;
  }
  for(i = 0; i < MAXSN; i++) {
    bufferAngle[i] = motion[motionNumber][frameNumber][i];
  }
  for( i = 0; i < 3; i++) {
    bufferBright[i] = motion[motionNumber][frameNumber][MAXSN+i];
  }
  bufferTime = motion[motionNumber][frameNumber][TIME];
  
  nextPose();
}

//Make a pose
int nextPose() {
  if(bufferTime > 0) {
    for(i = 0; i < MAXSN; i++) {
      targetAngle[i] = bufferAngle[i] << SHIFT;
      deltaAngle[i] = ((bufferAngle[i] << SHIFT) - nowAngle[i]) / (bufferTime * 10);
    }
    for( i = 0; i < 3; i++) {
      targetBright[i] = bufferBright[i] << SHIFT;
      deltaBright[i] = ((bufferBright[i] << SHIFT) - nowBright[i]) / (bufferTime * 10);
    }
  } else {
    for(i = 0; i < MAXSN; i++) {
      deltaAngle[i] = 0;
    }
    for(i = 0; i < 3; i++) {
      deltaBright[i] = 0;
    }
  }
  startTime = millis();
  endTime = startTime + (bufferTime * 100);
  bufferTime = 0;
}

//get buffer values of the next pose from serial data
int getPose() {
  int buf = 0;
  int value = 0;
  int maximum = 255;
  boolean readPose = true;
  if(bufferTime == 0) {
    //Initialize array to target angle
    for(i = 0; i < MAXSN; i++) {
      tempAngle[i] = bufferAngle[i];
    }
    for( i = 0; i < 3; i++) {
      tempBright[i] = bufferBright[i];
    }
  } else {
    buf = ERR;
    readPose = false;
  }
  //Read data
  while(readPose) {
    while(!Serial.available()) {}
    switch(Serial.read()) {
      case 'S':
        buf = readOneDigit();
        if(buf != ERR) {
          value = buf *10;
          buf = readOneDigit();
          if(buf != ERR) {
            value += buf;
            if(0 <= value && value < MAXSN) {
              while(!Serial.available()) {}
              if(Serial.read() == 'A') {
                maximum = 180;
                buf = readThreeDigit(maximum);
                if(buf != ERR) {
                  tempAngle[value] = buf;
                } else {
                  readPose = false;
                }
              } else {
                buf = ERR;
                readPose = false;
              }
            } else {
              buf = ERR;
              readPose = false;
            }
          }
        }
        break;
      case 'R':
        maximum = 255;
        buf = readThreeDigit(maximum);
        if(buf != ERR) {
          tempBright[R] = buf;
        } else {
          readPose = false;
        }
        break;
      case 'G':
        maximum = 255;
        buf = readThreeDigit(maximum);
        if(buf != ERR) {
          tempBright[G] = buf;
        } else {
          readPose = false;
        }
        break;
      case 'B':
        maximum = 255;
        buf = readThreeDigit(maximum);
        if(buf != ERR) {
          tempBright[B] = buf;
        } else {
          readPose = false;
        }
        break;
      case 'T':
        maximum = 255;
        buf = readThreeDigit(maximum);
        if(buf > 0) {
          bufferTime = buf;
          for(i = 0; i < MAXSN; i++){
            bufferAngle[i] = tempAngle[i];
          }
          for( i = 0; i < 3; i++) {
            bufferBright[i] = tempBright[i];
          }
        }
        readPose = false;
        break;
      default:
        buf = ERR;
        readPose = false;
        break;
    }
  }
  return buf;
}

int printThreeDigit(int buf) {
  String s = String(buf);
  if(s.length() == 2){
    Serial.print("0");
  } else if (s.length() == 1) {
    Serial.print("00");
  }
  Serial.print(s);
}

int digit;
//Read ASCII Three-digit
int readThreeDigit(int maximum) {
  int buf;
  buf = readOneDigit();
  if(buf != ERR) {
    digit = buf * 100;
    buf = readOneDigit();
    if(buf != ERR) {
      digit += buf * 10;
      buf = readOneDigit();
      if(buf != ERR) {
        digit += buf;
        if(digit <= maximum) {
          buf = digit;
        } else {
          buf = ERR;
        }
      }
    }
  }
  return buf;
}

//Read ASCII One-digit
int readOneDigit() {
  int buf;
  while(!Serial.available()) {}
  buf = Serial.read() - 48;
  if(buf < 0 || 9 < buf){
    if(buf!=17){
      buf = ERR;
    }
  }
  return buf;
}
  

まとめ

Arduino言語で書かれたラピロのファームウェアを改造すると、自分オリジナルの動作を記憶させることができる。サンプルのファームウェアは10個の動作パターンが配列で記述されており、ひとつの動作について8回分のモーターポジションが書き込まれている。これらの配列データを変更するとオリジナル動作を実行できるわけだ。なお、動作パターンを複数追加したかったのだが、Arduinoのメモリが不足してブルブルと暴走してしまうようなので、今のところ追加できるパターンは1個(つまり合計11個の動作パターン)。あと、できれば標準サンプルで実装しておいた方が良いと思われるサイレントモードも追加して「じーーーー」というモーターの動作音を止めるコマンドも追加した。Arduino言語も覚えて、なかなか楽しくなってきました。

▲上に戻る