はじめに
超音波の送受信で誘導者についていく追っかけロボットを作ろうと目指しました。
こんな感じです。
結果的には、完成には至れずまだ仕組みが完全に確立できていません。
いきなり謝辞から始まる
部品提供を受けているため申し訳ないのと、応募できなかった場合は返却しないといけないという免責や今後のコンテストへの影響がありそうでしたので途中まででもできた範囲で提出するものとします。
別の技術的アプローチを追加する必要がありそうですが出来たところまで、出すスタイルで行きたいと思います。
モニター提供を受けるときに申告した予定時間を大幅に費やしてもいますので、その時間をなかったものとして無駄にはしたくないですし・・・
申し訳ありませんが、ご了承ください。
最後まで、様々な所について調整を図っていた影響で、記事自体も乱雑になってしまい申し訳ありません。（情報絶対足りていない気がします。）
そして今、作った図や写真等を貼り付ける作業に入っていますが、サーバーが重たい。間に合わなくて "ToDo ★" が残ってしまっていたらすみません。
きっかけ
SONY の太田様の セミナー動画で、音源推定をやっていて、これを見ているとき超音波を使って実現したら静かに（非可聴周波数域の音波で）ついてくるロボットが出来るのでは？と思ったのがきっかけです。
こちらの動画です。 SPRESENSEではじめるローパワーエッジAI 太田 義則様の講義動画
でも実際はそんなに甘くはなかった・・・orz
システム構成
仕組み
誘導機、追っかけロボットの下記 1. ～ 6. の様な連携で考えました。
- 追っかけボットは超音波で叫ぶ。
- 誘導機に叫びが届いたら、超音波で応答（オウム返し）する。（誘導機は基本的にオウム返しするだけ）
- 誘導側から来た超音波をステレオで受信し、超音波が届くまでの左右の時間差と位相差から方向を求める。
- 追っかけロボットは自分の呼びかけから応答が返ってくるまでの時間で距離を把握する。
- 追っかけロボットは距離と方角から、移動方向とスピードを決めて車輪を動かし、誘導機の方向に向かう。
実装
機体
機体としては誘導機と、追っかけロボットの２機体構成です。
それぞれに SPRESENSE と超音波送受信機能を搭載させています。
誘導機
基本的に超音波をオウム返しするだけのものです。
ロボット飼い主の足に結び付ける想定で作成しました。もっと小型化して目立たない所にさり気なく付ける想定でしたが・・・でかい！
これくらいにはしたい。
追っかけロボット
車輪がついて自走出来、超音波で呼びかけたり音の方向を探知できたりするロボットです。
将来的には動物的なボディーを被せて可愛い感じに仕上げたかったのですが・・・ うーん。程遠い。
メカ部品
基本的 構成部品のサイズに合わせて3Dプリンタで骨組みを作り組み立てました。
設計データは公開したいですが STL ファイルや PDF ファイルを載せる方法が elchika にはないため、
手持ちのモバイルバッテリや、家で余っている車輪付きギア付きモーター等を利用しまし、
寸法に合わせこんですべて自作です。
ハードウェア
誘導する側、追っかけロボット側 いずれも SPRESERNE で 超音波送受信を行っています。
主な使用部品は以下の通り。
構成部品表
| 名称 | 品名/品番 | スペック/説明 | 工数 | 備考 |
|---|---|---|---|---|
| メイン基板 | CXD5602PWBMAIN1 | SPRESENSE本体ボード | 2 | 1つはコンテストモニターで提供いただいたもの |
| 拡張基板 | CXD5602PWBEXT1 | SPRESENSE拡張用ボード | 2 | １つはコンテストモニターで提供いただいたもの |
| SPRESENSE用 BLEベースボード | ISP1507搭載 | BLE UART | 1 | コンテストモニターで提供いただいた※1 |
| SPRESENSE用Qwiic接続基板 | ISP1507搭載 | BLE UART | 1 | コンテストモニターで提供いただいた※2 |
| 超音波送信機 | TCT40-16T | 中心周波数40KHz / 16Φ | 2 | 1つ以外はAliExpressで購入 |
| 超音波受信機 | TCT40-16R | 中心周波数40KHz / 16Φ | 3 | 1つ以外はAliExpressで購入 |
| モータードライバー | TB6612FNG | 2ch | 1 | |
| レベル変換 IC | TXB010BPWR | 双方向バッファータイプ8bit | 1 | 1.8V を 3.3V に変換(※１) |
| コネクタ類 | XH2, EH2, EH3, PI | 部品取り付け用 | - | 詳細は割愛 |
| 車輪・ギア付きモーター | ギヤ比1:120 | 2 |
※１
双方向の物は要らないのですが、手持ちに 74LCX245 位しかなく、1.8V がスペック外だったため贅沢にも！
接続図
ToDo ★接続図をここに入れる
接続図はわかりにくいので以下の表で接続を示します。
[誘導機]
ToDo ★接続表をここに入れる
[追っかけロボット]
ToDo ★接続表をここに入れる
ソフトウェア
ソースファイル
すみません。色々試しているうちに色んなリビジョンを git にコミットしてしまい、どれが一番いい物か判らなくなってしまいました。（もっとちゃんとコミットコメント書いておくべきでした。）一番進んでいて安定していた（と思われる）ソースを掲載します。（途中の場所や変なコメントが残っているかもしれません。）最低限どの Core に（未使用なもの以外）切り替えてもコンパイルが通る（検証が完了する)ところまでは確認してあります。
あと、コメント少なく整理できていなく汚いです。
誘導機側ソース
// ########################################
// ## Common Part ##
// ########################################
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
#include "Arduino.h"
#include <MP.h>
#include <MPMutex.h>
#if !defined(SUBCORE)
// [信号処理関連]
#include <FrontEnd.h>
#endif //!defined(SUBCORE)
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
// [アプリ定義]
//#define KEEP_SENDING_SONAR_MODE
// [汎用]
#define ARRAY_SIZEOF(x) (sizeof(x) / sizeof(x[0]))
// [端子関連]
#define PIN_RECV_LED LED0
#define PIN_SEND_LED LED1
#define PIN_MOTOR_LED LED2
#define PIN_FFT_LED LED3
#define ONBOARD_LED_ON HIGH
#define ONBOARD_LED_OFF LOW
#define PIN_SONAR_SEND 8
#define PIN_SONAR_RECV_L A0
#define PIN_SONAR_RECV_R A1
// [Message 関連]
// [ログ関連]
#define LOG_LEVEL_FATAL 0
#define LOG_LEVEL_ERROR 1
#define LOG_LEVEL_WARNING 2
#define LOG_LEVEL_INFO 3
#define LOG_LEVEL_DEBUG 4
#define LOG_MAX_LEN 240
#define DEFAULT_LOG_LEVEL LOG_LEVEL_DEBUG
#define BleLog(x,y) Serial2.printf(x,y)
#define FTL_LOG(format, ...) do \
{\
if( SysLogLevel >= LOG_LEVEL_FATAL) {\
char tmp_str[LOG_MAX_LEN+1];\
snprintf(tmp_str, sizeof(tmp_str), format, ##__VA_ARGS__);\
MPLog("F:%s\r\n",tmp_str);\
if (BleLogEnabled) BleLog("F:%s\r\n",tmp_str);\
}\
} while(0)
#define ERR_LOG(format, ...) do \
{\
if( SysLogLevel >= LOG_LEVEL_ERROR) {\
char tmp_str[LOG_MAX_LEN+1];\
snprintf(tmp_str, sizeof(tmp_str), format, ##__VA_ARGS__);\
MPLog("E:%s\r\n",tmp_str);\
if (BleLogEnabled) BleLog("F:%s\r\n",tmp_str);\
}\
} while(0)
#define WRN_LOG(format, ...) do \
{\
if( SysLogLevel >= LOG_LEVEL_WARNING) {\
char tmp_str[LOG_MAX_LEN+1];\
snprintf(tmp_str, sizeof(tmp_str), format, ##__VA_ARGS__);\
MPLog("W:%s\r\n",tmp_str);\
if (BleLogEnabled) BleLog("F:%s\r\n",tmp_str);\
}\
} while(0)
#define INF_LOG(format, ...) do \
{\
if( SysLogLevel >= LOG_LEVEL_INFO) {\
char tmp_str[LOG_MAX_LEN+1];\
snprintf(tmp_str, sizeof(tmp_str), format, ##__VA_ARGS__);\
MPLog("I:%s\r\n",tmp_str);\
if (BleLogEnabled) BleLog("F:%s\r\n",tmp_str);\
}\
} while(0)
#define DBG_LOG(format, ...) do \
{\
if( SysLogLevel >= LOG_LEVEL_DEBUG) {\
char tmp_str[LOG_MAX_LEN+1];\
snprintf(tmp_str, sizeof(tmp_str), format, ##__VA_ARGS__);\
MPLog("D:%s\r\n",tmp_str);\
if (BleLogEnabled) BleLog("F:%s\r\n",tmp_str);\
}\
} while(0)
// [信号処理関連]
#define SAMPLE_SIZE (1024)
//#define SAMPLE_SIZE (768)
//#define SAMPLE_SIZE (512)
//#define DEBUG_ENABLE
#define SONAR_FREQUENCY (40*1000)
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
typedef enum {
e_core_id_main_app_recv = 0,
e_core_id_sub_sonar_send,
e_core_id_sub_bt_dbg,
e_core_id_count
} e_core_id_t;
typedef enum {
e_msg_id_init_done = 1,
e_msg_id_recv_start,
e_msg_id_revv_stop,
e_msg_id_send_sonar,
e_msg_id_send_sonar_done,
e_msg_id_send_stop,
e_msg_id_ctrl_motor,
e_msg_id_ble_started,
e_msg_id_ble_log,
} e_msg_id_t;
union msg_data_t {
struct {
uint8_t core_id_flags;
} init_done;
struct {
char log_str;
} ble_log;
uint32_t dummy32;
};
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
bool BleLogEnabled = false;
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
const int SysLogLevel = DEFAULT_LOG_LEVEL;
const uint8_t all_sub_core_flags = ( 0
| (1 << e_core_id_sub_sonar_send)
| (1 << e_core_id_sub_bt_dbg)
);
const struct sonar_params_t {
struct { // [共通項目]
float speed_of_sound; // 音速 (m/s)
} common;
#if !defined(SUBCORE)
struct { // [受信用項目]
uint32_t sonar_freq; // 中心周波数
uint64_t sonar_duration_us; // 送信音波間隔
uint32_t sonar_accept_size_freq; // 検知範囲のサイズ（中心周波数からのズレサイズ）
uint64_t sonar_accept_size_duration_us; // 検知範囲のサイズ（中心周波数からのズレサイズ）
float freq_bandwidth_per_sample; // 周波数分解能
} recv_params;
#endif // !defined(SUBCORE)
struct { // [送信用項目]
uint32_t sonar_freq; // 送信周波数
uint64_t sonar_duration_us; // 送信音波間隔
} send_params;
} sonar_params = {
{ // [共通項目]
343.0 // 音速 (m/s)
},
#if !defined(SUBCORE)
{ // [受信用項目]
SONAR_FREQUENCY, // 中心周波数
( 3*1000), // 送信音波間隔
1000, // 検知範囲のサイズ（中心周波数からのズレサイズ）
(50*1000), // 検知範囲のサイズ（送信音波間隔中央値からのズレサイズ）
(AS_SAMPLINGRATE_192000 / SAMPLE_SIZE) // 周波数分解能
},
#endif // !defined(SUBCORE)
{ // [送信用項目]
SONAR_FREQUENCY, // 送信周波数
( 5*1000) // 送信音波間隔
}
};
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
// ########################################
// ## Main Core Part ##
// ########################################
#if !defined(SUBCORE)
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
// [信号処理関連]
#include <FrontEnd.h>
#include <MemoryUtil.h>
#include <arch/board/board.h>
#define ARM_MATH_CM4
#define __FPU_PRESENT 1U
#include <arm_math.h>
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
#define AppliMain_setup setup
#define AppliMain_loop loop
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
typedef enum {
e_app_state_failed_err = -1,
e_app_state_none = 0,
e_app_state_inactive,
e_app_state_idle,
e_app_state_sending_sonar,
//e_app_state_waiting_sonar,
e_app_state_count
} e_app_state_t;
typedef enum {
e_state_event_err = -1,
e_state_event_none = 0,
e_state_event_init_done,
e_state_event_send_start,
e_state_event_send_end,
} e_state_event_t;
typedef enum {
e_recv_state_inactive = 0,
e_recv_state_idle,
e_recv_state_in_range,
e_recv_state_done,
e_recv_state_count
} e_recv_state_t;
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
e_app_state_t curr_app_state = e_app_state_none;
e_recv_state_t recv_sonar_state = e_recv_state_inactive;
arm_rfft_fast_instance_f32 S;
FrontEnd *theFrontEnd;
bool isErr = false;
uint64_t send_start_time;
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
void AppState_Main(e_state_event_t state_event);
void SignalProcessor_setup();
void SignalProcessor_loop();
void frontend_attention_cb(const ErrorAttentionParam *param);
static bool frontend_done_cb(AsMicFrontendEvent ev, uint32_t result, uint32_t detail);
static void frontend_pcm_cb(AsPcmDataParam pcm);
void signal_process(int16_t* stereo_input, int16_t* stereo_output, uint32_t sample_size);
void frontend_signal_input(AsPcmDataParam pcm, uint8_t* input, uint32_t frame_size);
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
const int32_t channel_num = AS_CHANNEL_MONO;
const int32_t bit_length = AS_BITLENGTH_16;
const int32_t sample_size = SAMPLE_SIZE;
const int32_t frame_size = sample_size * (bit_length / 8) * channel_num;
const uint64_t send_interval_ms = 1000ull;
const uint64_t sonar_recv_dulation_ms = (1000);
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
void AppliMain_setup()
{
int rc = 0;
int subid;
Serial.begin(115200);
while (!Serial);
INF_LOG("Guide Started!");
pinMode(PIN_RECV_LED, OUTPUT);
pinMode(PIN_SEND_LED, OUTPUT);
pinMode(PIN_MOTOR_LED, OUTPUT);
pinMode(PIN_FFT_LED, OUTPUT);
digitalWrite(PIN_RECV_LED, ONBOARD_LED_OFF);
digitalWrite(PIN_SEND_LED, ONBOARD_LED_OFF);
digitalWrite(PIN_MOTOR_LED, ONBOARD_LED_OFF);
digitalWrite(PIN_FFT_LED, ONBOARD_LED_OFF);
/* Boot SubCore */
for (subid = (e_core_id_main_app_recv + 1); subid < e_core_id_count; subid++) {
rc = MP.begin(subid);
if (rc < 0) {
ERR_LOG("MP.begin(%d) error = %d\n", subid, rc);
}
}
/* Polling */
MP.RecvTimeout(MP_RECV_POLLING);
// Initialize Signal Processor
SignalProcessor_setup();
}
void AppliMain_loop()
{
int rc;
int subid;
int8_t msg_recv_id;
msg_data_t *rcv_msg;
e_state_event_t state_event;
static uint8_t init_done_flags = 0;
/* Receive message from SubCore */
for (subid = (e_core_id_main_app_recv + 1); subid <= e_core_id_count; subid++) {
state_event = e_state_event_none;
rc = MP.Recv(&msg_recv_id, &rcv_msg, subid);
if (rc > 0) {
// INF_LOG("%d)%d-%lx\n", subid, msgid, rcv_msg->dummy32);
//DBG_LOG("Main recieved! %d-%d=%d", subid, msg_recv_id, rc);
switch(msg_recv_id) {
case e_msg_id_init_done:
init_done_flags |= rcv_msg->init_done.core_id_flags;
if ( init_done_flags != all_sub_core_flags) {
INF_LOG("Waiting Init flag = %x (%x/%x)", rcv_msg->init_done.core_id_flags, init_done_flags, all_sub_core_flags);
}
else {
INF_LOG("Init DONE!! flag = %x (%x/%x)", rcv_msg->init_done.core_id_flags, init_done_flags, all_sub_core_flags);
state_event = e_state_event_init_done;
}
break;
case e_msg_id_send_sonar_done:
state_event = e_state_event_send_end;
break;
default:
ERR_LOG("Invalid MsgID:%d", msg_recv_id);
break;
}
delay(1000);
}
if ( e_recv_state_done == recv_sonar_state ) {
state_event = e_state_event_send_start;
}
AppState_Main(state_event);
SignalProcessor_loop();
}
}
void AppState_Main(e_state_event_t state_event)
{
int rc;
msg_data_t msg_data_snd;
e_app_state_t next_app_state = e_app_state_none;
// uint64_t millis_now;
// 状態遷移チェック
switch(curr_app_state)
{
case e_app_state_failed_err:
// Nothing to do!
break;
case e_app_state_none:
if ( e_state_event_init_done == state_event ) {
next_app_state = e_app_state_idle;
}
break;
case e_app_state_inactive:
// Nothing to do!
break;
case e_app_state_idle:
if ( e_state_event_send_start == state_event ) {
next_app_state = e_app_state_sending_sonar;
}
#if 0
millis_now = millis();
if (millis_now >= ( last_send_time + send_interval_ms) ) {
next_app_state = e_app_state_sending_sonar;
}
#endif
break;
case e_app_state_sending_sonar:
if (e_state_event_send_end == state_event) {
next_app_state = e_app_state_idle;
}
break;
default:
// Nothing to do!
break;
}
// 状態変化時
if (next_app_state != e_app_state_none) {
// 各状態離脱時の処理
switch(curr_app_state)
{
case e_app_state_failed_err:
// Nothing to do!
break;
case e_app_state_none:
// Nothing to do!
break;
case e_app_state_inactive:
// Nothing to do!
break;
case e_app_state_idle:
// Nothing to do!
break;
case e_app_state_sending_sonar:
// Nothing to do!
break;
default:
// Nothing to do!
break;
}
// 状態遷移
DBG_LOG("[APP STATE] %d->%d", curr_app_state, next_app_state);
curr_app_state = next_app_state;
// 各状態突入時の処理
switch(curr_app_state)
{
case e_app_state_failed_err:
// Nothing to do!
break;
case e_app_state_none:
// Nothing to do!
break;
case e_app_state_inactive:
// Nothing to do!
break;
case e_app_state_idle:
break;
case e_app_state_sending_sonar:
//last_send_time = millis();
//send_start_time = last_send_time;
break;
default:
// Nothing to do!
break;
}
}
}
void SignalProcessor_setup()
{
arm_rfft_fast_init_f32(&S, SAMPLE_SIZE);
/* Initialize memory pools and message libs */
initMemoryPools();
createStaticPools(MEM_LAYOUT_RECORDINGPLAYER);
/* setup FrontEnd and Mixer */
theFrontEnd = FrontEnd::getInstance();
/* set clock mode */
theFrontEnd->setCapturingClkMode(FRONTEND_CAPCLK_HIRESO);
/* begin FrontEnd and OuputMixer */
theFrontEnd->begin(frontend_attention_cb);
INF_LOG("Setup: FrontEnd began");
/* activate FrontEnd and Mixer */
theFrontEnd->setMicGain(0);
theFrontEnd->activate(frontend_done_cb);
delay(100); /* waiting for Mic startup */
INF_LOG("Setup: FrontEnd activated");
/* Initialize FrontEnd */
AsDataDest dst;
dst.cb = frontend_pcm_cb;
theFrontEnd->init(channel_num, bit_length, sample_size, AsDataPathCallback, dst);
INF_LOG("Setup: FrontEnd initialized");
/* Unmute */
theFrontEnd->start();
INF_LOG("Setup: FrontEnd started");
}
void SignalProcessor_loop()
{
if (isErr == true) {
board_external_amp_mute_control(true);
theFrontEnd->stop();
theFrontEnd->deactivate();
theFrontEnd->end();
INF_LOG("Capturing Process Terminated by ERROR!");
while(1) {};
}
}
void frontend_attention_cb(const ErrorAttentionParam *param) {
ERR_LOG("ERROR: Attention! Something happened at FrontEnd");
if (param->error_code >= AS_ATTENTION_CODE_WARNING) isErr = true;
}
static bool frontend_done_cb(AsMicFrontendEvent ev, uint32_t result, uint32_t detail){
UNUSED(ev); UNUSED(result); UNUSED(detail);
return true;
}
static void frontend_pcm_cb(AsPcmDataParam pcm) {
static uint8_t mono_input[frame_size];
static uint8_t stereo_output[frame_size*2];
static const bool time_measurement = false;
if (time_measurement) {
static uint32_t last_time = 0;
uint32_t current_time = micros();
uint32_t duration = current_time - last_time;
last_time = current_time;
INF_LOG(("duration = " + String(duration)).c_str());
}
frontend_signal_input(pcm, mono_input, frame_size);
signal_process((int16_t*)mono_input, (int16_t*)stereo_output, sample_size);
return;
}
void frontend_signal_input(AsPcmDataParam pcm, uint8_t* input, uint32_t frame_size) {
/* clean up the input buffer */
memset(input, 0, frame_size);
if (!pcm.is_valid) {
WRN_LOG("WARNING: Invalid data! @frontend_signal_input");
return;
}
if (pcm.size > frame_size) {
WRN_LOG("WARNING: Captured size is too big! {");
WRN_LOG(String(pcm.size).c_str());
WRN_LOG("} @frontend_signal_input");
pcm.size = frame_size;
}
/* copy the signal to signal_input buffer */
if (pcm.size != 0) {
memcpy(input, pcm.mh.getPa(), pcm.size);
} else {
WRN_LOG("WARNING: Captured size is zero! @frontend_signal_input");
}
}
void signal_process(int16_t* mono_input, int16_t* stereo_output, uint32_t sample_size) {
// 処理開始時間
uint64_t current_time = micros();
int rc;
msg_data_t msg_data_snd;
static uint64_t recv_start_time = 0;
static uint64_t signal_start_time = 0;
static uint64_t signal_end_time = 0;
static float angle_sum = 0;
static uint16_t angle_count = 0;
static const int targetIndexMin = (int)((float)(sonar_params.recv_params.sonar_freq - sonar_params.recv_params.sonar_accept_size_freq) / sonar_params.recv_params.freq_bandwidth_per_sample); // 最小周波数インデックス
static const int targetIndexMax = (int)((float)(sonar_params.recv_params.sonar_freq + sonar_params.recv_params.sonar_accept_size_freq) / sonar_params.recv_params.freq_bandwidth_per_sample); // 最大周波数インデックス
const uint64_t minDuration = sonar_params.recv_params.sonar_duration_us - sonar_params.recv_params.sonar_accept_size_duration_us;
const uint64_t maxDuration = sonar_params.recv_params.sonar_duration_us + sonar_params.recv_params.sonar_accept_size_duration_us;
const float micDistance = (sonar_params.common.speed_of_sound / sonar_params.recv_params.sonar_freq) / 2.0; // 波長の1/2
// 変数をスイッチの外で宣言
float angleLeft = 0.0f, angleRight = 0.0f, phaseDiff = 0.0f;
float timeDiff = 0.0f, angle = 0.0f;
uint64_t duration = 0;
if (curr_app_state != e_app_state_idle) {
//DBG_LOG("Not Ready Sig Process!");
recv_sonar_state = e_recv_state_inactive;
return;
}
digitalWrite(PIN_FFT_LED, ONBOARD_LED_ON);
static float monoSrc[SAMPLE_SIZE]; // モノラルチャネル信号
//static float rightChannel[SAMPLE_SIZE]; // 右チャネル信号
static float fftMono[SAMPLE_SIZE]; // 左チャネルFFT結果
//static float fftRight[SAMPLE_SIZE]; // 右チャネルFFT結果
static float monoDst[SAMPLE_SIZE];
#if 0
for (int i = 0; i < SAMPLE_SIZE; i++) {
monoSrc[i] = mono_input[i * 2]; // 左チャネル
//rightChannel[i] = mono_input[i * 2 + 1]; // 右チャネル
}
/*
for (int i = 0, n = 0; n < SAMPLE_SIZE; ++n) {
monoSrc[n] = (float)mono_input[i++];
}
*/
#else
// PCMデータを浮動小数点形式に変換
arm_q15_to_float(&mono_input[0], &monoSrc[0], SAMPLE_SIZE);
#endif
arm_rfft_fast_f32(&S, &monoSrc[0], &fftMono[0], 0);
float maxSigLevel;
uint32_t peakIndex;
arm_cmplx_mag_f32(&fftMono[0], &monoDst[0], SAMPLE_SIZE / 2);
//arm_cmplx_mag_f32(fftRight, fftRight, SAMPLE_SIZE / 2);
arm_max_f32(&monoDst[0], SAMPLE_SIZE / 2, &maxSigLevel, &peakIndex);
//arm_max_f32(fftRight, SAMPLE_SIZE / 2, &maxSigLevelRight, &peakIndexRight);
bool inRange = (peakIndex >= targetIndexMin && peakIndex <= targetIndexMax);
//DBG_LOG("inRange = %d", (int)inRange);
#if 0 // for debug
static uint64_t last_dbg_timestamp = 0;
if ( current_time > (last_dbg_timestamp + 200000ll) ) {
#if 1
DBG_LOG("InRange = %d (%d <= %d <= %d)", inRange, targetIndexMin, peakIndex, targetIndexMax);
#endif
#if 0
DBG_LOG("INDEX %d,%d FREQ %10.2f,%10.2f", peakIndexLeft, peakIndexRight, (float)peakIndexLeft * df, (float)peakIndexRight * df);
#endif
#if 0
Serial.printf("FFT:");for (int i = 0; i< 32; i++) Serial.printf("%f%c", fftMono[i+0], ((i==31)? '\n': ','));
#endif
#if 0
Serial.printf("DST:");for (int i = 0; i< 32; i++) Serial.printf("%f%c", monoDst[i+0], ((i==31)? '\n': ','));
#endif
last_dbg_timestamp = current_time;
}
#endif // for debug
switch (recv_sonar_state) {
case e_recv_state_inactive: // 受信処理初回
recv_start_time = current_time;
recv_sonar_state = e_recv_state_idle;
break;
case e_recv_state_idle: // 40K超音波未検出
if (inRange) {
DBG_LOG("SIG Started");
signal_start_time = current_time;
angle_sum = 0;
angle_count = 0;
recv_sonar_state = e_recv_state_in_range;
}
break;
case e_recv_state_in_range: // 40K超音波検出済み
#if 0
if (inRange) { // 検出継続
// 角度計算
//angleLeft = atan2f(fftLeft[peakIndexLeft * 2 + 1], fftLeft[peakIndexLeft * 2]);
//angleRight = atan2f(fftRight[peakIndexRight * 2 + 1], fftRight[peakIndexRight * 2]);
phaseDiff = angleRight - angleLeft;
if (phaseDiff > M_PI) phaseDiff -= 2 * M_PI;
if (phaseDiff < -M_PI) phaseDiff += 2 * M_PI;
timeDiff = phaseDiff / (2 * M_PI * sonar_params.recv_params.sonar_freq);
angle = asin(sonar_params.common.speed_of_sound * timeDiff / micDistance) * 180.0 / M_PI;
angle_sum += angle;
angle_count++;
}
#endif
if (!inRange) { // 検出離脱時
signal_end_time = current_time;
duration = signal_end_time - signal_start_time;
// 長さが範囲内なら決定し、方向と距離を計算
if (duration >= minDuration && duration <= maxDuration) {
// float avgAngle = angle_sum / angle_count;
float distance = (sonar_params.common.speed_of_sound * duration) / 2000000.0;
INF_LOG("[RECV DONE] Distance=%5.2f", distance);
/* Start periodic Sending SONAR */
rc = MP.Send((int8_t)e_msg_id_send_sonar, &msg_data_snd, (int)e_core_id_sub_sonar_send);
if (rc < 0) {
ERR_LOG("MP.Send error = %d\n", rc);
}
recv_sonar_state = e_recv_state_done;
} else {
DBG_LOG("Invalid signal duration = %llu", duration);
recv_sonar_state = e_recv_state_idle;
}
}
break;
case e_recv_state_done:
recv_sonar_state = e_recv_state_idle;
break;
default:
recv_sonar_state = e_recv_state_inactive;
break;
}
digitalWrite(PIN_FFT_LED, ONBOARD_LED_OFF);
}
// ########################################
// ## Sub1 Core Part <Sonar Sender> ##
// ########################################
#elif (SUBCORE == 1)
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
#include <sys/ioctl.h>
#include <stdio.h>
#include <fcntl.h>
#include <nuttx/timers/pwm.h>
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
#define SonarSender_setup setup
#define SonarSender_loop loop
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
int fd;
struct pwm_info_s info;
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
void SonarSender_setup(void);
void SonarSender_loop(void);
void SendOneShotSonar(void);
//void send_signal(uint16_t output_hz);
//void encode(uint8_t c);
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
const e_core_id_t self_core_id = e_core_id_sub_sonar_send;
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
void SonarSender_setup(void)
{
int rc;
msg_data_t msg_data_snd;
MP.begin();
MP.RecvTimeout(MP_RECV_POLLING);
fd = open("/dev/pwm0", O_RDONLY);
msg_data_snd.init_done.core_id_flags = (uint8_t)(1 << self_core_id);
rc = MP.Send((int8_t)e_msg_id_init_done, &msg_data_snd);
if (rc < 0) {
ERR_LOG("MP.Send(%d) error = %d\n", e_msg_id_init_done, rc);
}
#if 0//defined(KEEP_SENDING_SONAR_MODE)
info.frequency = sonar_params.send_params.sonar_freq; // 40kHz
info.duty = 0x7fff;
// Start Sonar send
ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)((uintptr_t)&info));
ioctl(fd, PWMIOC_START, 0);
#endif // defined(KEEP_SENDING_SONAR_MODE)
}
void SonarSender_loop(void)
{
int rc;
int subid;
int8_t msg_recv_id;
msg_data_t *msg_recv_dat;
for (subid = 0; subid < e_core_id_count; subid ++)
{
if ( subid == self_core_id) continue;
else if ( subid == 0 ) rc = MP.Recv(&msg_recv_id, &msg_recv_dat);
else rc = MP.Recv(&msg_recv_id, &msg_recv_dat, subid);
if (rc > 0) {
//DBG_LOG("Sub1 recieved! %d-%d=%d", subid, msg_recv_id, rc);
switch(msg_recv_id) {
case e_msg_id_send_sonar:
SendOneShotSonar();
break;
default:
ERR_LOG("Invalid MsgID:%d", msg_recv_id);
break;
}
delay(1000);
}
//else ERR_LOG("MP.Recv(%d) ret=%d", subid, rc);
}
}
void SendOneShotSonar(void)
{
int rc;
msg_data_t msg_data_snd;
digitalWrite(PIN_SEND_LED, ONBOARD_LED_ON);
// DBG_LOG("Send SONAR>");
info.frequency = sonar_params.send_params.sonar_freq; // 40kHz
info.duty = 0x7fff;
// Start Sonar send
ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)((uintptr_t)&info));
ioctl(fd, PWMIOC_START, 0);
delayMicroseconds(sonar_params.send_params.sonar_duration_us);
// Stop Sonar send
ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)((uintptr_t)&info));
ioctl(fd, PWMIOC_STOP, 0);
digitalWrite(PIN_SEND_LED, ONBOARD_LED_OFF);
rc = MP.Send((int8_t)e_msg_id_send_sonar_done, &msg_data_snd);
if (rc < 0) {
ERR_LOG("MP.Send error = %d\n", rc);
}
}
// ########################################
// ## Sub2 Core Part BleDebug> ##
// ########################################
#elif (SUBCORE == 2)
#define BleDebug_setup setup
#define BleDebug_loop loop
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
#define ARRAY_SIZE 128
#define LOG_BUFFER_SIZE 256
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
const e_core_id_t self_core_id = e_core_id_sub_bt_dbg;
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
void BleDebug_setup()
{
int rc;
msg_data_t msg_data_snd;
// Initialize BLE Serial
Serial2.begin(115200);
// if (!Serial2.begin(115200)) {
// ERR_LOG("BLE initialization failed");
// return;
// }
INF_LOG("BLE initialized successfully");
INF_LOG("BLE connected");
MP.begin();
msg_data_snd.init_done.core_id_flags = (uint8_t)(1 << self_core_id);
rc = MP.Send((int8_t)e_msg_id_init_done, &msg_data_snd);
if (rc < 0) {
ERR_LOG("MP.Send(%d) error = %d\n", e_msg_id_init_done, rc);
}
}
void BleDebug_loop()
{
int rc;
int subid;
int8_t msg_recv_id;
msg_data_t *msg_recv_dat;
uint64_t millis_now;
for (subid = 0; subid < e_core_id_count; subid ++)
{
if ( subid == self_core_id) continue;
else if ( subid == 0 ) rc = MP.Recv(&msg_recv_id, &msg_recv_dat);
else rc = MP.Recv(&msg_recv_id, &msg_recv_dat, subid);
if (rc > 0) {
//DBG_LOG("Sub2 recieved! %d-%d=%d", subid, msg_recv_id, rc);
switch(msg_recv_id) {
default:
ERR_LOG("Invalid MsgID:%d", msg_recv_id);
break;
}
delay(1000);
}
//else ERR_LOG("MP.Recv(%d) ret=%d", subid, rc);
}
}
// ########################################
// ## Sub4 Core Part ##
// ########################################
#elif (SUBCORE == 3)
#error "Core-3 Not Used!"
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
// ########################################
// ## Sub5 Core Part ##
// ########################################
// ########################################
// ## Sub4 Core Part ##
// ########################################
#elif (SUBCORE == 4)
#error "Core-4 Not Used!"
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
// ########################################
// ## Sub5 Core Part ##
// ########################################
#elif (SUBCORE == 5)
#error "Core-5 Not Used!"
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
#endif // !defined(SUBCORE)追っかけロボット側ソース
// ########################################
// ## Common Part ##
// ########################################
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
#include "Arduino.h"
#include <MP.h>
#include <MPMutex.h>
#if !defined(SUBCORE)
// [信号処理関連]
#include <FrontEnd.h>
#endif //!defined(SUBCORE)
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
// [アプリ定義]
//#define KEEP_SENDING_SONAR_MODE
// [汎用]
#define ARRAY_SIZEOF(x) (sizeof(x) / sizeof(x[0]))
// [端子関連]
#define PIN_RECV_LED LED0
#define PIN_SEND_LED LED1
#define PIN_MOTOR_LED LED2
#define PIN_FFT_LED LED3
#define ONBOARD_LED_ON HIGH
#define ONBOARD_LED_OFF LOW
#define PIN_SONAR_SEND 8
#define PIN_SONAR_RECV_L A0
#define PIN_SONAR_RECV_R A1
// [Message 関連]
// [ログ関連]
#define LOG_LEVEL_FATAL 0
#define LOG_LEVEL_ERROR 1
#define LOG_LEVEL_WARNING 2
#define LOG_LEVEL_INFO 3
#define LOG_LEVEL_DEBUG 4
#define LOG_MAX_LEN 240
#define DEFAULT_LOG_LEVEL LOG_LEVEL_DEBUG
#define BleLog(x,y) Serial2.printf(x,y)
#define FTL_LOG(format, ...) do \
{\
if( SysLogLevel >= LOG_LEVEL_FATAL) {\
char tmp_str[LOG_MAX_LEN+1];\
snprintf(tmp_str, sizeof(tmp_str), format, ##__VA_ARGS__);\
MPLog("F:%s\r\n",tmp_str);\
if (BleLogEnabled) BleLog("F:%s\r\n",tmp_str);\
}\
} while(0)
#define ERR_LOG(format, ...) do \
{\
if( SysLogLevel >= LOG_LEVEL_ERROR) {\
char tmp_str[LOG_MAX_LEN+1];\
snprintf(tmp_str, sizeof(tmp_str), format, ##__VA_ARGS__);\
MPLog("E:%s\r\n",tmp_str);\
if (BleLogEnabled) BleLog("F:%s\r\n",tmp_str);\
}\
} while(0)
#define WRN_LOG(format, ...) do \
{\
if( SysLogLevel >= LOG_LEVEL_WARNING) {\
char tmp_str[LOG_MAX_LEN+1];\
snprintf(tmp_str, sizeof(tmp_str), format, ##__VA_ARGS__);\
MPLog("W:%s\r\n",tmp_str);\
if (BleLogEnabled) BleLog("F:%s\r\n",tmp_str);\
}\
} while(0)
#define INF_LOG(format, ...) do \
{\
if( SysLogLevel >= LOG_LEVEL_INFO) {\
char tmp_str[LOG_MAX_LEN+1];\
snprintf(tmp_str, sizeof(tmp_str), format, ##__VA_ARGS__);\
MPLog("I:%s\r\n",tmp_str);\
if (BleLogEnabled) BleLog("F:%s\r\n",tmp_str);\
}\
} while(0)
#define DBG_LOG(format, ...) do \
{\
if( SysLogLevel >= LOG_LEVEL_DEBUG) {\
char tmp_str[LOG_MAX_LEN+1];\
snprintf(tmp_str, sizeof(tmp_str), format, ##__VA_ARGS__);\
MPLog("D:%s\r\n",tmp_str);\
if (BleLogEnabled) BleLog("F:%s\r\n",tmp_str);\
}\
} while(0)
// [信号処理関連]
#define SAMPLE_SIZE (1024)
//#define SAMPLE_SIZE (768)
//#define SAMPLE_SIZE (512)
//#define DEBUG_ENABLE
#define SONAR_FREQUENCY (40*1000)
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
typedef enum {
e_core_id_main_app_recv = 0,
e_core_id_sub_sonar_send,
e_core_id_sub_bt_dbg,
e_core_id_count
} e_core_id_t;
typedef enum {
e_msg_id_init_done = 1,
e_msg_id_recv_start,
e_msg_id_revv_stop,
e_msg_id_send_sonar,
e_msg_id_send_sonar_done,
e_msg_id_send_stop,
e_msg_id_ctrl_motor,
e_msg_id_ble_started,
e_msg_id_ble_log,
} e_msg_id_t;
union msg_data_t {
struct {
uint8_t core_id_flags;
} init_done;
struct {
char log_str;
} ble_log;
uint32_t dummy32;
};
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
bool BleLogEnabled = false;
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
const int SysLogLevel = DEFAULT_LOG_LEVEL;
const uint8_t all_sub_core_flags = ( 0
| (1 << e_core_id_sub_sonar_send)
| (1 << e_core_id_sub_bt_dbg)
);
const struct sonar_params_t {
struct { // [共通項目]
float speed_of_sound; // 音速 (m/s)
} common;
#if !defined(SUBCORE)
struct { // [受信用項目]
uint32_t sonar_freq; // 中心周波数
uint64_t sonar_duration_us; // 送信音波間隔
uint32_t sonar_accept_size_freq; // 検知範囲のサイズ（中心周波数からのズレサイズ）
uint64_t sonar_accept_size_duration_us; // 検知範囲のサイズ（中心周波数からのズレサイズ）
float freq_bandwidth_per_sample; // 周波数分解能
} recv_params;
#endif // !defined(SUBCORE)
struct { // [送信用項目]
uint32_t sonar_freq; // 送信周波数
uint64_t sonar_duration_us; // 送信音波間隔
} send_params;
} sonar_params = {
{ // [共通項目]
343.0 // 音速 (m/s)
},
#if !defined(SUBCORE)
{ // [受信用項目]
SONAR_FREQUENCY, // 中心周波数
( 3*1000), // 送信音波間隔
1000, // 検知範囲のサイズ（中心周波数からのズレサイズ）
(50*1000), // 検知範囲のサイズ（送信音波間隔中央値からのズレサイズ）
(AS_SAMPLINGRATE_192000 / SAMPLE_SIZE) // 周波数分解能
},
#endif // !defined(SUBCORE)
{ // [送信用項目]
SONAR_FREQUENCY, // 送信周波数
( 5*1000) // 送信音波間隔
}
};
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
// ########################################
// ## Main Core Part ##
// ########################################
#if !defined(SUBCORE)
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
// [信号処理関連]
#include <FrontEnd.h>
#include <MemoryUtil.h>
#include <arch/board/board.h>
#define ARM_MATH_CM4
#define __FPU_PRESENT 1U
#include <arm_math.h>
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
#define AppliMain_setup setup
#define AppliMain_loop loop
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
typedef enum {
e_app_state_failed_err = -1,
e_app_state_none = 0,
e_app_state_inactive,
e_app_state_idle,
e_app_state_sending_sonar,
//e_app_state_waiting_sonar,
e_app_state_count
} e_app_state_t;
typedef enum {
e_state_event_err = -1,
e_state_event_none = 0,
e_state_event_init_done,
e_state_event_send_start,
e_state_event_send_end,
} e_state_event_t;
typedef enum {
e_recv_state_inactive = 0,
e_recv_state_idle,
e_recv_state_in_range,
e_recv_state_done,
e_recv_state_count
} e_recv_state_t;
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
e_app_state_t curr_app_state = e_app_state_none;
e_recv_state_t recv_sonar_state = e_recv_state_inactive;
arm_rfft_fast_instance_f32 S;
FrontEnd *theFrontEnd;
bool isErr = false;
uint64_t send_start_time;
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
void AppState_Main(e_state_event_t state_event);
void SignalProcessor_setup();
void SignalProcessor_loop();
void frontend_attention_cb(const ErrorAttentionParam *param);
static bool frontend_done_cb(AsMicFrontendEvent ev, uint32_t result, uint32_t detail);
static void frontend_pcm_cb(AsPcmDataParam pcm);
void signal_process(int16_t* stereo_input, int16_t* stereo_output, uint32_t sample_size);
void frontend_signal_input(AsPcmDataParam pcm, uint8_t* input, uint32_t frame_size);
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
const int32_t channel_num = AS_CHANNEL_MONO;
const int32_t bit_length = AS_BITLENGTH_16;
const int32_t sample_size = SAMPLE_SIZE;
const int32_t frame_size = sample_size * (bit_length / 8) * channel_num;
const uint64_t send_interval_ms = 1000ull;
const uint64_t sonar_recv_dulation_ms = (1000);
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
void AppliMain_setup()
{
int rc = 0;
int subid;
Serial.begin(115200);
while (!Serial);
INF_LOG("Guide Started!");
pinMode(PIN_RECV_LED, OUTPUT);
pinMode(PIN_SEND_LED, OUTPUT);
pinMode(PIN_MOTOR_LED, OUTPUT);
pinMode(PIN_FFT_LED, OUTPUT);
digitalWrite(PIN_RECV_LED, ONBOARD_LED_OFF);
digitalWrite(PIN_SEND_LED, ONBOARD_LED_OFF);
digitalWrite(PIN_MOTOR_LED, ONBOARD_LED_OFF);
digitalWrite(PIN_FFT_LED, ONBOARD_LED_OFF);
/* Boot SubCore */
for (subid = (e_core_id_main_app_recv + 1); subid < e_core_id_count; subid++) {
rc = MP.begin(subid);
if (rc < 0) {
ERR_LOG("MP.begin(%d) error = %d\n", subid, rc);
}
}
/* Polling */
MP.RecvTimeout(MP_RECV_POLLING);
// Initialize Signal Processor
SignalProcessor_setup();
}
void AppliMain_loop()
{
int rc;
int subid;
int8_t msg_recv_id;
msg_data_t *rcv_msg;
e_state_event_t state_event;
static uint8_t init_done_flags = 0;
/* Receive message from SubCore */
for (subid = (e_core_id_main_app_recv + 1); subid <= e_core_id_count; subid++) {
state_event = e_state_event_none;
rc = MP.Recv(&msg_recv_id, &rcv_msg, subid);
if (rc > 0) {
// INF_LOG("%d)%d-%lx\n", subid, msgid, rcv_msg->dummy32);
//DBG_LOG("Main recieved! %d-%d=%d", subid, msg_recv_id, rc);
switch(msg_recv_id) {
case e_msg_id_init_done:
init_done_flags |= rcv_msg->init_done.core_id_flags;
if ( init_done_flags != all_sub_core_flags) {
INF_LOG("Waiting Init flag = %x (%x/%x)", rcv_msg->init_done.core_id_flags, init_done_flags, all_sub_core_flags);
}
else {
INF_LOG("Init DONE!! flag = %x (%x/%x)", rcv_msg->init_done.core_id_flags, init_done_flags, all_sub_core_flags);
state_event = e_state_event_init_done;
}
break;
case e_msg_id_send_sonar_done:
state_event = e_state_event_send_end;
break;
default:
ERR_LOG("Invalid MsgID:%d", msg_recv_id);
break;
}
delay(1000);
}
if ( e_recv_state_done == recv_sonar_state ) {
state_event = e_state_event_send_start;
}
AppState_Main(state_event);
SignalProcessor_loop();
}
}
void AppState_Main(e_state_event_t state_event)
{
int rc;
msg_data_t msg_data_snd;
e_app_state_t next_app_state = e_app_state_none;
// uint64_t millis_now;
// 状態遷移チェック
switch(curr_app_state)
{
case e_app_state_failed_err:
// Nothing to do!
break;
case e_app_state_none:
if ( e_state_event_init_done == state_event ) {
next_app_state = e_app_state_idle;
}
break;
case e_app_state_inactive:
// Nothing to do!
break;
case e_app_state_idle:
if ( e_state_event_send_start == state_event ) {
next_app_state = e_app_state_sending_sonar;
}
#if 0
millis_now = millis();
if (millis_now >= ( last_send_time + send_interval_ms) ) {
next_app_state = e_app_state_sending_sonar;
}
#endif
break;
case e_app_state_sending_sonar:
if (e_state_event_send_end == state_event) {
next_app_state = e_app_state_idle;
}
break;
default:
// Nothing to do!
break;
}
// 状態変化時
if (next_app_state != e_app_state_none) {
// 各状態離脱時の処理
switch(curr_app_state)
{
case e_app_state_failed_err:
// Nothing to do!
break;
case e_app_state_none:
// Nothing to do!
break;
case e_app_state_inactive:
// Nothing to do!
break;
case e_app_state_idle:
// Nothing to do!
break;
case e_app_state_sending_sonar:
// Nothing to do!
break;
default:
// Nothing to do!
break;
}
// 状態遷移
DBG_LOG("[APP STATE] %d->%d", curr_app_state, next_app_state);
curr_app_state = next_app_state;
// 各状態突入時の処理
switch(curr_app_state)
{
case e_app_state_failed_err:
// Nothing to do!
break;
case e_app_state_none:
// Nothing to do!
break;
case e_app_state_inactive:
// Nothing to do!
break;
case e_app_state_idle:
break;
case e_app_state_sending_sonar:
//last_send_time = millis();
//send_start_time = last_send_time;
break;
default:
// Nothing to do!
break;
}
}
}
void SignalProcessor_setup()
{
arm_rfft_fast_init_f32(&S, SAMPLE_SIZE);
/* Initialize memory pools and message libs */
initMemoryPools();
createStaticPools(MEM_LAYOUT_RECORDINGPLAYER);
/* setup FrontEnd and Mixer */
theFrontEnd = FrontEnd::getInstance();
/* set clock mode */
theFrontEnd->setCapturingClkMode(FRONTEND_CAPCLK_HIRESO);
/* begin FrontEnd and OuputMixer */
theFrontEnd->begin(frontend_attention_cb);
INF_LOG("Setup: FrontEnd began");
/* activate FrontEnd and Mixer */
theFrontEnd->setMicGain(0);
theFrontEnd->activate(frontend_done_cb);
delay(100); /* waiting for Mic startup */
INF_LOG("Setup: FrontEnd activated");
/* Initialize FrontEnd */
AsDataDest dst;
dst.cb = frontend_pcm_cb;
theFrontEnd->init(channel_num, bit_length, sample_size, AsDataPathCallback, dst);
INF_LOG("Setup: FrontEnd initialized");
/* Unmute */
theFrontEnd->start();
INF_LOG("Setup: FrontEnd started");
}
void SignalProcessor_loop()
{
if (isErr == true) {
board_external_amp_mute_control(true);
theFrontEnd->stop();
theFrontEnd->deactivate();
theFrontEnd->end();
INF_LOG("Capturing Process Terminated by ERROR!");
while(1) {};
}
}
void frontend_attention_cb(const ErrorAttentionParam *param) {
ERR_LOG("ERROR: Attention! Something happened at FrontEnd");
if (param->error_code >= AS_ATTENTION_CODE_WARNING) isErr = true;
}
static bool frontend_done_cb(AsMicFrontendEvent ev, uint32_t result, uint32_t detail){
UNUSED(ev); UNUSED(result); UNUSED(detail);
return true;
}
static void frontend_pcm_cb(AsPcmDataParam pcm) {
static uint8_t mono_input[frame_size];
static uint8_t stereo_output[frame_size*2];
static const bool time_measurement = false;
if (time_measurement) {
static uint32_t last_time = 0;
uint32_t current_time = micros();
uint32_t duration = current_time - last_time;
last_time = current_time;
INF_LOG(("duration = " + String(duration)).c_str());
}
frontend_signal_input(pcm, mono_input, frame_size);
signal_process((int16_t*)mono_input, (int16_t*)stereo_output, sample_size);
return;
}
void frontend_signal_input(AsPcmDataParam pcm, uint8_t* input, uint32_t frame_size) {
/* clean up the input buffer */
memset(input, 0, frame_size);
if (!pcm.is_valid) {
WRN_LOG("WARNING: Invalid data! @frontend_signal_input");
return;
}
if (pcm.size > frame_size) {
WRN_LOG("WARNING: Captured size is too big! {");
WRN_LOG(String(pcm.size).c_str());
WRN_LOG("} @frontend_signal_input");
pcm.size = frame_size;
}
/* copy the signal to signal_input buffer */
if (pcm.size != 0) {
memcpy(input, pcm.mh.getPa(), pcm.size);
} else {
WRN_LOG("WARNING: Captured size is zero! @frontend_signal_input");
}
}
void signal_process(int16_t* mono_input, int16_t* stereo_output, uint32_t sample_size) {
// 処理開始時間
uint64_t current_time = micros();
int rc;
msg_data_t msg_data_snd;
static uint64_t recv_start_time = 0;
static uint64_t signal_start_time = 0;
static uint64_t signal_end_time = 0;
static float angle_sum = 0;
static uint16_t angle_count = 0;
static const int targetIndexMin = (int)((float)(sonar_params.recv_params.sonar_freq - sonar_params.recv_params.sonar_accept_size_freq) / sonar_params.recv_params.freq_bandwidth_per_sample); // 最小周波数インデックス
static const int targetIndexMax = (int)((float)(sonar_params.recv_params.sonar_freq + sonar_params.recv_params.sonar_accept_size_freq) / sonar_params.recv_params.freq_bandwidth_per_sample); // 最大周波数インデックス
const uint64_t minDuration = sonar_params.recv_params.sonar_duration_us - sonar_params.recv_params.sonar_accept_size_duration_us;
const uint64_t maxDuration = sonar_params.recv_params.sonar_duration_us + sonar_params.recv_params.sonar_accept_size_duration_us;
const float micDistance = (sonar_params.common.speed_of_sound / sonar_params.recv_params.sonar_freq) / 2.0; // 波長の1/2
// 変数をスイッチの外で宣言
float angleLeft = 0.0f, angleRight = 0.0f, phaseDiff = 0.0f;
float timeDiff = 0.0f, angle = 0.0f;
uint64_t duration = 0;
if (curr_app_state != e_app_state_idle) {
//DBG_LOG("Not Ready Sig Process!");
recv_sonar_state = e_recv_state_inactive;
return;
}
digitalWrite(PIN_FFT_LED, ONBOARD_LED_ON);
static float monoSrc[SAMPLE_SIZE]; // モノラルチャネル信号
//static float rightChannel[SAMPLE_SIZE]; // 右チャネル信号
static float fftMono[SAMPLE_SIZE]; // 左チャネルFFT結果
//static float fftRight[SAMPLE_SIZE]; // 右チャネルFFT結果
static float monoDst[SAMPLE_SIZE];
#if 0
for (int i = 0; i < SAMPLE_SIZE; i++) {
monoSrc[i] = mono_input[i * 2]; // 左チャネル
//rightChannel[i] = mono_input[i * 2 + 1]; // 右チャネル
}
/*
for (int i = 0, n = 0; n < SAMPLE_SIZE; ++n) {
monoSrc[n] = (float)mono_input[i++];
}
*/
#else
// PCMデータを浮動小数点形式に変換
arm_q15_to_float(&mono_input[0], &monoSrc[0], SAMPLE_SIZE);
#endif
arm_rfft_fast_f32(&S, &monoSrc[0], &fftMono[0], 0);
float maxSigLevel;
uint32_t peakIndex;
arm_cmplx_mag_f32(&fftMono[0], &monoDst[0], SAMPLE_SIZE / 2);
//arm_cmplx_mag_f32(fftRight, fftRight, SAMPLE_SIZE / 2);
arm_max_f32(&monoDst[0], SAMPLE_SIZE / 2, &maxSigLevel, &peakIndex);
//arm_max_f32(fftRight, SAMPLE_SIZE / 2, &maxSigLevelRight, &peakIndexRight);
bool inRange = (peakIndex >= targetIndexMin && peakIndex <= targetIndexMax);
//DBG_LOG("inRange = %d", (int)inRange);
#if 0 // for debug
static uint64_t last_dbg_timestamp = 0;
if ( current_time > (last_dbg_timestamp + 200000ll) ) {
#if 1
DBG_LOG("InRange = %d (%d <= %d <= %d)", inRange, targetIndexMin, peakIndex, targetIndexMax);
#endif
#if 0
DBG_LOG("INDEX %d,%d FREQ %10.2f,%10.2f", peakIndexLeft, peakIndexRight, (float)peakIndexLeft * df, (float)peakIndexRight * df);
#endif
#if 0
Serial.printf("FFT:");for (int i = 0; i< 32; i++) Serial.printf("%f%c", fftMono[i+0], ((i==31)? '\n': ','));
#endif
#if 0
Serial.printf("DST:");for (int i = 0; i< 32; i++) Serial.printf("%f%c", monoDst[i+0], ((i==31)? '\n': ','));
#endif
last_dbg_timestamp = current_time;
}
#endif // for debug
switch (recv_sonar_state) {
case e_recv_state_inactive: // 受信処理初回
recv_start_time = current_time;
recv_sonar_state = e_recv_state_idle;
break;
case e_recv_state_idle: // 40K超音波未検出
if (inRange) {
DBG_LOG("SIG Started");
signal_start_time = current_time;
angle_sum = 0;
angle_count = 0;
recv_sonar_state = e_recv_state_in_range;
}
break;
case e_recv_state_in_range: // 40K超音波検出済み
#if 0
if (inRange) { // 検出継続
// 角度計算
//angleLeft = atan2f(fftLeft[peakIndexLeft * 2 + 1], fftLeft[peakIndexLeft * 2]);
//angleRight = atan2f(fftRight[peakIndexRight * 2 + 1], fftRight[peakIndexRight * 2]);
phaseDiff = angleRight - angleLeft;
if (phaseDiff > M_PI) phaseDiff -= 2 * M_PI;
if (phaseDiff < -M_PI) phaseDiff += 2 * M_PI;
timeDiff = phaseDiff / (2 * M_PI * sonar_params.recv_params.sonar_freq);
angle = asin(sonar_params.common.speed_of_sound * timeDiff / micDistance) * 180.0 / M_PI;
angle_sum += angle;
angle_count++;
}
#endif
if (!inRange) { // 検出離脱時
signal_end_time = current_time;
duration = signal_end_time - signal_start_time;
// 長さが範囲内なら決定し、方向と距離を計算
if (duration >= minDuration && duration <= maxDuration) {
// float avgAngle = angle_sum / angle_count;
float distance = (sonar_params.common.speed_of_sound * duration) / 2000000.0;
INF_LOG("[RECV DONE] Distance=%5.2f", distance);
/* Start periodic Sending SONAR */
rc = MP.Send((int8_t)e_msg_id_send_sonar, &msg_data_snd, (int)e_core_id_sub_sonar_send);
if (rc < 0) {
ERR_LOG("MP.Send error = %d\n", rc);
}
recv_sonar_state = e_recv_state_done;
} else {
DBG_LOG("Invalid signal duration = %llu", duration);
recv_sonar_state = e_recv_state_idle;
}
}
break;
case e_recv_state_done:
recv_sonar_state = e_recv_state_idle;
break;
default:
recv_sonar_state = e_recv_state_inactive;
break;
}
digitalWrite(PIN_FFT_LED, ONBOARD_LED_OFF);
}
// ########################################
// ## Sub1 Core Part <Sonar Sender> ##
// ########################################
#elif (SUBCORE == 1)
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
#include <sys/ioctl.h>
#include <stdio.h>
#include <fcntl.h>
#include <nuttx/timers/pwm.h>
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
#define SonarSender_setup setup
#define SonarSender_loop loop
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
int fd;
struct pwm_info_s info;
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
void SonarSender_setup(void);
void SonarSender_loop(void);
void SendOneShotSonar(void);
//void send_signal(uint16_t output_hz);
//void encode(uint8_t c);
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
const e_core_id_t self_core_id = e_core_id_sub_sonar_send;
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
void SonarSender_setup(void)
{
int rc;
msg_data_t msg_data_snd;
MP.begin();
MP.RecvTimeout(MP_RECV_POLLING);
fd = open("/dev/pwm0", O_RDONLY);
msg_data_snd.init_done.core_id_flags = (uint8_t)(1 << self_core_id);
rc = MP.Send((int8_t)e_msg_id_init_done, &msg_data_snd);
if (rc < 0) {
ERR_LOG("MP.Send(%d) error = %d\n", e_msg_id_init_done, rc);
}
#if 0//defined(KEEP_SENDING_SONAR_MODE)
info.frequency = sonar_params.send_params.sonar_freq; // 40kHz
info.duty = 0x7fff;
// Start Sonar send
ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)((uintptr_t)&info));
ioctl(fd, PWMIOC_START, 0);
#endif // defined(KEEP_SENDING_SONAR_MODE)
}
void SonarSender_loop(void)
{
int rc;
int subid;
int8_t msg_recv_id;
msg_data_t *msg_recv_dat;
for (subid = 0; subid < e_core_id_count; subid ++)
{
if ( subid == self_core_id) continue;
else if ( subid == 0 ) rc = MP.Recv(&msg_recv_id, &msg_recv_dat);
else rc = MP.Recv(&msg_recv_id, &msg_recv_dat, subid);
if (rc > 0) {
//DBG_LOG("Sub1 recieved! %d-%d=%d", subid, msg_recv_id, rc);
switch(msg_recv_id) {
case e_msg_id_send_sonar:
SendOneShotSonar();
break;
default:
ERR_LOG("Invalid MsgID:%d", msg_recv_id);
break;
}
delay(1000);
}
//else ERR_LOG("MP.Recv(%d) ret=%d", subid, rc);
}
}
void SendOneShotSonar(void)
{
int rc;
msg_data_t msg_data_snd;
digitalWrite(PIN_SEND_LED, ONBOARD_LED_ON);
// DBG_LOG("Send SONAR>");
info.frequency = sonar_params.send_params.sonar_freq; // 40kHz
info.duty = 0x7fff;
// Start Sonar send
ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)((uintptr_t)&info));
ioctl(fd, PWMIOC_START, 0);
delayMicroseconds(sonar_params.send_params.sonar_duration_us);
// Stop Sonar send
ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)((uintptr_t)&info));
ioctl(fd, PWMIOC_STOP, 0);
digitalWrite(PIN_SEND_LED, ONBOARD_LED_OFF);
rc = MP.Send((int8_t)e_msg_id_send_sonar_done, &msg_data_snd);
if (rc < 0) {
ERR_LOG("MP.Send error = %d\n", rc);
}
}
// ########################################
// ## Sub2 Core Part BleDebug> ##
// ########################################
#elif (SUBCORE == 2)
#define BleDebug_setup setup
#define BleDebug_loop loop
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
#define ARRAY_SIZE 128
#define LOG_BUFFER_SIZE 256
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
const e_core_id_t self_core_id = e_core_id_sub_bt_dbg;
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
void BleDebug_setup()
{
int rc;
msg_data_t msg_data_snd;
// Initialize BLE Serial
Serial2.begin(115200);
// if (!Serial2.begin(115200)) {
// ERR_LOG("BLE initialization failed");
// return;
// }
INF_LOG("BLE initialized successfully");
INF_LOG("BLE connected");
MP.begin();
msg_data_snd.init_done.core_id_flags = (uint8_t)(1 << self_core_id);
rc = MP.Send((int8_t)e_msg_id_init_done, &msg_data_snd);
if (rc < 0) {
ERR_LOG("MP.Send(%d) error = %d\n", e_msg_id_init_done, rc);
}
}
void BleDebug_loop()
{
int rc;
int subid;
int8_t msg_recv_id;
msg_data_t *msg_recv_dat;
uint64_t millis_now;
for (subid = 0; subid < e_core_id_count; subid ++)
{
if ( subid == self_core_id) continue;
else if ( subid == 0 ) rc = MP.Recv(&msg_recv_id, &msg_recv_dat);
else rc = MP.Recv(&msg_recv_id, &msg_recv_dat, subid);
if (rc > 0) {
//DBG_LOG("Sub2 recieved! %d-%d=%d", subid, msg_recv_id, rc);
switch(msg_recv_id) {
default:
ERR_LOG("Invalid MsgID:%d", msg_recv_id);
break;
}
delay(1000);
}
//else ERR_LOG("MP.Recv(%d) ret=%d", subid, rc);
}
}
// ########################################
// ## Sub4 Core Part ##
// ########################################
#elif (SUBCORE == 3)
#error "Core-3 Not Used!"
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
// ########################################
// ## Sub5 Core Part ##
// ########################################
// ########################################
// ## Sub4 Core Part ##
// ########################################
#elif (SUBCORE == 4)
#error "Core-4 Not Used!"
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
// ########################################
// ## Sub5 Core Part ##
// ########################################
#elif (SUBCORE == 5)
#error "Core-5 Not Used!"
// -----=====<<<<<[[[[[ Include ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Define ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Types ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Valiable ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Class ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Prototype ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Tables ]]]]]>>>>>=====-----
// -----=====<<<<<[[[[[ Functions ]]]]]>>>>>=====-----
#endif // !defined(SUBCORE)ソフト周りの説明
マルチコア回り
基本的に送信も受信も分けた方がそれぞれの処理がもたつかずに、超音波送受信を乱さないと思ったので以下の様な割り当てでマルチコア構成にしています。
各コアをまるでマルチスレッドの様に使い、MP.Send() や MP.Recv() 関数を使ってコア間通信でメッセージ送受信し同期をとりつつ動いています。
[誘導機]
メインコア：超音波受信 ＋ 信号処理(FFT 等)
サブコア１：超音波送信処理
サブコア２：デバッグ用（ほぼ未使用）
[追っかけロボット]
メインコア：超音波受信 ＋ 信号処理(FFT 等)
サブコア１：超音波送信処理
サブコア２：モーター制御処理。
SPRESENSE はコアが増えると、Arduino IDE をそのコアの数分開く必要があるのがうっとおしかったので、誘導機 / 追っかけロボット それぞれ、一つずつ IDE を開けばいいように統合しています。
内部的にコンパイルスイッチ（条件コンパイル）で、メニューから [ツール] - [Core] - [Core＊] で選んだコアのコンパイルが掛かり書き込みが出来る様になっています。
ですので、Arduino IDE を一度開いたら
- [ツール] - [Core] - [Main Core] を選んで [書き込み]
- [ツール] - [Core] - [Sub2 Core] を選んで [書き込み]
- [ツール] - [Core] - [Sub3 Core] を選んで [書き込み]
の手順で書き込めるようになっています。
超音波送信制御周り
受信処理側は前回のコンテストである程度経験があったのですが、超音波の送信周りは高速で 40KHz の PWM を制御するのかといったあたりが判らず、
信号処理のセミナー資料
の超音波通信周りを参考にしました。
SPRESENSE Arduino は Nuttx という POSIX系 OSをくるんで出来ているらしく、 POSIX によくあるファイルシステムにデバイスを関連付けることで制御できるようです。
ioctrl というNuttx の機能を使えば、高速の PWM 制御が使えるとの事で使用させていただきました。
超音波受信回り
ハイレゾでステレオサンプリングして FFT を流すというサンプルプログラムが無く、リファレンスの様な情報も少ないため、ステレオデータをどう扱うのかがずっとわからず苦労しました。
試行錯誤しているうちに時間が溶けた感じです。
もっと早く始めたかったのですが、超音波送受信素子が１セットしかなく AliExpress に注文した分が１月半ばまで到着しないというトラブルがありすっかり遅れてしまいました。（苦しい言い訳…！）
誘導機側のモノラルのセットでは結構すんなり制御出来たので油断してしまいました。ステレオになったとたん訳が分からない動きになって苦労しました。そしてまだ解決できていません。（もう一回苦しい言い訳…！）
また超音波受信の時に、壁などに当たって反射した自分の出した超音波を誤認識しない様に、送信期間を区別しました。この期間も調整が必要で、あまり長すぎると、近距離が探知できませんし、短すぎても上手くいきませんでした。
また、送信直後からサンプリングをし始めると、自分が出した超音波を拾ってしまうらしくうまくいきません。送信直後は受信を無視する期間が必要なのと、その長さのチューニングが必要そうです。
BLE周り
追っかけロボットは移動するので、シリアルログ等は BLE にて無線で送ってデバッグしようと試みましたが、いざ使おうとすると 何故か PC でもスマホでもタブレットでもいずれでもペアリングできず、後で他が片付いてから再調査としていたら時間切れになってしまって使えていません。
結局レベル変換が必要だった分遠回りしてしまったのかもしれません。サブ基板でやればレベル変換も不要ですし・・・
衝突防止
超音波に干渉しない光学式の i2c 距離センサを使って、距離測定とは別に障害物への衝突防止をと考えていましたが、手が回らず未使用です。
まとめ
残課題
・ステレオ信号の FFT がうまくいっていない。
・方向判定という基本的な動作すらまだ未完成。
（位相ズレと、レベル差、信号の遅れなどをハイブリッドに組み合わせて導き出すひつようがあると感じている）
・超音波送受信のアンプをもっと整備して距離を稼ぐという辺りのチューニングが出来ていない。
反省点
メカ、ハード、ソフト、全てが実績のない物をやったのが一番問題だったかもしれません。
メカなんか、２輪型のロボットはそこらじゅうで入手可能なので、出来上がっている物をベースにすればよかったと。
ハードにもう少し時間を掛けたかった。フィルターまわり、アンプまわりを整備することで距離を稼ぐという辺りは、基本的な動作がうまくいってからチューニングフェーズで頑張ると決めていたため、そこまで全くいきませんでした。
今回は色々試行錯誤のすえ解決せず途中で断念せざるを得ない状況でした。
やり方（使用技術や解決法のアプローチ）を変える必要がありそうですが、部品提供モニターを申告したネタを変えないことという縛りがあるので、
作戦変更できませんでした。（まだまだ苦しい言い訳！）
※この縛りは次回以降ちょっと考慮頂きたいところが正直なところです。
（「申提供部品を使って実現できずに申告したやり方と大幅に異なった場合はその理由を記事に書くこと」くらいの縛りであれば助かります）
大阪在住の本業組込み系SEです。ハードに近いところのソフト屋さんです。
電子工作は趣味でやっています。
よろしくお願いします。
▼こちらにも別の作品を投稿しています。参考まで。
https://protopedia.net/prototyper/taksan
投稿者の人気記事
-
TakSan0
さんが
2025/01/31
に
編集
をしました。
(メッセージ: 初版)
-
TakSan0
さんが
2025/01/31
に
編集
をしました。
-
TakSan0
さんが
2025/01/31
に
編集
をしました。
ログインしてコメントを投稿する