ラブコフをバイスタンプでコントロールする
今回はtoio(トイオ) を使って、バイスタンプでコントロールできる『動くラブコフ』を作ってみました。ラブ～
製作経緯
制作のきっかけは、縁あってソニー・インタラクティブエンテインメント(以下SIE)様からtoioを頂いたことでした。
まずtoioとは何ぞやというところですが、これはSIEで開発された『遊びのツール』です。プログラミング教育などとセットで語られることが多かったり、専用ソフトが発売されていたりして、『子供の教育用ツール』として認知されている方もおられるかもしれませんが、全然その枠に収まるようなモノではありません。モノづくり・Makerの観点で見ると、ArduinoやRaspberry Piのような、自分で自由自在にGPIOにモジュールを追加していくような汎用性はないのですが、各種センサ、通信モジュール、モーター、バッテリーが非常にコンパクトな筐体に収まっており、またその制御仕様もかなり詳細に公開されているので、何か『動くモノ』を作りたいときには、非常に有力な選択肢になり得ます。
SIE様が私にこれを下さったのは、「是非お子様と、toioのプログラミング教育用ソフトで遊んでみてください」ということだったので、それはもちろんそうさせて頂くとして、それとは別に、せっかくなので自分の創作にも何か活かせないかなと考えてみました。
そこで白羽の矢が立ったのが、『仮面ライダーリバイス』に登場するラブコフです。ラブコフは（個人的に）仮面ライダーシリーズの中でも屈指のゆるキャラであり、『リバイス』の中でも特に大好きなキャラクターです。
toioの活かし方を考えたとき、ラブコフを題材にすると何かと上手くハマるんじゃないかと思いました。というのも、ラブコフは蛇なので、toioを中に仕込んで地を滑るように動いたとしても、そこまで違和感は出ないものと思われます（実際、劇中でも動いているときはそんな感じ）。また、自分がここまで培ってきたバイスタンプの扱いに関する技術を組み合わせることで、例えばバイスに向かって暴言を吐くとか、ただ動くだけじゃなくて、より劇中のラブコフの印象に近いオモチャを作ることができるのでは、とも考えました。
ということで、なんかいける気がしましたので、toioを使った『動くラブコフ』の制作にとりかかることにしました。
特徴
特徴は至ってシンプルで、タイトルどおり、バイスタンプでラブコフをコントロールできます。
アダプターを取り付けたコブラバイスタンプを傾けると、傾きに応じてラブコフが動き回ります。
バイスタンプの天面ボタンを押すと、こちらを向いて返事をしてくれます。ただ、この機能については使える状況が限定されます。これについては、後段の『ソフトウェア解説』のところで説明します。
なお、コブラバイスタンプ以外のバイスタンプにアダプターをセットしてラブコフをコントロールしようとすると、言うことを聞いてくれません。クズ呼ばわりされて終わります。
ハードウェア解説
ハードの方は大きく分けると『ラブコフ本体』と『アダプター』の２つの話があるのですが、まずは『ラブコフ本体』の方からご説明致します。
少しずつ3Dモデリングの勉強をしているとはいえ、今は流石にラブコフの造形を一から作れるほどのスキルは持ち合わせていません。
ということで、ベースには食玩『装動』シリーズのラブコフを使用することにしました。ちなみにこの装動ラブコフ、近所では全く手に入らず、泣く泣くフリマアプリで購入しました。食玩ってこんなに入手難度高いもんだっけ..?
で、早速組み立てたものがこちらです。これにtoioが収まってくれれば何の問題もないのですが、
残念ながら、ギリギリ収まってくれそうにありません。無理矢理中をくり抜いて収めると、体の各所から角が飛び出てしまいそうです。
かと言ってラブコフをtoioの上に乗せてしまうと、ラブコフで頑張る意味があんまりありません。
バイスでも誰でもOK。
以上を踏まえた結果、
胴体パーツを3Dプリンタで新規に作成しました。これをラブコフのパーツの間に挟み込んで、toioの収納スペースを確保しようという作戦です。ラブコフは蛇ですので多少胴体が伸びても問題ないでしょうし、またこの方式なら、貴重なラブコフのフィギュアを破壊せずに済み、いつでも元に戻せます。
自分としてはここまで曲面の多い部品を3Dプリンタで作成するのは初めてでしたので結構悪戦苦闘しましたが、twitter上で先人達に色々テクニック（←パーツ分割やら造形方向変更やら）を教えてもらって、何とか形にできました。
あと、今回はフィギュアの一部になるパーツということで、自分にしては珍しく、かなり真面目にヤスリがけしました。
使用したフィラメントは、家の中に長いこと眠っていたどこぞかのPLAフィラメントですが、ヤスリはダイソーで売っていた耐水ペーパーのセットです。これの#240から#1500までを順にかけていくと、触った感じはほぼツルツル、積層痕も個人的には気にならないレベルになりました。
1枚目がヤスリがけ前、2枚目がヤスリがけ後ですが、多分写真だとよくわかりませんね。
最後に塗装して完成です。微妙にラブコフ本体の色と違っていたり、マスキングが甘くてうっかり塗装がはみ出てしまったところもありますが、許容範囲ということで。ちなみに以下の色を使用しました。
専用設計なので、もちろんtoioはジャストフィット。
後はこのコアスプレンダーにレッグフライヤーとチェストフライヤーを取り付けて、インパルス『動くラブコフ』の完成です。ちなみに『ガンダムSEED』のメイン３キャラで一番好きなのはシン・アスカです。
ご覧の通り、ベルトのおかげで『腰から伸びてる感』が非常に強くなってしまって珍妙な体型になっでしまいましたが、そこは目を瞑って頂けますと幸いです。
奇しくも本編で『さくらちゃんは大食い』ということが最近わかったので、その栄養がラブコフにいっている、という超解釈でもまあ良いでしょう。
さて、ラブコフ側は準備ができたので、続いてラブコフをコントロールする側の準備です。
動かすだけなら、toio本体にプリセットされている『フリーそうこう』機能を使えば、お手軽な上にかなり軽快にラブコフをコントロールできるのですが、もう少し『リバイス』らしくしたかったので、バイスタンプで操作できるようにしました（※ そんな劇中設定はもちろんなく、あくまで雰囲気だけの問題です）。
バイスタンプに上記のようにアダプターをセットする形で実現していますが、実はこのアダプターは前作『バイスタンプの音声を差し替えて色々遊ぶ』のときに作成したものと全く同じです。というより、元々今回のラブコフのコントロールのために作成したものを一部機能だけ先出する形で公開していたのが前作、という位置付けになります。
ik
中にはM5StickC Plusを丸ごと組み込んでいて、その中に搭載されている6軸センサで傾きを検知、傾きに応じた制御信号をBLE(Bluetooth Low Energy)通信モジュールでtoioに送っています。さらに、赤外線LEDとMP3モジュールを追加することで、バイスタンプの種類やボタン操作に応じたtoio制御、音声再生をできるようにしました。配線、具材については、上記記事内の『ハードウェア解説』をご参照頂けますと幸いです。
ソフトウェア解説
今回のキモは、toioとM5StickC Plusをどう繋ぐかです。冒頭述べた通り、toioは制御仕様がかなり詳細に公開されているので、頑張れば通信部分を自力で実装することも可能なのですが、大変ありがたいことに、既にM5Stack用のtoioライブラリを作成して公開してくださっている方がおられました。
ほぼほぼこちらを使わせて頂いたのですが、最終更新日が2020/7ということで、最新の仕様とは少し異なっていたり実装がないものがあったりしたので、それについてはソースコードを参考にさせて頂きながら自分で改修しました。この手を加えた版のライブラリについては、今回は公開は控えておこうかなと思っています。おそらくtoioを自力で制御しようと思う方なら難なく対応できるレベルだと思いますので、申し訳ありませんが自力で頑張ってください。
今回のアダプターはまず最初にバイスタンプを認識していますが、そもそもバイスタンプの認識に必要な赤外線通信の仕様理解については、こちらで詳しめに解説しているので、必要な方はそちらをご覧頂けますと幸いです。アダプターをセットすると、最初にバイスタンプが送信してくるIDをチェックし、コブラのID(=40)なら後の制御を許可し、それ以外のIDならラブコフに「クズ!」と言わせて終わるようにしています。制御を許可するIDは自由に設定できるので、お好みで『スパイダー』を許可するのも良いかもしれません（※ヒロミさん想定）。
アダプターをセットするとラブコフが中央に移動してこちらを向いたり、また天面ボタンを押すと、ラブコフがどの方向を向いていてもこちらを向いて愛嬌を振り撒いてくれるのですが、実はこの動作を普通のロボットブログラミングで実装しようと思うと、メチャメチャ難しい内容だったりします。例えば、「こっちを向く」という行為を実現しようとしたとき、ロボットが「こっち」に対して左90°を向いていたら右に90°回らないといけないし、「こっち」に対して右90°を向いていたら左に90°回らないといけない。
つまり、ロボットが「自分は何を基準にどこを向いているのか」がわからないと、「こっちを向く」というアクションの内容を決めることができないことになるのですが、何とtoioにはそれを可能にする『絶対位置検出』という強力な機能があります。
toioは上記のような専用マット上であれば、自分はどの位置にいてどの方向を向いているのかを検出することができ、さらに「この位置に移動してこの方向を向きなさい」という命令が可能な、『目標指定付きモーター制御』という、これまた強力な機能を持っています。これらの機能のおかげで、本来は難しいはずの「こっちを向く」という動作をかなり簡単に実装することができます。つまり、『絶対位置検出』で「今自分がどこにいるか」を把握し、さらに『目標指定付きモーター制御』で「今いる位置でカメラの方を向いて」という命令を出している、というわけです（※さすがにカメラの位置は固定）。
ちなみに、そもそも天面ボタンを押すとなぜ通信コマンドを送れるのか、については、前作『バイスタンプの音声を差し替えて色々遊ぶ』の記事内容を見ると何となくわかると思いますので、必要であればそちらも併せてご参照ください。
最後におまけで、ラブコフをが何かにぶつかると「イテッ！」と言いますが、これはtoioの『衝突検出』という機能を使って実現しています。音声は主に『DXリベラドライバー』から自分で録音しました（「クズ!」だけは劇中音声の加工）。
まとめ
以上、toioを使った『動くラブコフ』作成、およびバイスタンプによるコントロールでした。今回の作品はtoioの『絶対位置検出』『目標指定付きモーター制御』『衝突検出』といった機能を使って実現しましたが、他にも『姿勢検出』『シェイク検出』等々、機能はまだまだたくさんあり、正直言ってまだtoioのポテンシャルの半分も引き出せていないと思います。アイデア次第でもっともっと面白いことができる、非常に優れたツールだと思います。
ちなみに、5月発売予定の『Chibiぬいぐるみ ラブコフ』は予約済みですので、届き次第、toioをうまく載せこめそうか、トライしてみようと思います。うまくいきそうなら、動いている姿だけですが動画に撮って公開しようかなと思っています。やっぱり、モフモフで動いているラブコフの姿が見たいですので。
ソースコード
M5StickC Plus向けのメインプログラム（.ino）を以下で公開しますが、実際にはこれを動かすには私がカスタムしたtoioライブラリが必要になります。このライブラリについては、先にも述べましたが非公開とさせて頂きますので、ご了承ください。
////////// 基本設定 /////////////////////////////////////////////////////////////
#include <M5StickCPlus.h>
#define PIN_LED 10
#define PIN_MP3_RX 25
#define PIN_MP3_TX 26
#define IR_RX_TX_FOR_STAMP_PIN 33
#define STAMP_ID_REX 1
#define STAMP_ID_COBRA 40
#define STAMP_ID_MAX 100
uint8_t stamp_id = 0;
#define STATE_INIT 0
#define STATE_SET 1
#define STATE_TOP 2
#define STATE_STAMP 3
uint8_t state = STATE_INIT;
#define RESET_MS 200
unsigned long reset_start_ms = 0;
////////// IR信号送受信処理 ////////////////////////////////////////////////////////////
#define LEN_IR_DATA 8
#define IR_THRESHOLD_FOR_STAMP 500
#define READ_INTERVAL_MS 2050
#define SEND_IR_DELAY_MS 50
// デバッグ用プリント
void print_data(uint8_t data){
for(uint8_t i=0;i<LEN_IR_DATA;i++){
Serial.print(bitRead(data, LEN_IR_DATA-1-i)); // 8bitの左端から表示
}
Serial.print(" / ");
Serial.println(data);
}
void send_start(uint8_t pin){
digitalWrite(pin, HIGH);
delayMicroseconds(6000);
digitalWrite(pin, LOW);
delayMicroseconds(500);
}
void send_bit_0(uint8_t pin){
digitalWrite(pin, HIGH);
delayMicroseconds(1500);
digitalWrite(pin, LOW);
delayMicroseconds(500);
}
void send_bit_1(uint8_t pin){
digitalWrite(pin, HIGH);
delayMicroseconds(500);
digitalWrite(pin, LOW);
delayMicroseconds(1500);
}
void send_ir(uint8_t pin, uint8_t data){
pinMode(pin, OUTPUT);
send_start(pin);
for(uint8_t i=0; i<LEN_IR_DATA; i++){
// dataを左端から順に送信する
if(bitRead(data, LEN_IR_DATA-1-i) == 0){
send_bit_0(pin);
}else{
send_bit_1(pin);
}
}
}
////////// 音声処理 /////////////////////////////////////////////////////////////
#include <DFPlayerMini_Fast.h>
#define SOUND_TOP 1
#define SOUND_STAMP 2
#define VOICE_WELCOME 3
#define VOICE_HELLO 4
#define VOICE_ITE 5
#define VOICE_KUZU 6
HardwareSerial hs_mp3_player(1);
DFPlayerMini_Fast mp3_player;
#define SOUND_VOLUME_DEFAULT 23 // 0〜30 20
void play_sound(uint8_t sound_num){
mp3_player.playFromMP3Folder(sound_num);
Serial.print(F("Play sound: "));
Serial.println(sound_num);
}
void pause_sound(){
mp3_player.pause();
}
////////// TOIO制御 /////////////////////////////////////////////////////////
#include "ToioCustom.h"
// ライブラリは以下のものをベースに書き換えたものを使用
// https://github.com/futomi/M5StackToio
// TOIO簡易プレイマットの座標情報
#define COORDINATE_X_LIMIT_LEFT 98
#define COORDINATE_X_ORIGIN 250
#define COORDINATE_X_LIMIT_RIGHT 402
#define COORDINATE_Y_LIMIT_TOP 142
#define COORDINATE_Y_ORIGIN 250
#define COORDINATE_Y_LIMIT_BOTTOM 358
#define ANGLE_FRONT 90
#define MOVE_TYPE_WITH_ROLL 0
#define MOVE_TYPE_WITH_ROLL_WITHOUT_BACK 1
#define MOVE_TYPE_AFTER_ROLL 2
#define ACC_TYPE_CONST 0
#define ACC_TYPE_INC 1
#define ACC_TYPE_DEC 2
#define ACC_TYPE_INC_AND_DEC 3
#define MOTOR_L_FORWARD true
#define MOTOR_L_BACKWARD false
#define MOTOR_R_FORWARD true
#define MOTOR_R_BACKWARD false
#define MOTOR_SPEED_VERY_SLOW 10
#define MOTOR_SPEED_SLOW 20
#define TOIO_THROTTLE_MAX 100
#define TOIO_STEERING_MAX 50
#define MOTION_STATE_INIT 0
#define MOTION_STATE_COBRA_GO_TO_CENTER_WAIT 1
#define MOTION_STATE_COBRA_GO_TO_CENTER 2
#define MOTION_STATE_COBRA_TURN_FRONT_WAIT 3
#define MOTION_STATE_COBRA_TURN_FRONT 4
#define MOTION_STATE_COBRA_HELLO_1 5
#define MOTION_STATE_COBRA_HELLO_2 6
#define MOTION_STATE_COBRA_HELLO_3 7
#define MOTION_STATE_COBRA_HELLO_4 8
#define MOTION_STATE_COBRA_HELLO_5 9
#define MOTION_STATE_OTHER_GO_TO_CENTER_WAIT 1
#define MOTION_STATE_OTHER_GO_TO_CENTER 2
#define MOTION_STATE_OTHER_TURN_FRONT_WAIT 3
#define MOTION_STATE_OTHER_TURN_FRONT 4
#define MOTION_STATE_OTHER_TURN_BACK 5
#define MOTION_STATE_OTHER_TURN_BACK_END 6
unsigned long wait_start_time_ms = 0;
unsigned long wait_keep_time_ms = 0;
ToioCustom toio;
ToioCoreCustom* toiocore;
float accX = 0; // 自分の基本姿勢だと0。 奥 or 手前に倒しても、値が変わらない。左に倒すと1、右に倒すと-1に近づく。
float accY = 0; // 自分の基本姿勢だと−1[g]。奥 or 手前 or 左 or 右に倒していくと、0に近づく。
float accZ = 0; // 自分の基本姿勢だと0。奥に倒すと-1、手前に倒すと1に近づく。左 or 右に倒しても、値が変わらない。
uint16_t current_x = 0;
uint16_t current_y = 0;
uint8_t motion_state = MOTION_STATE_INIT;
ToioCoreSensorData sensor_data = {0x0000, 0x0000, 0x0000};
void control_toio(){
M5.Imu.getAccelData(&accX,&accY,&accZ);
// 正位置に近いときは、止めるようにする
if(accY < -0.95){
accX = 0.0;
accZ = 0.0;
}
// 重力加速度以上にはならないようにする
if(accX < -1){
accX = -1.0;
}else if(accX > 1){
accX = 1.0;
}
if(accZ < -1){
accZ = -1.0;
}else if(accZ > 1){
accZ = 1.0;
}
int8_t throttle = (int8_t)(TOIO_THROTTLE_MAX * (-accZ));
int8_t steering = (int8_t)(TOIO_STEERING_MAX * (-accX));
toiocore->drive(throttle, steering);
}
////////// メイン処理 ////////////////////////////////////////////////////////////
void setup(){
M5.begin();
Serial.begin(115200);
pinMode(PIN_LED, OUTPUT);
pinMode(IR_RX_TX_FOR_STAMP_PIN, INPUT);
// G25と共用で使用しないG36をフローティングにする
gpio_pulldown_dis(GPIO_NUM_36);
gpio_pullup_dis(GPIO_NUM_36);
// 液晶画面は省電力化のためOFFにする
M5.Axp.ScreenBreath(0); // Max 12
// 6軸センサ初期化
M5.Imu.Init();
//---------- MP3プレイヤー ----------
hs_mp3_player.begin(9600, SERIAL_8N1, PIN_MP3_RX, PIN_MP3_TX);
if(!mp3_player.begin(hs_mp3_player)) {
Serial.println(F("Unable to begin music_player:"));
Serial.println(F("1.Please recheck the connection!"));
Serial.println(F("2.Please insert the SD card!"));
while(true);
}
Serial.println(F("mp3_player online."));
delay(100);
mp3_player.volume(SOUND_VOLUME_DEFAULT);
// 動作状態確認用LEDオン
digitalWrite(PIN_LED, LOW); // LED点灯
// TOIOスキャン開始(5秒間)
// ここで指定した時間必ず待つことになるので注意。
// ここの時間を短くして、M5StickCの電源ONの前にtoioの電源を先にONにしておけば、速やかに繋げられる。
Serial.println("Scanning your toio core...");
std::vector<ToioCoreCustom*> toiocore_list = toio.scan(5);
// 見つからなければ終了
size_t n = toiocore_list.size();
if (n == 0) {
Serial.println("No toio Core Cube was found. Turn on your Toio Core Cube, then press the reset button of your Toio Core Cube.");
return;
}
// 最初に見つかった Toio Core Cube の ToioCore オブジェクト
toiocore = toiocore_list.at(0);
Serial.println("Your toio core was found.");
delay(2000);
// BLE 接続開始
Serial.println("Connecting...");
if (!toiocore->connect()) {
Serial.println("Failed to establish a BLE connection.");
return;
}
Serial.println("Connected.");
// LED点灯
toiocore->turnOnLed(0x00, 0x00, 0x80);
// 衝突検出の閾値変更（初期値7、最大10)
toiocore->setConfClash(5);
// 切断されたときにLEDを消灯させるためのコールバックをセット
toiocore->onConnection([](bool state) {
if(state == false){
toiocore->turnOffLed();
}
});
// 目標指定付きモーター制御イベントのコールバックをセット
toiocore->onGoToTarget([](ToioCoreGoToTargetData gototarget){
;
});
// モーション検出イベントのコールバックをセット
toiocore->onMotion([](ToioCoreMotionData motion) {
if(motion.clash){
play_sound(VOICE_ITE);
}
});
}
void loop(){
// 基本的にバイスタンプからの信号を受信モードで待つ
pinMode(IR_RX_TX_FOR_STAMP_PIN, INPUT);
reset_start_ms = millis();
while(analogRead(IR_RX_TX_FOR_STAMP_PIN) < IR_THRESHOLD_FOR_STAMP){
// 受信待ち待機
if(state != STATE_INIT && millis() - reset_start_ms > RESET_MS){
state = STATE_INIT;
}
}
// 閾値を超えた(ONになった)ので、5500μs待つ
delayMicroseconds(5500);
// 送受信データは丁度1byteなので、1byteの変数に受信結果を収める
uint8_t recv_data = 0;
if(analogRead(IR_RX_TX_FOR_STAMP_PIN) >= IR_THRESHOLD_FOR_STAMP){
// ここでも閾値を超えていれば、スタンプから信号が来たとみなし、およそ2000μsごとの読み取りを開始する
for(uint8_t i=0; i<LEN_IR_DATA; i++){
delayMicroseconds(READ_INTERVAL_MS);
if(analogRead(IR_RX_TX_FOR_STAMP_PIN) < IR_THRESHOLD_FOR_STAMP){
// ON:1500μs, OFF: 500μs ... 0
// ON: 500μs, OFF:1500μs ... 1
// なので、2000μ秒ごとのサイクルで1000μsの位置がONなら0、OFFなら1となる
recv_data = recv_data | (1 << (LEN_IR_DATA-1-i));
}
}
// スタンプから受け取った信号を表示する
Serial.print("Stamp -> Adapter: ");
print_data(recv_data);
uint8_t send_data = 0;
if(recv_data < STAMP_ID_MAX){// 100未満は全てバイスタンプのIDとみなす
if(state == STATE_SET){
if(stamp_id == STAMP_ID_COBRA){
wait_keep_time_ms = 1500;
motion_state = MOTION_STATE_COBRA_GO_TO_CENTER_WAIT;
}else{
wait_keep_time_ms = 1500;
motion_state = MOTION_STATE_OTHER_GO_TO_CENTER_WAIT;
}
wait_start_time_ms = millis();
send_data = 140; // シーケンス開始要求
}else{
send_data = 101; // ドライバーセット待機
stamp_id = recv_data;
state = STATE_SET;
}
}else{
switch(recv_data){
case 129: // バイスタンプ発光終了通知
send_data = 130; // バイスタンプ発光終了応答
break;
case 131: // ドライバーセット後の押印全押し操作要求
case 132: // ドライバーセット後の押印半押し操作要求
play_sound(SOUND_STAMP);
if(stamp_id == STAMP_ID_COBRA){
wait_keep_time_ms = 1500;
motion_state = MOTION_STATE_COBRA_TURN_FRONT_WAIT;
}else{
wait_keep_time_ms = 1500;
motion_state = MOTION_STATE_OTHER_TURN_FRONT_WAIT;
}
wait_start_time_ms = millis();
send_data = 174; // バイスタンプ点滅発光(遅)要求
break;
case 133: // ドライバーセット後の天面ボタン操作要求
play_sound(SOUND_TOP);
if(stamp_id == STAMP_ID_COBRA){
wait_keep_time_ms = 1000;
motion_state = MOTION_STATE_COBRA_TURN_FRONT_WAIT;
}else{
wait_keep_time_ms = 1000;
motion_state = MOTION_STATE_OTHER_TURN_FRONT_WAIT;
}
wait_start_time_ms = millis();
// send_data = 143; // 何もしないときの応答
send_data = 174; // バイスタンプ点滅発光(遅)要求
break;
case 224: // バイスタンプ点滅発光(遅)応答
send_data = 127; // スタンプセット継続要求
break;
default:
send_data = 127; // スタンプセット継続要求
}
}
delay(SEND_IR_DELAY_MS);
// 読み取ったデータに対するレスポンスをバイスタンプへ伝える
send_ir(IR_RX_TX_FOR_STAMP_PIN, send_data);
// レスポンスとしてスタンプへ伝えた結果を表示する
Serial.print("Stamp <- Adapter: ");
print_data(send_data);
}
// Toioのイベント処理
toio.loop();
// 時間経過処理
if(stamp_id == STAMP_ID_COBRA){
switch(motion_state){
case MOTION_STATE_INIT:
break;
case MOTION_STATE_COBRA_GO_TO_CENTER_WAIT:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 中心へ移動開始、1.5秒後にアクションを起こすよう設定
toiocore->goToTarget(COORDINATE_X_ORIGIN, COORDINATE_Y_ORIGIN, ANGLE_FRONT, MOVE_TYPE_WITH_ROLL_WITHOUT_BACK, 100, ACC_TYPE_INC_AND_DEC);
motion_state = MOTION_STATE_COBRA_GO_TO_CENTER;
wait_start_time_ms = millis();
wait_keep_time_ms = 1500;
}
break;
case MOTION_STATE_COBRA_GO_TO_CENTER:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 中心に移動してきてからの最初の行動
play_sound(VOICE_WELCOME);
// 右向き開始、0.15秒後にアクションを起こすよう設定
toiocore->controlMotor(true, 10, false, 10, 150);
motion_state = MOTION_STATE_COBRA_HELLO_1;
wait_start_time_ms = millis();
wait_keep_time_ms = 150;
}
break;
case MOTION_STATE_COBRA_TURN_FRONT_WAIT:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 前を向いて、0.2秒後にアクションを起こすよう設定
sensor_data = toiocore->getSensor(); // 現在地の取得
toiocore->goToTarget(sensor_data.x, sensor_data.y, ANGLE_FRONT, MOVE_TYPE_WITH_ROLL_WITHOUT_BACK, 40, ACC_TYPE_INC_AND_DEC);
motion_state = MOTION_STATE_COBRA_TURN_FRONT;
wait_start_time_ms = millis();
wait_keep_time_ms = 200;
}
break;
case MOTION_STATE_COBRA_TURN_FRONT:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 前を向き終わってからの最初の行動
play_sound(VOICE_HELLO);
// 右向き開始、0.15秒後にアクションを起こすよう設定
toiocore->controlMotor(MOTOR_L_FORWARD, MOTOR_SPEED_VERY_SLOW, MOTOR_R_BACKWARD, MOTOR_SPEED_VERY_SLOW, 150);
motion_state = MOTION_STATE_COBRA_HELLO_1;
wait_start_time_ms = millis();
wait_keep_time_ms = 150;
}
case MOTION_STATE_COBRA_HELLO_1:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 左向き開始、0.3秒後にアクションを起こすよう設定
toiocore->controlMotor(MOTOR_L_BACKWARD, MOTOR_SPEED_VERY_SLOW, MOTOR_R_FORWARD, MOTOR_SPEED_VERY_SLOW, 300);
motion_state = MOTION_STATE_COBRA_HELLO_2;
wait_start_time_ms = millis();
wait_keep_time_ms = 300;
}
break;
case MOTION_STATE_COBRA_HELLO_2:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 右向き開始、0.3秒後にアクションを起こすよう設定
toiocore->controlMotor(MOTOR_L_FORWARD, MOTOR_SPEED_VERY_SLOW, MOTOR_R_BACKWARD, MOTOR_SPEED_VERY_SLOW, 300);
motion_state = MOTION_STATE_COBRA_HELLO_3;
wait_start_time_ms = millis();
wait_keep_time_ms = 300;
}
break;
case MOTION_STATE_COBRA_HELLO_3:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 左向き開始、0.3秒後にアクションを起こすよう設定
toiocore->controlMotor(MOTOR_L_BACKWARD, MOTOR_SPEED_VERY_SLOW, MOTOR_R_FORWARD, MOTOR_SPEED_VERY_SLOW, 300);
motion_state = MOTION_STATE_COBRA_HELLO_4;
wait_start_time_ms = millis();
wait_keep_time_ms = 300;
}
break;
case MOTION_STATE_COBRA_HELLO_4:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 右向き開始、0.15秒後にアクションを終了するよう設定
toiocore->controlMotor(MOTOR_L_FORWARD, MOTOR_SPEED_VERY_SLOW, MOTOR_R_BACKWARD, MOTOR_SPEED_VERY_SLOW, 150);
motion_state = MOTION_STATE_COBRA_HELLO_5;
wait_start_time_ms = millis();
wait_keep_time_ms = 150;
}
break;
case MOTION_STATE_COBRA_HELLO_5:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 一連のモーションを終了する
motion_state = MOTION_STATE_INIT;
}
break;
default:
;
}
}else{
switch(motion_state){
case MOTION_STATE_INIT:
break;
case MOTION_STATE_OTHER_GO_TO_CENTER_WAIT:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 中心へ移動開始、2秒後にアクションを起こすよう設定
toiocore->goToTarget(COORDINATE_X_ORIGIN, COORDINATE_Y_ORIGIN, ANGLE_FRONT, MOVE_TYPE_WITH_ROLL_WITHOUT_BACK, 60, ACC_TYPE_INC_AND_DEC);
motion_state = MOTION_STATE_OTHER_GO_TO_CENTER;
wait_start_time_ms = millis();
wait_keep_time_ms = 2000;
}
break;
case MOTION_STATE_OTHER_GO_TO_CENTER:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 中心に移動してきてからの最初の行動
play_sound(VOICE_KUZU);
// 1.5秒後に後ろを向くよう設定
motion_state = MOTION_STATE_OTHER_TURN_BACK;
wait_start_time_ms = millis();
wait_keep_time_ms = 1500;
}
break;
case MOTION_STATE_OTHER_TURN_FRONT_WAIT:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 前向き開始、760秒後にアクションを起こすよう設定
toiocore->controlMotor(MOTOR_L_FORWARD, MOTOR_SPEED_SLOW, MOTOR_R_BACKWARD, MOTOR_SPEED_SLOW, 760);
motion_state = MOTION_STATE_OTHER_TURN_FRONT;
wait_start_time_ms = millis();
wait_keep_time_ms = 760;
}
break;
case MOTION_STATE_OTHER_TURN_FRONT:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 前を向き終わってからの最初の行動
play_sound(VOICE_KUZU);
// 1.5秒後に後ろを向くよう設定
motion_state = MOTION_STATE_OTHER_TURN_BACK;
wait_start_time_ms = millis();
wait_keep_time_ms = 1500;
}
case MOTION_STATE_OTHER_TURN_BACK:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// その場でゆっくり後ろを振り向く
toiocore->controlMotor(MOTOR_L_FORWARD, MOTOR_SPEED_VERY_SLOW, MOTOR_R_BACKWARD, MOTOR_SPEED_VERY_SLOW, 1520);
// 1.52秒後のアクションを設定
motion_state = MOTION_STATE_OTHER_TURN_BACK_END;
wait_start_time_ms = millis();
wait_keep_time_ms = 1520;
}
break;
case MOTION_STATE_OTHER_TURN_BACK_END:
if(millis() - wait_start_time_ms >= wait_keep_time_ms){
// 一連のモーションを終了する
motion_state = MOTION_STATE_INIT;
}
break;
default:
;
}
}
// 一連の行動をとっていないときは、傾きによるTOIOのコントロール
if(stamp_id == STAMP_ID_COBRA && motion_state == MOTION_STATE_INIT){
control_toio();
}
}