ダンスなどのモーションデータは、PLEN:bit 本体のEEPROMに入っています。
0番のモーションを取得して、モーション再生のロジックを確認してみます。
ソースに、EEPROMからのデータ取得の関数が用意してあります。export されているので、ユーザプログラムからも呼び出し可能ですね。
// blockId=PLEN:bit_reep
// block="readEEPROM %eepAdr| byte%num"
// eepAdr.min=910 eepAdr.max=2000
// num.min=0 num.max=43
export function reep(eepAdr: number, num: number) {
let data = pins.createBuffer(2);
data[0] = eepAdr >> 8;
data[1] = eepAdr & 0xFF;
// need adr change code
pins.i2cWriteBuffer(romAdr1, data)
let value = (pins.i2cReadBuffer(romAdr1, num, false));
return value
}
読み出しのアドレスと、読み込みバイト数（最大43）を引数に入れると、データの配列を返してくれます。
モーション読み出しのアドレスは
let readAdr = 0x32 + 860 * [モーション番号]
で計算できます。モーション番号0番（左ステップ）のデータだと、0x32ですね。そこから860バイト読み込んでみます。
let listLen = 0
let readAdr = 0
input.onButtonPressed(Button.A, function () {
readAdr = 50 + 860 * 0
listLen = 43
for (let i = 0; i < 20; i++) {
let mBuf = plenbit.reep(readAdr, listLen)
for (let index = 0; index <= listLen - 1; index++) {
serial.writeNumber(readAdr + index)
serial.writeString(":")
serial.writeNumber(mBuf[index])
serial.writeLine("")
}
readAdr = readAdr + listLen
}
})
Aボタンを押した時に、43バイト * 20回 で 860バイトを 読み込んで、読み込んだものを[アドレス] : [値] でシリアルに出力するプログラムです。
ブロックだとこうなります。
「plenbit.reep」は、定義されている関数名のままで表示されました。
では、ターミナルと接続して、実行！
読めてますね。いい感じです。（ごくたまーに、取りこぼしでデータが欠けることあったので受信結果はチェックしてください）
データは、255(0xFF)が出るまでが有効値です。0番モーションの場合、アドレス265から 0xFFになりましたので、アドレス50～アドレス264までの215バイトになります。
データは文字列データとして入っているみたいです。アスキーコードに従って変換すれば、62(0x3E) は「>」、77(0x4D)は「M」という感じ。
全体だとこうですね。
読みやすく成形するとこういう感じ。
>MF0000006400000000000000000000000000000000 >MF000100c800000000fe1f00e8000000000000022b >MF00020064000000000000fe1f00000000014e01e1 >MF000300c8000000000000fdd50000000001e1ff18 >MF0004006400000000000000000000000000000000
“>” がコマンド開始。”MF”は決まりのヘッダ。
次の2バイト”00″が、モーション番号。
次の2バイト”00″が、フレーム番号？かな？2行目だと”01″、3行目だと”02″になってます。
次の4バイト”0064″が、モーションにかける時間。
残り4バイト*8が、各サーボの値となります。サーボの値は、標準値からの変化値が入ってます。
1行目は全部0なので、標準位置。
2行目は、4バイトずつ区切ってみると、
0: 0000 1: 0000 2: fe1f 3: 00e8 4: 0000 5: 0000 6: 0000 7: 022b
となります。2（左手）、３（左足首）、７（右足首）を動かしてます。0x7FFF を超える値の場合、0x10000をマイナスして、マイナス値とします。
10進にしてみると、0xFE1F = 65055、0x10000 = 65536 なので -481、0xE8 = 232 という感じです。角度は10倍で指定されているので、実際は -48度と23度になるようです。
これで取得したデータを、1行ずつ、setAngle(data, time); で実際にサーボに反映します。
export function setAngle(angle: number[], msec: number) {
let step = [0, 0, 0, 0, 0, 0, 0, 0];
msec = msec / motionSpeed;//now 15//default 10; //speedy 20 Speed Adj
for (let val = 0; val < 8; val++) {
let target = (servoSetInit[val] - angle[val]);
if (target != servoAngle[val]) { // Target != Present
step[val] = (target - servoAngle[val]) / (msec);
}
}
for (let i = 0; i <= msec; i++) {
for (let val = 0; val < 8; val++) {
servoAngle[val] += step[val];
servoWrite(val, (servoAngle[val] / 10));
}
//basic.pause(1); //Nakutei yoi
}
}
（ msec / motionSpeed ）回使って、角度を現在の角度から指定された角度に変更します。
motionSpeed は
let motionSpeed = 15;
と定義してありました。
let step = [0, 0, 0, 0, 0, 0, 0, 0];
step配列で、1回で何度動くかを各サーボごとに計算して保持しています。
let target = (servoSetInit[val] - angle[val]);
動く先の角度は、基準の角度から指定された角度を引いたものになります。イメージ的に逆な感じではありますが…。
if (target != servoAngle[val])
servoAngle配列で、現在のサーボの値を保持しています。もし変化先が同じ角度だったら、何もしない。
step[val] = (target - servoAngle[val]) / (msec);
違ってた場合は、1回で動く角度を設定します。動き先の角度から現在の角度を引いたものが、トータルで動くべき角度。これを msec 回 で動かすので、1回で動かすべき角度は上記の式の通り。角度の値を10倍で保持してるのは、ここでの割り算の誤差を減らすためですね。
servoAngle[val] += step[val]; servoWrite(val, (servoAngle[val] / 10));
msec 回ループを回し、毎回、角度に step[val]; を加算して、その値をサーボに書き込み。
モーションデータのフォーマットとそれによるサーボコントロールの仕組みが理解できました！