目的

本記事では、ARLISS 2025に参加し、SONY SPRESENSEを用いたCanSatシステムを製作・打ち上げした取り組みについて紹介します。

本チームでは、浮遊ローバ（A-PARTS）と車輪ローバ（B-PARTS）が連携して動作する2機体システムというミッションを立てて参加しました。浮遊ローバはダクテッドファンを動力として動作し、車輪ローバが走行するための安全なルートを広範囲に偵察する機体です。車輪ローバは、浮遊ローバの後方から安全なルートを走行し、センサで詳細な地形データを取得する機体です。最終的に、浮遊ローバと車輪ローバの2機がお互いに状態を送受信し、連携動作を行うことが目標となります。

本システムの開発にあたっては、100kinSAT（ヒャッキンサット）を参考にし、SONY SPRESENSEマイコンを中心としたハードウェア構成を採用しました。100kinSATの設計思想を基に、機体間連携・物体検出・動画撮影などの独自機能を追加実装しています。

この記事では、特に 車輪ローバ（B-PARTS） に焦点を当て、ハードウェア設計・回路構成・ソフトウェア実装など、SPRESENSEマイコンを用いた開発の全体像について詳しく解説します。

ARLISSとは

ARLISS（アーリス）は、アメリカのネバダ州にあるブラックロック砂漠で毎年9月に開催されるCanSatのサブオービタル（大気圏内）打上げ実証実験です。
国内外から大学生を中心に参加しており、アマチュアロケット団体（AeroPAC）の協力のもとCanSatを打ち上げ、様々なミッションを行います。
競技会としての側面もあり、CanSatをゴールまで正確に制御したチームに送られる ”Accuracy Award”や優れたミッションに送られる ”Best Mission Award”などの賞が設けられています。

CanSat/100kinSATとは

CanSat（カンサット）は「Can（缶）」と「Satellite（サテライト、衛星）」を組み合わせた造語で、CanSatと呼ばれる小型模擬人工衛星による惑星探査を模した競技です。
CanSatの製作を通して、実際の宇宙開発で行われているような衛星モデルの開発を学ぶことができます。
そして、100kinSAT（ヒャッキンサット）は、衛星モデルの開発を学ぶCanSat競技の初心者向け開発キットです。

システム構成

100kinSATは「制御系」「ミッション系」「電源系」「通信系」「構造系」から構成されます。

制御用マイコンとして、SONYのSPRESENSEを使用しています。

そして、ミッション遂行のためにさまざまなセンサーや駆動部品を搭載しています。

システム構成のブロック図を以下に示します。

部品

部品名	値段 [円]	個数
SONY SPRESENSE メインボード	6050	1
SONY SPRESENSE カメラモジュール	3850	1
スライドスイッチ 1回路2接点基板用横向き	25	1
XHコネクターベース付ポストトップ型 2P B2B-XH-A(LF)(SN)	10	1
コネクター付コード 2P(D) 赤黒	50	1
DIPスイッチ 2P	70	1
トワイライトワイヤレスモジュール TWE-Lite アンテナ別付けタイプ	1450	1
ターミナルブロック 2.54mm 2P 緑縦	25	1
基板取付用小型ダイナミックスピーカー UGCM0603APE	60	1
TB6612モータードライバーブレークアウトボードキット	380	1
FEETECH ギアードモーター FM90	330	2
BNO055使用 9軸センサーフュージョンモジュールキット	2450	1
CdSセル(1MΩ)GL5528	100	1
低損失表面実装型三端子レギュレーター 3.3V800mA NJM2845DL1-33	50	1
低損失表面実装型三端子レギュレーター 5V800mA NJM2845DL1-05	50	1
電池ボックス単4×1本ピン	40	4
ピンヘッダー 1×40 (40P)	35	1
ジャンパーピン黒(2.54mmピッチ)	100	1
分割ロングピンソケット 1×42 (42P)	80	1

3Dプリント部品

3Dデータ、回路図はこちらにあります。

3Dプリント部品は全てSOLIDWORKSにて設計しBambu Labを使用してプリントします。

カメラマウントとモータマウントは、従来の100kinSATのものをそのまま利用しています。

メジャータイヤ

砂漠の悪路を走破するには、ある程度大きなタイヤが必要です。しかし、3Dプリンタで大きなタイヤを作ると、キャリアに収納できないという問題があります。また、3Dプリント製のタイヤは砂地でのグリップ力や耐久性にも課題があります。

そこで、市販の巻尺（メジャー）を素材としたタイヤを採用しました。メジャーは押し込んで畳むことで小さく収納でき、ケースが展開されると自身の弾性で自動的に元の形状に戻ります。これにより、モーターなどの展開機構を必要とせず、大きな接地面積を確保できるため、悪路でも走破可能な設計を実現しています。
メジャー部には、駆動系からのトルクと路面からの反力の双方が作用するため、特定箇所への応力集中を回避する目的で、メジャー端部を円弧状に加工しました。また、メジャーとホイールの固定には、汎用性およびメンテナンス性を考慮し、結束バンドを使用しています。

メジャーとモータを接続するホイールユニットは、「メジャーを固定するパーツ」と「動力を伝達するパーツ」の二点で構成されています。
固定用パーツは、軽量化および小型化を最大限に図るため、メジャーを保持するための最小限の縁（フチ）のみを残した形状としました。
メジャーとホイールの接合点は、同心円状に配置しつつも、内側の円に対して約45度オフセットさせています。これにより、メジャーが路面を効率的に「掻く」動作となり、不整地における高い走行性能を実現しています。

さらに、タイヤおよび後述するスタビライザの構成要素としてメジャーを採用することで、機構全体の折りたたみ性が向上し、機体を大幅にコンパクト化することが可能となりました。

ARLISSでは、キャリア内部（直径146mm、高さ240mm）に収納可能なサイズであることが求められる収納試験が実施されます。本機体は折りたたみ時に直径100mm、高さ180mmまでコンパクト化でき、この制約を十分にクリアしています。

以下は車輪ローバの寸法図（平面・右側面）と、メジャーを折りたたんだ状態の実物写真です。実際にキャリアに収納する際は、メジャーが展開しないよう押さえておくために、写真のようにクリアファイルで覆っています。

特に、我々のチームでは**浮遊ローバ（A-PARTS）と車輪ローバ（B-PARTS）**の二機を同時に収納する必要がありました。浮遊ローバは翼を折りたたむ機構を備えていますが、それでも車輪ローバより大きいサイズとなります。メジャーによる折りたたみ機構のおかげで車輪ローバを極限までコンパクト化でき、二機同時収納を実現しました。

スタビライザ

本機のような平行二輪駆動方式では、作用・反作用の法則により、車輪を回転させるトルクに対して、本体が逆方向に回転しようとする課題があります。
この問題を解決するため、スタビライザ機構を実装しました。本機構により、本体の不要な回転を抑制し、安定した走行を可能としています。

基板

100kinSATの基板は、100mm x 50mmの2層プリント基板2枚（トップ基板（画像右）とボトム基板（画像左））で構成されています

2枚の基板を4個のスペーサで固定することで、機体の本体構造も兼ねた設計となっています

KiCADを用いて設計し、JLCPCBのPCBAサービスを活用して制作しています

トップ基板側には、メインとなるSPRESENSEマイコンと周辺回路（電源レギュレータ、モータドライバ、9軸センサ、照度センサ、無線通信、microSDカードスロット、圧電ブザー、電熱線）が実装されています

ボトム基板側には、電源となる単四電池4本分の電子回路とモータ・カメラ部品の取り付け穴が実装されています

回路図

トップ基板の回路は、SPRESENSE拡張ボード+αというような構成になっています

GPIOの電圧レベル変換やmicroSDカードとのSDIO接続回路はSPRESENSE拡張ボードの回路図を参考にしました

100kinSATに使用しないマイク等の回路は省略しています

電源回路

単四電池4本を直列に接続した6V電源からレギュレータを通して5Vと3.3Vを生成しています

5V電圧はSPRESENSE本体へ、3.3V電圧はIOのレベル変換やセンサなどへ供給します

GPIOレベル変換

SPRESENSEのGPIOの電圧レベルは1.8Vとなっています。

今回使用したセンサ類は3.3Vで駆動するため、電圧レベル変換ICを搭載しています。

モータ制御

今回のミッションにおいて100kinSATは、浮遊ローバ（A-PARTS）が走行する様子を撮影します。

撮影に最適なポジションへ移動するために、2つのタイヤが搭載されています。

これらのタイヤはモータで駆動しますが、SPRESENSEのGPIOに流れる電流量ではモータを駆動させることはできません。

そこで、デュアルモータドライバICを使うことでSPRESENSEの少ない電流量でも2つのモータを制御・駆動できるようにしています。

無線通信

浮遊ローバ（A-PARTS）との連携や地上局に見立てたノートパソコンとの無線通信を行うためにTWE-Liteモジュールを搭載しています。

TWE-Liteは2.4GHz帯の電波を使って無線通信を行うことができるモジュールです。

100kinSATでは、表面実装のモジュールとピンヘッダのモジュールのいずれかを選択して実装できるようにしています。

これは100kinSATが教育目的として初学者向けに設計しているため、はんだ付けに不慣れな人は実装が難しい表面実装部品ではなく、ピンヘッダのモジュールで簡単に無線通信機能を扱えるようにするための工夫となっています。

また、TXとRXのUART配線の途中にスライドスイッチを設けています。これは、TWE-Liteのファーム書き込み時にスイッチをオフにすることで、SPRESENSE側のUARTプログラムがファーム書き込みへ影響を与えないようにするための工夫です。

9軸センサ

100kinSATの姿勢情報を取得するために9軸センサを搭載しています。

今回利用したBNO055はマイコン搭載の9軸センサとなっていて、センサ側のマイコンがセンサフュージョンによる高精度な姿勢情報を計算して出力してくれるため、ソフトウェアの実装が簡単になる利点があります。

照度センサ

照度センサによって、ロケット格納時は周囲が暗い、放出後は明るいといった状態変化を検知することができます。

9軸センサ（加速度センサ）と組み合わせることで、着地の判定をより安全にすることも可能です。

スピーカ

スピーカを搭載することでエラー発生時や制御モード切り替え時に音で動作状況を判断できるため、開発時や本番でのステータス確認に便利となっています。

また、音が鳴ったほうがテンションが上がります。

microSDカード

CanSat競技では「制御履歴」を記録し、期待通りの動作を行えたか検証する必要があります。

SPRESENSEとmicroSDカードをSDIOで接続して、ファイルの読み書きができるようにしています。これにより、制御履歴をmicroSDカードへ保存できるようになっています。また、カメラで撮影したデータもmicroSDへ保存しています。

SDIOは高速に通信するため、各信号の配線長が等しくなるように工夫しています。

SPRESENSEのIOは1.8Vのため、SDIOの電圧レベル変換ICを実装しています。

ソフトウェア設計

本機体のソフトウェアは、保守性と拡張性を重視した設計になっています。

State Patternによるミッション状態管理

本機体のソフトウェアは、 State Pattern（状態パターン） を採用して設計されています

State Patternとは

State Patternは、オブジェクトの内部状態に応じて振る舞いを変更するためのデザインパターンです。状態ごとにクラスを分離することで、以下のメリットがあります

条件分岐の削減: if-elseやswitch文による状態判定が不要になる

状態ごとの責務分離: 各状態のロジックが独立したクラスに閉じ込められる

拡張性: 新しい状態の追加が既存コードに影響を与えにくい

可読性: 状態遷移の流れが明確になる
CanSatのミッションでは、「センサ初期化」「発射待機」「放出検知」「降下」「脱出」「検出」「録画」「探索」といった複数のフェーズを順次実行する必要があります。State Patternを採用することで、各フェーズの処理を独立したクラスとして実装し、状態遷移を明確に管理できます。

参考リンク

State - Refactoring.Guru - State Patternの概念と実装例

Stateパターン - Wikipedia - デザインパターンとしての解説

ミッション状態遷移

本機体は以下の8つの状態を順次遷移しながらミッションを遂行します：


CALIBRATION → STANDBY → LAUNCH → DROP → ESCAPE → DETECTION → RECORDING → EXPLORE

各状態の役割：

CALIBRATION: センサ初期化・キャリブレーション
STANDBY: ロケット発射前の待機
LAUNCH: ロケットからの放出検知（CdSセンサ使用）
DROP: パラシュート降下中の監視
ESCAPE: 着陸容器からの脱出（4種類の動作パターン）
DETECTION: A-PARTS物体検出（Edge Impulse使用）
RECORDING: A-PARTS発射動画の撮影（30秒間）
EXPLORE: 自律移動・周辺撮影

State Patternの実装

各状態はICansatStateインターフェースを実装し、以下の3つのライフサイクルメソッドを持ちます：

onEnter(): 状態開始時の初期化処理（1回のみ実行）
onUpdate(): 状態のメインループ処理（200msごとに周期実行）
onExit(): 状態終了時の後処理（次の状態への遷移準備）

状態遷移の流れは以下のようになります

この設計により、各状態の責務が明確になり、新しい状態の追加や既存状態の変更が容易になっています。また、状態ごとにテストを行いやすくなるというメリットもあります。

データ管理と設定の工夫

クラッシュリカバリ機能

電源断や予期せぬリセットが発生した場合でも、前回の状態から再開可能な設計になっています。

SD+Flash二重保存: 現在の状態番号をstate.txtとしてSDカードとFlashメモリの両方に保存
- SDカードの破損や脱落といったトラブルに備え、Flashメモリにもバックアップ
- SDカードが使用できない状況でも、Flashメモリから状態を復元して最低限の動作を継続
- 電源の瞬断による再起動が発生しても、保存された状態から復帰することで、ロケットからの放出判定を再度行わないように制御
起動時の状態復元: begin()実行時に保存された状態を読み取り、該当する状態から再開
状態遷移時の自動保存: onExit()で次の状態を保存し、確実な状態管理を実現

センサデータバッファリング

センサログを効率的にSDカードへ書き込むため、バッファリング機構を実装しています。

4KBリングバッファ: センサデータをメモリ上に一時蓄積
バッファ満杯時の自動フラッシュ: バッファが満杯になると自動的にSDカードに書き込み
200ms周期のデータ収集: GNSS、IMU、CdS、電圧などのセンサ値をCSV形式で記録

JSON設定ファイル

ミッションパラメータを再コンパイル不要で調整可能なconfig.jsonを実装しています。

実装の背景

打ち上げ前には、ネバダ州ブラックロック砂漠の現地で最終調整を行います。砂漠という過酷な環境下では：

砂埃によるPCの故障リスク
強風や直射日光による作業の困難さ
限られた時間内での調整の必要性

このため、パラメータを変更するたびにプログラムを書き込み直すのは非常に手間がかかります。JSON設定ファイルを採用することで、テキストエディタでの編集とSDカードの差し替えだけで調整が完了します。

機能

タイムアウト値の設定: 各状態のタイムアウト時間を個別に設定可能
閾値の設定: CdSセンサの閾値、検出失敗回数の上限などを調整可能
デフォルト値のフォールバック: ファイルが存在しない場合はコード内のデフォルト値を使用

実際のログデータ

実際に記録されたログデータの例を以下に示します。

システムログ（system_log.log）の例 - 状態遷移の記録


CansatController: begin() started
Power On (Supply)

=== Configuration Dump ===
[Calibration] Timeout: 300000
Timeout: 3600000
[Launch] CdS: 400, Timeout: 600000
[Drop] Timeout: 1500000
[Escape] Timeout: 20000
[Detection] MaxFailedCount: 50
[Recording] Timeout: 5000, Time: 600000
=== End of Configuration Dump ===

CansatController: GNSS initialized successfully
CansatController: BNO055 initialized successfully
CansatController: begin() finished
timestamp: log
5538: config CalibrationState
5544: Entering CalibrationState
7582: Exiting CalibrationState
...（省略）...
5128934: Exiting EscapeState
5131332: Entering DetectionState
5132592: Detection mode set
5132598: Succeed to initialize Edge Impulse
5132806: HeartBeat
...（省略：約10秒間隔で生存確認のためのHeartBeat）...
5235936: Exiting DetectionState
5236625: Entering RecordingState
5238165: Recording mode set
5243343: Timeout. Recording start
5243349: Recording started
AVI recording started successfully
...（省略）...
5843421: Recording finished successfully. Duration: 600000 ms, Frames: 600041
5843428: Finished recording successfully. Changing to ExploreState
5843437: Exiting RecordingState
5844133: Entering ExploreState
5845392: Explore mode set

センサログ（sensor_log.csv）の例


time,date,mode,lat,lng,alt,cds,ax,ay,az,gx,gy,gz,mx,my,mz,roll,pitch,heading,voltage
7561,,0,0.000000,0.000000,0.00,358,-4.70,-0.34,8.66,0.69,1.31,-0.88,23.38,-69.88,-12.19,-28.50,-2.13,358.38,4029
8076,,1,0.000000,0.000000,0.00,306,-4.69,-0.33,8.62,-0.44,0.13,-0.50,23.06,-70.56,-9.56,-28.44,-2.13,358.44,4029
8289,,1,0.000000,0.000000,0.00,290,-4.81,-0.32,8.66,9.50,-1.38,5.38,22.88,-69.69,-9.19,-28.56,-1.81,358.06,4029
...（省略）...
192621,2025/09/12 15:19:25Z,1,40.868755,-119.103920,1199.87,237,-2.57,0.61,9.54,0.06,0.31,0.06,19.56,45.19,-26.69,-15.88,3.31,118.88,4028
192835,2025/09/12 15:19:26Z,1,40.868755,-119.103920,1199.87,233,-2.60,0.60,9.50,0.00,0.06,0.06,19.56,44.50,-26.38,-15.88,3.31,118.88,4029
193047,2025/09/12 15:19:26Z,1,40.868755,-119.103920,1199.87,224,-2.55,0.60,9.54,-0.06,0.06,0.00,19.19,44.50,-26.69,-15.88,3.31,118.88,4029
193258,2025/09/12 15:19:26Z,1,40.868755,-119.103920,1199.87,210,-2.59,0.59,9.48,0.06,0.88,-0.06,18.88,44.75,-26.69,-16.00,3.31,118.88,4029

センサログには、200ms周期でGNSS（緯度・経度・高度）、9軸IMU（加速度・ジャイロ・地磁気・姿勢）、CdS照度センサ、バッテリー電圧などが記録されます。起動直後はGPS未測位のため座標が0.000000ですが、測位後はブラックロック砂漠の実際の座標（40.868755°N, -119.103920°W）が記録されています。

Edge Impulseによる物体検出

DETECTION状態では、Edge Impulseを用いたエッジMLで物体検出を実装しています。

Edge Impulseとは

Edge Impulseは、組み込みデバイス向けの機械学習開発プラットフォームです。通常、機械学習モデルの開発には専門的な知識と複雑な環境構築が必要ですが、Edge Impulseを使うことで以下のことが簡単にできます：

データ収集: センサデータや画像をWebブラウザから直接アップロード

ラベリング: GUIベースで直感的にアノテーション作業が可能

モデル設計: ニューラルネットワークの構成をビジュアルに設定

トレーニング: クラウド上でモデルを学習（GPUを自前で用意する必要なし）

最適化: 対象デバイスに合わせてモデルを自動的に量子化・軽量化

デプロイ: C++ライブラリとして出力し、Arduino等の環境に簡単に組み込み可能

特にSPRESENSEは公式サポート対象となっており、Edge Impulseで作成したモデルをそのままSPRESENSE上で動作させることができます。

参考リンク

Edge Impulse 公式サイト

Edge Impulse Documentation

SPRESENSE向けEdge Impulseチュートリアル

使用モデル: FOMO (Faster Objects, More Objects)

FOMOは、Edge Impulseが提供する軽量な物体検出モデルです。従来のYOLOやSSDなどと比較して、以下の特徴があります：

低メモリ消費: 組み込みデバイスでも動作可能
高速推論: リアルタイム検出が可能
バウンディングボックス: 物体の位置と信頼度を取得

データセットの作成とラベリング

A-PARTSを検出するため、様々な角度・距離・照明条件で撮影した画像にラベル付けを行いました。

Edge Impulseのラベリングツールを使用し、A-PARTSの位置にバウンディングボックスを設定しています。

画像処理パイプライン

カメラ撮影: QQVGA（160×120）サイズのYUV422形式で撮影
色空間変換: YUV422 → RGB888形式に変換
リサイズ: 160×120 → 96×96ピクセルにリサイズ
ML推論: Edge ImpulseのFOMOモデルで物体検出
結果処理: 検出されたA-PARTSの位置（X, Y座標）と信頼度を取得

モデルのトレーニング結果

FOMOモデルのトレーニングを行った結果、以下の精度を達成しました

F1スコア: 95.0%
Precision（適合率）: 0.92
Recall（再現率）: 0.98

混同行列から、A-PARTSの検出率は98.0%と高い精度を示しています。

テストセットでの評価でも、Accuracy 92.86%、F1スコア 0.97と良好な結果が得られました。

しかし、次章で述べるように、機体の物理的な問題により実際の打ち上げではこれらの機能が正常に動作しませんでした。

そのため、以下は別の場所で撮影したデモンストレーション用の映像となっています。

ここに動画が表示されます

打ち上げ結果

本セクションでは、車輪ローバ（B-PARTS）の打ち上げ結果についてのみ述べます。浮遊ローバ（A-PARTS）の結果については割愛します。

ARLISS 2025では、本機体を2回打ち上げました。

1回目の打ち上げ

1回目の打ち上げでは、着地後に機体が動作しないという問題が発生しました。途中でログが切れており、データが記録されていませんでした。電池ホルダーから電池が2個外れていて、打ち上げ時の加速、ロケット内での振動、またはパラシュート降下時の衝撃のいずれかで発生したと考えられます。

なお、打ち上げ前にはグルーガンや結束バンドなどで外れそうなパーツを補強していましたが、それでも外れてしまいました。実際の打ち上げや落下時の衝撃のすごさを実感しました。

以下は回収直後の機体の写真です。

2回目の打ち上げ

2回目の打ち上げでは、着地とケース展開後に機体が動作することを確認できましたが、スタビライザー（姿勢安定用の部品）が破損しており、機体はその場で回転するだけで、直進走行することができませんでした。この破損も、打ち上げ時の加速、ロケット内での振動、またはパラシュート降下時の衝撃が原因と推測されます。

ここに動画が表示されます

一方で、カメラシステムは正常に動作しており、以下のように、砂漠に降り立った後に画像の撮影することができました。

なお、本チームはARLISS 2025でBest Mission Award 2位、Technical System Award 3位を受賞しましたが、これは浮遊ローバ（A-PARTS）を含むチーム全体のミッションに対する評価です。

謝辞

本機体の開発および本記事の作成にあたり、100kinSATの運営者である@_ymt_117さんに多大なご協力をいただきました。100kinSATの設計資料やノウハウは本機体の開発において非常に参考になりました。また、本記事の執筆・レビューにもご協力いただきました。この場を借りて感謝申し上げます。

ソースコード

完全なソースコードはGitHubリポジトリで公開しています。

本セクションでは、車輪ローバ（B-PARTS）の実装について、主要なソースコードを解説します。なお、プロジェクト全体のコード量が約6000行と多いため、本記事ではState Patternによる状態管理やミッション制御などの主要部分のみを掲載し、センサ・アクチュエータの制御クラスなど汎用的な部分についてはGitHubリポジトリへのリンクを記載しています。

注意: 以下に掲載するコードは開発の最終版であり、実際の打ち上げ時のコードとは一部異なる可能性があります。打ち上げ後の改良やデバッグ、機能追加などが含まれているため、実際のミッション時の動作とは完全には一致しない場合があります。

メインのソースコード

メインのファイルであり、Cansatの制御を担っているCansatControllerの初期化と更新処理を行います


parts_B_wheeled_rover.ino
/**
 * @file parts_B_wheeled_rover.ino
 * @brief ARLISS 2025 TDU Team-Dauntless-Uncharted Bパーツメインプログラム
 *
 * @author TDU Team-Dauntless-Uncharted
 * @date 2025
 */

#include <Arduino.h>
#include "Controller/CansatController.hpp"

/**
 * @brief CanSat中央コントローラーのグローバルインスタンス
 */
CansatController cansat;

void setup() {
    cansat.begin();
    delay(2000);  // システム安定化待ち
}

void loop() {
    cansat.update();
    delay(200);  // 200ms周期（5Hz）
}

Cansat本体のソースコード

Cansatの状態を管理するためにStateパターンを採用しています

CansatController

Cansat全体の司令塔として、すべてのセンサ、アクチュエータを管理します

SDカードへの読み書き、状態を切り替える処理も行います


CansatController.hpp
/**
 * @file CansatController.hpp
 * @brief CanSat全体を統括する中央コントローラー
 */

#pragma once
#include "Sensor/Gnss/GnssSensor.hpp"
#include "Sensor/CdS/CdSSensor.hpp"
#include "Sensor/Camera/CameraController.hpp"
#include <BNO055Library.h>

#include "Actuator/Motor/Motor.hpp"
#include "Actuator/Led/Led.hpp"
#include "Actuator/Speaker/Speaker.hpp"

#include "Utils/FileIO/SDLogger.hpp"
#ifdef USE_FLASH
#include "Utils/FileIO/FlashIO.hpp"
#endif // USE_FLASH
#include "Utils/PowerController/PowerController.hpp"
#include "Utils/Serial/SerialWriter.hpp"
#include <TwelitePacket.h>

#include "Controller/ICansatState.hpp"
#include <array>
#include <cstdarg>
#include <cstdio>

/**
 * @def SENSOR_BUFFER_SIZE
 * @brief センサログのバッファサイズ（バイト）
 *
 * センサデータをメモリ上にバッファリングし、
 * まとめてSDカードに書き込むことで効率化する
 */
#define SENSOR_BUFFER_SIZE 4096

/**
 * @struct UserConfig
 * @brief ユーザー設定可能なパラメータ
 *
 * config.json ファイルから読み込まれる設定値を保持する。
 * ファイルが存在しない、または特定のキーがない場合は、
 * ここで定義されたデフォルト値が使用される
 */
struct UserConfig {
    unsigned long calibrationStateTimeoutThreshold = 5 * 60 * 1000;  ///< CALIBRATIONタイムアウト（ミリ秒）
    double standbyStateAltThreshold = 20;                            ///< STANDBYでの高度閾値
    unsigned long standbyStateTimeoutThreshold = 10 * 60 * 1000;     ///< STANDBYタイムアウト（ミリ秒）
    int launchStateCdsThreshold = 400;                               ///< LAUNCHのCdS閾値
    unsigned long launchStateTimeoutThreshold = 20 * 60 * 1000;      ///< LAUNCHタイムアウト（ミリ秒）
    unsigned long dropStateTimeoutThreshold = 10 * 60 * 1000;        ///< DROPタイムアウト（ミリ秒）
    unsigned long escapeStateTimeoutThreshold = 15 * 1000;           ///< ESCAPEタイムアウト（ミリ秒）
    int detectionMaxFailedCount = 25;                                ///< DETECTION失敗上限回数
    int detectionTurn180delay = 700;                                 ///< DETECTION 180度回転時間（ミリ秒）
    unsigned long recordingTimeoutThreshold = 5 * 60 * 1000;         ///< RECORDINGタイムアウト（ミリ秒）
    int recordingTime = 30 * 1000;                                   ///< RECORDING録画時間（ミリ秒）
};

/**
 * @class CansatController
 * @brief CanSat全体を統括する中央コントローラークラス
 *
 * すべてのセンサ、アクチュエータ、ログ機能を管理し、
 * Stateマシンパターンで動作する
 *
 * 主な機能:
 * - センサデータの一括管理（GNSS, IMU, CdS, Camera）
 * - アクチュエータ制御（Motor, LED, Speaker）
 * - 状態遷移管理（State Pattern）
 * - ログ管理（SD/Flash）
 * - 設定ファイル読み込み（config.json）
 */
class CansatController {
public:
    /**
     * @brief コンストラクタ
     */
    CansatController();

    /**
     * @brief システム全体を初期化する
     */
    void begin();

    /**
     * @brief メインループで周期的に呼び出す更新処理
     *
     * 約200msごとに呼び出される
     * 以下の処理を実行する
     * 1. ハートビートログ（10秒ごと）
     * 2. 全センサ値の更新
     * 3. センサログのバッファリング
     * 4. 現在の状態の onUpdate() 呼び出し
     */
    void update();

    /**
     * @brief 状態を変更する
     * @param newState 新しい状態のユニークポインタ
     *
     * 状態遷移の流れ:
     * 1. 現在の状態の onExit() を呼び出し
     * 2. 状態ポインタを移動
     * 3. 新しい状態の onEnter() を呼び出し
     */
    void changeState(std::unique_ptr<ICansatState> newState);

    /**
     * @brief システムログを出力する（printf形式）
     * @param format フォーマット文字列
     * @param ... 可変長引数
     */
    void writeSystemLog(const char* format, ...);

    // --- センサアクセス ---

    GnssSensor &getGnss() { return _gnss; }                ///< GNSS センサへの参照取得
    BNO055 &getBno055() { return _bno055; }                ///< IMU センサへの参照取得
    CdSSensor &getCds() { return _cds; }                   ///< CdS センサへの参照取得
    CameraController &getCamera() { return _camera; }      ///< カメラへの参照取得
    PowerController &getPower() { return _power; }         ///< 電源管理への参照取得

    // --- アクチュエータアクセス ---

    Motor &getMotor() { return _motor; }                   ///< モーターへの参照取得
    Led &getLed(int idx) { return _led[idx]; }             ///< LED（0-3）への参照取得
    Speaker &getSpeaker() { return _speaker; }             ///< スピーカーへの参照取得

    // --- データ管理アクセス ---

    twelite::TwelitePacket &getTwelite() { return _twelite; }  ///< Twelite無線通信への参照取得
    SDLogger &getSDLogger() { return _sdLogger; }          ///< SD ロガーへの参照取得
#ifdef USE_FLASH
    FlashIO &getFlashIO() { return _flash; }               ///< Flash I/Oへの参照取得
#endif // USE_FLASH
    SerialWriter &getSerialWriter() { return _writer; }    ///< シリアルライターへの参照取得

    /**
     * @brief Twelite接続フラグを設定する
     * @param isConnectTwelite 接続済みかどうか
     */
    void setIsConnectTwelite(bool isConnectTwelite) { _isConnectTwelite = isConnectTwelite; }

    /**
     * @brief カメラ初期化フラグを設定する
     * @param initCamera 初期化済みかどうか
     */
    void setInitCamera(bool initCamera) { _initCamera = initCamera; }

    /**
     * @brief Tweliteが接続済みかを確認する
     * @return true: 接続済み, false: 未接続
     */
    bool isConnectTwelite() { return _isConnectTwelite; }

    /**
     * @brief カメラが初期化済みかを確認する
     * @return true: 初期化済み, false: 未初期化
     */
    bool isInitCamera() { return _initCamera; }

    /**
     * @brief ユーザー設定を取得する
     * @return UserConfig 構造体のコピー
     */
    UserConfig getUserConfig() { return _config; }

    /**
     * @brief センサログをバッファに追記する
     *
     * 現在のセンサ値をCSV形式でバッファに追記する
     * バッファが満杯になったら自動的にSDカードに書き込む
     */
    void appendSensorLog();

    /**
     * @brief LEDをビットフラグで制御する
     * @param state ビットフラグ（bit0=LED0, bit1=LED1, bit2=LED2, bit3=LED3）
     *
     * 状態番号をビットパターンとしてLEDに表示する
     * 例: state=5 (0b0101) → LED0とLED2が点灯
     */
    void setLed(const int state);

private:
    UserConfig _config;                 ///< ユーザー設定（config.jsonから読み込み）
    unsigned long _lastHeartBeat = 0;   ///< 最後のハートビート出力時刻（ミリ秒）

    /**
     * @brief 現在の設定をログに出力する
     *
     * config.jsonから読み込んだ設定値をシステムログに出力する
     */
    void dumpConfig();

    /**
     * @brief config.jsonファイルを読み込む
     *
     * SDカードの /mnt/sd0/config.json を読み込み、
     * UserConfig 構造体の各フィールドを更新する
     * ファイルが存在しないかパースに失敗した場合は何もしません。
     */
    void readConfigFile();

    /**
     * @brief 初期状態を設定する
     *
     * SDカード（またはFlash）から state.txt を読み取り、
     * 対応する状態のインスタンスを生成して changeState() を呼び出す
     * ファイルが存在しない場合は CalibrationState から開始する
     */
    void configState();

    // --- センサログバッファ ---
    const String CSV_HEADER = "time,date,mode,lat,lng,alt,cds,ax,ay,az,gx,gy,gz,mx,my,mz,roll,pitch,heading,voltage"; ///< CSVヘッダー
    char _sensorBuffer[SENSOR_BUFFER_SIZE];  ///< センサデータバッファ
    size_t _head = 0;                        ///< バッファの書き込み位置

    // --- 状態管理 ---
    std::unique_ptr<ICansatState> _state;  ///< 現在の状態（State Pattern）

    bool _isConnectTwelite = false;  ///< Twelite接続済みフラグ
    bool _initCamera = false;  ///< カメラ初期化済みフラグ

    // --- センサ ---
    GnssSensor _gnss;             ///< GNSS センサ
    BNO055 _bno055;               ///< 9軸 IMU センサ
    Vector<float> _acceleration;  ///< 加速度ベクトル（BNO055から取得）
    Vector<float> _gyro;          ///< ジャイロベクトル（BNO055から取得）
    Vector<float> _magnetic;      ///< 磁気ベクトル（BNO055から取得）
    EulerAngles _euler;           ///< オイラー角（roll, pitch, heading）
    CdSSensor _cds;               ///< CdS 光センサ
    CameraController _camera;     ///< カメラコントローラー

    // --- アクチュエータ ---
    int _motorR_pins[3];        ///< 右モーターピン配列
    int _motorL_pins[3];        ///< 左モーターピン配列
    Motor _motor;               ///< モーターコントローラー
    std::array<Led, 4> _led;    ///< LED配列（LED0-LED3）
    Speaker _speaker;           ///< スピーカー

    PowerController _power;     ///< 電源管理

    // --- データ管理 ---
    twelite::TwelitePacket _twelite;  ///< Twelite無線通信
    SDLogger _sdLogger;         ///< SDカードロガー
#ifdef USE_FLASH
    FlashIO _flash;             ///< Flash ストレージI/O
#endif // USE_FLASH
    SerialWriter _writer;       ///< シリアル出力
};

CansatController.hpp


CansatController.cpp
/**
 * @file CansatController.cpp
 * @brief CanSat中央コントローラーの実装
 */

// NOTE: ArduinoJsonのPROGMEM機能を無効化
#define ARDUINOJSON_ENABLE_PROGMEM 0

#include <Arduino.h>
#include <ArduinoJson.h>
#include "Controller/CansatController.hpp"

#include "Controller/States/CalibrationState.hpp"
#include "Controller/States/StandbyState.hpp"
#include "Controller/States/LaunchState.hpp"
#include "Controller/States/DropState.hpp"
#include "Controller/States/EscapeState.hpp"
#include "Controller/States/DetectionState.hpp"
#include "Controller/States/RecordingState.hpp"
#include "Controller/States/ExploreState.hpp"

CansatController::CansatController()
    : _motorR_pins{8, 4, 5},        // 右モーター: PWMピン8, 方向ピン4/5
      _motorL_pins{7, 2, 3},        // 左モーター: PWMピン7, 方向ピン2/3
      _gnss(1000),                  // GNSSタイムアウト1000ms
      _bno055(),
      _cds(A0),                     // CdSセンサ: アナログピンA0
      _motor(_motorR_pins, _motorL_pins),
      _led{Led(LED0), Led(LED1), Led(LED2), Led(LED3)},  // Spresense内蔵LED
      _speaker(9),                  // スピーカー: ピン9
      _twelite(11),
      _sdLogger(),
      _camera(),
      _power()
{
}

void CansatController::writeSystemLog(const char* format, ...) {
    char buf[256];

    // 可変長引数をフォーマット文字列に従って展開
    va_list args;
    va_start(args, format);
    vsnprintf(buf, sizeof(buf), format, args);
    va_end(args);

    // SDカードとシリアル出力の両方に書き込み
    _sdLogger.appendSystemLog(buf);
    _writer.log(buf);
}

void CansatController::begin() {
    _writer.begin();
    _writer.log("CansatController: Starting begin()");

    _twelite.begin(Serial2, 115200);
    _twelite.off();
    setIsConnectTwelite(false);
    
    if (!_sdLogger.begin(CSV_HEADER)) {
        for (int i = 0; i < 5; i++) {
            _speaker.playBeep();
            delay(100);
        }
        _writer.log("CansatController: SD Logger initialization failed!");
    } else {
        _writer.log("CansatController: SD Logger initialized successfully");
    }
    _sdLogger.appendSystemLog("CansatController: begin() started");

    _power.begin();
    writeSystemLog(_power.getBootCause());

    readConfigFile();   
    dumpConfig();

    if (!_gnss.begin()) {
        writeSystemLog("CansatController: GNSS initialization failed!");
    } else {
        writeSystemLog("CansatController: GNSS initialized successfully");
    }

#ifdef WAIT_GNSS_RECEIVE
    if (!_gnss.waitReceive()) {
        writeSystemLog("CansatController: GNSS position fix failed!");
    } else {
        writeSystemLog("CansatController: GNSS position fix succeeded");
    }
#endif
    
    if (!_bno055.begin(true)) {
        writeSystemLog("CansatController: BNO055 initialization failed!");
    } else {
        writeSystemLog("CansatController: BNO055 initialized successfully");
    }
    // _bno055.setAccelerometerTo16G();

    writeSystemLog("CansatController: begin() finished\ntimestamp: log");
    configState();
}

void CansatController::update() {
    unsigned long now = millis();

    // ハートビートログ: 10秒ごとに生存確認ログを出力
    if (now - _lastHeartBeat >= 10000) {
        writeSystemLog("%lu: HeartBeat", now);
        _lastHeartBeat = now;
    }

    // 全センサの値を更新
    _gnss.update();
    _acceleration = _bno055.getAcceleration();
    _gyro = _bno055.getGyroscope();
    _magnetic = _bno055.getMagnetometer();
    _euler = _bno055.getEulerAngles();

    // センサログをバッファに追記
    appendSensorLog();

    // 現在の状態の onUpdate() を呼び出し
    if (_state) _state->onUpdate();
}

void CansatController::changeState(std::unique_ptr<ICansatState> newState) {
    // 現在の状態の終了処理
    if (_state) _state->onExit();

    // 状態ポインタを移動（所有権の移譲）
    _state = std::move(newState);

    // 新しい状態の初期化処理
    if (_state) _state->onEnter();
}

void CansatController::readConfigFile() {
    char buffer[512];
    size_t bytesRead = _sdLogger.readJSONFile("/mnt/sd0/config.json", buffer, sizeof(buffer));

    // ファイルが存在しない、または読み込み失敗の場合はデフォルト値を使用
    if (bytesRead == 0) {
        return;
    }

    // ArduinoJsonでJSONをパース
    DynamicJsonDocument doc(512);
    DeserializationError error = deserializeJson(doc, buffer);

    // JSONパースに失敗した場合はデフォルト値を使用
    if (error) {
        return;
    }

    // 各キーの存在をチェックしてから値を代入（存在しないキーはデフォルト値を維持）
    
    // Calibration設定
    if (doc.containsKey("Calibration") && doc["Calibration"].containsKey("Timeout")) {
        _config.calibrationStateTimeoutThreshold = doc["Calibration"]["Timeout"];
    }
    
    // Standby設定
    if (doc.containsKey("Standby")) {
        if (doc["Standby"].containsKey("Alt")) {
            _config.standbyStateAltThreshold = doc["Standby"]["Alt"];
        }
        if (doc["Standby"].containsKey("Timeout")) {
            _config.standbyStateTimeoutThreshold = doc["Standby"]["Timeout"];
        }
    }
    
    // Launch設定
    if (doc.containsKey("Launch")) {
        if (doc["Launch"].containsKey("CdS")) {
            _config.launchStateCdsThreshold = doc["Launch"]["CdS"];
        }
        if (doc["Launch"].containsKey("Timeout")) {
            _config.launchStateTimeoutThreshold = doc["Launch"]["Timeout"];
        }
    }
    
    // Drop設定
    if (doc.containsKey("Drop") && doc["Drop"].containsKey("Timeout")) {
        _config.dropStateTimeoutThreshold = doc["Drop"]["Timeout"];
    }
    
    // Escape設定
    if (doc.containsKey("Escape")) {
        if (doc["Escape"].containsKey("Timeout")) {
            _config.escapeStateTimeoutThreshold = doc["Escape"]["Timeout"];
        }
    }
    
    // Detection設定
    if (doc.containsKey("Detection") && doc["Detection"].containsKey("MaxFailedCount")) {
        if (doc["Detection"].containsKey("MaxFailedCount")) {
            _config.detectionMaxFailedCount = doc["Detection"]["MaxFailedCount"];
        }
        if (doc["Detection"].containsKey("Turn180Delay")) {
            _config.detectionTurn180delay = doc["Detection"]["Turn180Delay"];
        }
    }
    
    // Recording設定
    if (doc.containsKey("Recording")) {
        if (doc["Recording"].containsKey("Timeout")) {
            _config.recordingTimeoutThreshold = doc["Recording"]["Timeout"];
        }
        if (doc["Recording"].containsKey("Time")) {
            _config.recordingTime = doc["Recording"]["Time"];
        }
    }
}

void CansatController::dumpConfig() {
    writeSystemLog("\n=== Configuration Dump ===");

    char logBuf[128];  // ログメッセージ用バッファ
    // Calibration設定
    snprintf(logBuf, sizeof(logBuf), "[Calibration] Timeout: %d", 
             _config.calibrationStateTimeoutThreshold);
    writeSystemLog(logBuf);    

    // Standby設定
    snprintf(logBuf, sizeof(logBuf), "[Standby] Alt: %.2f, Timeout: %d", 
             _config.standbyStateAltThreshold, _config.standbyStateTimeoutThreshold);
    writeSystemLog(logBuf);
    
    // Launch設定
    snprintf(logBuf, sizeof(logBuf), "[Launch] CdS: %d, Timeout: %d", 
             _config.launchStateCdsThreshold, _config.launchStateTimeoutThreshold);
    writeSystemLog(logBuf);
    
    // Drop設定
    snprintf(logBuf, sizeof(logBuf), "[Drop] Timeout: %d", 
             _config.dropStateTimeoutThreshold);
    writeSystemLog(logBuf);
    
    // Escape設定
    snprintf(logBuf, sizeof(logBuf), "[Escape] Timeout: %d", 
             _config.escapeStateTimeoutThreshold);
    writeSystemLog(logBuf);
    
    // Detection設定
    snprintf(logBuf, sizeof(logBuf), "[Detection] MaxFailedCount: %d", 
             _config.detectionMaxFailedCount);
    writeSystemLog(logBuf);
    
    // Recording設定
    snprintf(logBuf, sizeof(logBuf), "[Recording] Timeout: %d, Time: %d", 
             _config.recordingTimeoutThreshold, _config.recordingTime);
    writeSystemLog(logBuf);

    writeSystemLog("=== End of Configuration Dump ===\n");
}

void CansatController::configState() {
    State state;

    // SDカードから状態番号を読み取り
    if (!_sdLogger.readState((int&)state)) {
        // SDカードから読み取れない場合、Flashから読み取りを試みる
#ifdef USE_FLASH
        _writer.log("Use Flash");
        _flash.readState((int&)state);
#endif // USE_FLASH
    }

    // 状態番号に応じた状態インスタンスを生成
    switch (state) {
    case State::CALIBRATION:
        writeSystemLog("%lu: config CalibrationState", millis());
        changeState(std::make_unique<CalibrationState>(*this));
        break;
    case State::STANDBY:
        writeSystemLog("%lu: config StandbyState", millis());
        changeState(std::make_unique<StandbyState>(*this));
        break;
    case State::LAUNCH:
        writeSystemLog("%lu: config LaunchState", millis());
        changeState(std::make_unique<LaunchState>(*this));
        break;
    case State::DROP:
        writeSystemLog("%lu: config DropState", millis());
        changeState(std::make_unique<DropState>(*this));
        break;
    case State::ESCAPE:
        writeSystemLog("%lu: config EscapeState", millis());
        changeState(std::make_unique<EscapeState>(*this));
        break;
    case State::DETECTION:
        writeSystemLog("%lu: config DetectionState", millis());
        changeState(std::make_unique<DetectionState>(*this));
        break;
    case State::RECORDING:
        writeSystemLog("%lu: config RecordingState", millis());
        changeState(std::make_unique<RecordingState>(*this));
        break;
    case State::EXPLORE:
        writeSystemLog("%lu: config ExploreState", millis());
        changeState(std::make_unique<ExploreState>(*this));
        break;
    default:
        writeSystemLog("%lu: config CalibrationState", millis());
        changeState(std::make_unique<CalibrationState>(*this));
        break;
    }
}

void CansatController::appendSensorLog() {
    // ステップ1: 全センサ値のスナップショットを取得
    unsigned long t_ms = millis();

    char date[32];           // 例: "2025-08-23 12:34:56"
    date[0] = '\0';
    // getCurrentDate() が const char* を返すタイプなら strcpy_safety
    {
        const char* p = _gnss.getCurrentDate();   // 不安定なポインタ
        if (p) {
            // 安全コピー（必ず終端する）
            strncpy(date, p, sizeof(date) - 1);
            date[sizeof(date) - 1] = '\0';
        } else {
            strcpy(date, ""); // 空にしておく
        }
    }

    int state_i = _state ? _state->getState() : -1;

    float lat = _gnss.getLatitude();
    float lon = _gnss.getLongitude();
    float alt = _gnss.getAltitude();

    int   cds  = _cds.read();

    float ax = _acceleration.x, ay = _acceleration.y, az = _acceleration.z;
    float gx = _gyro.x, gy = _gyro.y, gz = _gyro.z;
    float mx = _magnetic.x, my = _magnetic.y, mz = _magnetic.z;
    float roll = _euler.roll, pitch = _euler.pitch, heading = _euler.heading;

    int voltage_mV = _power.getVoltage();

    // ステップ2: CSV行を組み立て
    char line[192];
    int len = snprintf(
        line, sizeof(line),
        "%lu,%s,%d,%.6f,%.6f,%.2f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.2f,%.2f,%.2f,%d\n",
        t_ms, date, state_i,
        lat, lon, alt,
        cds, ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, heading, voltage_mV
    );

    // ステップ3: フォーマット失敗または切り詰めを検出
    if (len < 0) {
        _writer.log("appendSensorLog: snprintf failed");
        return;
    }
    if ((size_t)len >= sizeof(line)) {
        _writer.log("appendSensorLog: line truncated, dropping");
        return;
    }

    // ステップ4: バッファ境界チェック
    if (_head + (size_t)len > SENSOR_BUFFER_SIZE) {
        // バッファが満杯になったら、現在の内容をSDカードに書き出し
        _sdLogger.appendSensorLog(_sensorBuffer, _head);
        _head = 0;  // バッファをリセット
    }

    // ステップ5: バッファに追記
    memcpy(&_sensorBuffer[_head], line, (size_t)len);
    _head += (size_t)len;
}

void CansatController::setLed(const int state) {
    // 各LEDをビットフラグで制御
    // state のビット i が立っていれば LED i を点灯、そうでなければ消灯
    for (int i = 0; i < 4; i++) {
        if (state & (1 << i)) {
            _led[i].on();
        } else {
            _led[i].off();
        }
    }
}

Cansatの状態管理

8つの状態を定義しており、各状態が持つべき機能を決めています

onEnter()：その状態に入ったときの処理
onUpdate()：その状態のメインループの処理
onExit()：その状態から抜けるときの処理


ICansatState.hpp
/**
 * @file ICansatState.hpp
 * @brief CanSat状態マシンの基底インターフェース
 */

#pragma once

class CansatController;

/**
 * @enum State
 * @brief CanSatのミッション状態を表す列挙型
 *
 * 各状態は0から順番に番号が割り当てられ、state.txtファイルに保存される
 * この番号を使って特定の状態から実行を開始する
 */
enum State {
    CALIBRATION = 0,  ///< キャリブレーション状態：センサ初期化
    STANDBY,          ///< スタンバイ状態：発射前待機
    LAUNCH,           ///< 発射検知状態：CdSセンサで放出検知
    DROP,             ///< 降下状態：Twelite起動、降下監視
	ESCAPE,           ///< 脱出状態：コンテナからの脱出
	DETECTION,        ///< 検知状態：A-parts（目標）の物体検知
	RECORDING,        ///< 録画状態：A-parts発射動画の撮影
	EXPLORE,          ///< 探索状態：自律移動と周辺撮影
};

/**
 * @class ICansatState
 * @brief CanSat状態の抽象基底クラス（インターフェース）
 *
 * Stateマシンパターンを実装するための基底クラス
 * 各具体的な状態（CalibrationState, StandbyStateなど）はこのクラスを継承し、
 * onEnter(), onUpdate(), onExit() をオーバーライドする
 *
 * 状態遷移の流れ:
 * 1. 前の状態の onExit() が呼ばれる
 * 2. CansatController::changeState() で状態が切り替わる
 * 3. 新しい状態の onEnter() が呼ばれる（初期化処理）
 * 4. メインループで onUpdate() が周期的に呼ばれる（200msごと）
 * 5. 次の状態に遷移する際、onExit() が呼ばれる（終了処理）
 */
class ICansatState {
public:
	/**
	 * @brief 仮想デストラクタ
	 */
	virtual ~ICansatState() = default;

	/**
	 * @brief 状態開始時に1回だけ呼ばれる処理
	 */
	virtual void onEnter() {}

	/**
	 * @brief 状態のメインループ処理（周期的に呼ばれる）
	 * 
	 * 次の状態に遷移する場合は、この中で changeState() を呼び出す
	 */
	virtual void onUpdate() {}

	/**
	 * @brief 状態終了時に1回だけ呼ばれる処理
	 */
	virtual void onExit() {}

	/**
	 * @brief 現在の状態を返す
	 * @return 状態を表す列挙値
	 */
	virtual State getState() const {}
};

1. CALIBRATION

センサのキャリブレーションを行う状態です


CalibrationState.hpp
#pragma once
#include "Controller/ICansatState.hpp"

#include <BNO055Library.h>

/**
 * @class CalibrationState
 * @brief キャリブレーション状態：センサの初期化とキャリブレーションを行う
 *
 * CanSatの最初の状態。BNO055（9軸IMU）のキャリブレーションを行い、
 * センサが安定するまで待機します。現在の実装では即座にStandbyStateへ遷移する
 */
class CalibrationState : public ICansatState {
public:
	/**
	 * @brief コンストラクタ
	 * @param ctx CansatControllerへの参照
	 */
	CalibrationState(CansatController& ctx) : _ctx(ctx) {}

	/**
	 * @brief 状態開始時の初期化処理
	 */
	void onEnter() override;

	/**
	 * @brief 状態の更新処理（200msごとに呼ばれる）
	 */
	void onUpdate() override;

	/**
	 * @brief 状態終了時の処理
	 */
	void onExit() override;

	/**
	 * @brief 現在の状態を取得
	 * @return State::CALIBRATION
	 */
	State getState() const override;

private:
	CansatController& _ctx;        ///< CansatControllerへの参照
	unsigned long _startTime;      ///< キャリブレーション開始時刻（タイムアウト判定用）

	/**
	 * @brief BNO055のキャリブレーションが完了しているか判定
	 * @param calib BNO055のキャリブレーションステータス
	 * @return system≥1, gyro≥3, accel≥3, mag≥3の場合true
	 * @note 各センサのキャリブレーションは0-3の4段階（3が完全）
	 */
	bool isFullyCalibrated(CalibrationStatus calib);
};


CalibrationState.cpp
#include "Controller/States/CalibrationState.hpp"
#include "Controller/States/StandbyState.hpp"
#include "Controller/CansatController.hpp"

void CalibrationState::onEnter() {
	_ctx.writeSystemLog("%lu: Entering CalibrationState", millis());

	// 状態に対応したビープ音とLED点灯パターンを実行
	_ctx.getSpeaker().playState((int)State::CALIBRATION);
	_ctx.setLed((int)State::CALIBRATION);

	_startTime = millis();  // タイムアウト判定用の開始時刻を記録
}

// センサのキャリブレーション
void CalibrationState::onUpdate() {
	_ctx.getSerialWriter().log("Updating CalibrationState");

	// キャリブレーション処理
	while (true) {
		CalibrationStatus calib = _ctx.getBno055().getCalibrationStatus();

		// 全センサのキャリブレーション完了判定
		if (isFullyCalibrated(calib)) {
			_ctx.writeSystemLog("%lu: Calibration finished. Change to StandbyState", millis());
			break;
		}

		// 地磁気センサのキャリブレーション進捗をLED0で表示
		if (calib.mag < 3) {
			_ctx.getLed(0).off();
		} else {
			_ctx.getLed(0).on();
		}

		// 加速度センサのキャリブレーション進捗をLED1で表示
		if (calib.accel < 3) {
			_ctx.getLed(1).off();
		} else {
			_ctx.getLed(1).on();
		}

		// ジャイロセンサのキャリブレーション進捗をLED2で表示
		if (calib.gyro < 3) {
			_ctx.getLed(2).off();
		} else {
			_ctx.getLed(2).on();
		}

		// システム全体のキャリブレーション進捗をLED3で表示
		if (calib.system < 3) {
			_ctx.getLed(3).off();
		} else {
			_ctx.getLed(3).on();
		}

		// タイムアウト判定（デフォルト5分）
		long elapsedTime = millis() - _startTime;
		_ctx.getSerialWriter().logf("Elapsed time: %lu", elapsedTime);
		if (elapsedTime > _ctx.getUserConfig().calibrationStateTimeoutThreshold) {
			_ctx.writeSystemLog("%lu: Timeout. Change to StandbyState", millis());
			break;
		}
	}

	_ctx.changeState(std::make_unique<StandbyState>(_ctx));
}

void CalibrationState::onExit() {
	_ctx.writeSystemLog("%lu: Exiting CalibrationState", millis());

	// 次の状態（STANDBY）をSDカードに保存
	if (!_ctx.getSDLogger().writeState((int)State::STANDBY)) {
		_ctx.getSerialWriter().log("Failed to write state in SD");
	}

#ifdef USE_FLASH
	// USE_FLAGが定義されている場合はFlashにも保存
	if (!_ctx.getFlashIO().writeState((int)State::STANDBY)) {
		_ctx.getSerialWriter().log("Failed to write state in Flash");
	}
#endif // USE_FLASH
}

State CalibrationState::getState() const {
	return State::CALIBRATION;
}

// BNO055のキャリブレーション完了判定
// system≥1: システム全体が最低限動作可能
// gyro≥3, accel≥3, mag≥3: 各センサが完全にキャリブレーション完了
bool CalibrationState::isFullyCalibrated(CalibrationStatus calib) {
    return (calib.system >= 1 && calib.gyro >= 3 && calib.accel >= 3 && calib.mag >= 3);
}

2. STANDBY

ケースの収納から、ロケットが発射されるまでの状態です


StandbyState.hpp
/**
 * @file StandbyState.hpp
 */

#pragma once
#include "Controller/ICansatState.hpp"

/**
 * @class StandbyState
 * @brief スタンバイ状態：発射前の待機状態
 */
class StandbyState : public ICansatState {
public:
	/**
	 * @brief コンストラクタ
	 * @param ctx CansatControllerへの参照
	 */
	StandbyState(CansatController& ctx) : _ctx(ctx) {}

	/**
	 * @brief 状態開始時の初期化処理
	 */
	void onEnter() override;

	/**
	 * @brief 状態の更新処理（200msごとに呼ばれる）
	 */
	void onUpdate() override;

	/**
	 * @brief 状態終了時の処理
	 */
	void onExit() override;

	/**
	 * @brief 現在の状態を取得
	 * @return State::STANDBY
	 */
	State getState() const override;

private:
	CansatController& _ctx;        ///< CansatControllerへの参照
	unsigned long _startTime;      ///< 待機開始時刻（タイムアウト判定用）
};


StandbyState.cpp
/**
 * @file StandbyState.cpp
 */

#include "Controller/States/StandbyState.hpp"
#include "Controller/States/LaunchState.hpp"
#include "Controller/CansatController.hpp"

void StandbyState::onEnter() {
	_ctx.writeSystemLog("%lu: Entering StandbyState", millis());

	// 状態に対応したビープ音とLED点灯パターンを実行
	_ctx.getSpeaker().playState((int)State::STANDBY);
	_ctx.setLed((int)State::STANDBY);

	_startTime = millis();  // タイムアウト判定用の開始時刻を記録
}

void StandbyState::onUpdate() {
	_ctx.getSerialWriter().log("Updating StandbyState");

	// 高度閾値判定（config.jsonで設定可能）
	if (_ctx.getGnss().getAltitude() > _ctx.getUserConfig().standbyStateAltThreshold) {
		_ctx.writeSystemLog("%lu: Above an altitude. Change to LaunchState", millis());
		_ctx.changeState(std::make_unique<LaunchState>(_ctx));
		return;
	}

	// タイムアウト判定
    long elapsedTime = millis() - _startTime;
	_ctx.getSerialWriter().logf("Elapsed time: %lu", elapsedTime);
    if (elapsedTime > _ctx.getUserConfig().standbyStateTimeoutThreshold) {
		_ctx.writeSystemLog("%lu: Timeout. Change to LaunchState", millis());
		_ctx.changeState(std::make_unique<LaunchState>(_ctx));
		return;
    }
}

void StandbyState::onExit() {
	_ctx.writeSystemLog("%lu: Exiting StandbyState", millis());

	// 次の状態（LAUNCH）をSDカードに保存
	if (!_ctx.getSDLogger().writeState((int)State::LAUNCH)) {
		_ctx.getSerialWriter().log("Failed to write state in SD");
	}

#ifdef USE_FLASH
	// USE_FLAGが定義されている場合はFlashにも保存
	if (!_ctx.getFlashIO().writeState((int)State::LAUNCH)) {
		_ctx.getSerialWriter().log("Failed to write state in Flash");
	}
#endif // USE_FLASH

	// カメラが未初期化の場合、PHOTO_MODEで初期化
	if (!_ctx.isInitCamera()) {
		if (!_ctx.getCamera().begin(PHOTO_MODE)) {
			return;
		}

		if (!_ctx.getCamera().startStreaming(true)) {
			return;
		}
		_ctx.setInitCamera(true);
	}

	// 状態遷移前に1枚撮影してSDカードに保存
	void* imgBuff = nullptr;
    size_t imgSize = 0;
    if (_ctx.getCamera().takePicture(&imgBuff, &imgSize)) {
        _ctx.getSerialWriter().log("Save taken picture to SD card...");
        _ctx.getSDLogger().saveJPEGImage(imgBuff, imgSize);
    } else {
		_ctx.getSerialWriter().log("Failed to take picture");
    }

	// カメラを終了して次の状態に備える
	_ctx.getCamera().end();
	_ctx.setInitCamera(false);
}

State StandbyState::getState() const {
	return State::STANDBY;
}

3. LAUNCH

放出することを検知する状態です。CdSで放出したことを検知します


LaunchState.hpp
/**
 * @file LaunchState.hpp
 */

#pragma once
#include "Controller/ICansatState.hpp"

/**
 * @class LaunchState
 * @brief 発射検知状態：ロケットからの放出を検知する
 */
class LaunchState : public ICansatState {
public:
	/**
	 * @brief コンストラクタ
	 * @param ctx CansatControllerへの参照
	 */
	LaunchState(CansatController& ctx) : _ctx(ctx) {}

	/**
	 * @brief 状態開始時の初期化処理
	 */
	void onEnter() override;

	/**
	 * @brief 状態の更新処理（200msごとに呼ばれる）
	 */
	void onUpdate() override;

	/**
	 * @brief 状態終了時の処理
	 */
	void onExit() override;

	/**
	 * @brief 現在の状態を取得
	 * @return State::LAUNCH
	 */
	State getState() const override;

private:
	CansatController& _ctx;        ///< CansatControllerへの参照
	unsigned long _startTime;      ///< 検知開始時刻（タイムアウト判定用）
};


LaunchState.cpp
/**
 * @file LaunchState.cpp
 */

#include "Controller/States/LaunchState.hpp"
#include "Controller/States/DropState.hpp"
#include "Controller/CansatController.hpp"

void LaunchState::onEnter() {
	_ctx.writeSystemLog("%lu: Entering LaunchState", millis());

	// 状態に対応したビープ音とLED点灯パターンを実行
	_ctx.getSpeaker().playState((int)State::LAUNCH);
	_ctx.setLed((int)State::LAUNCH);

	_startTime = millis();  // タイムアウト判定用の開始時刻を記録
}

void LaunchState::onUpdate() {
	_ctx.getSerialWriter().log("Updating LaunchState");

	// CdSセンサで放出を検知（閾値未満=明るい=ペイロードベイから出た）
	// 放出を検知したら DROP モードに遷移する
    if (_ctx.getCds().read() < _ctx.getUserConfig().launchStateCdsThreshold) {
		_ctx.writeSystemLog("%lu: Detect separation. Change to DropState", millis());
		_ctx.changeState(std::make_unique<DropState>(_ctx));
		return;
    }

	// タイムアウト判定（config.jsonで設定可能）
	unsigned long elapsedTime = millis() - _startTime;
	_ctx.getSerialWriter().logf("Elapsed time: %lu", elapsedTime);
	if (elapsedTime > _ctx.getUserConfig().launchStateTimeoutThreshold) {
		_ctx.writeSystemLog("%lu: Timeout. Change to DropState", millis());
		_ctx.changeState(std::make_unique<DropState>(_ctx));
		return;
	}
}

void LaunchState::onExit() {
	_ctx.writeSystemLog("%lu: Exiting LaunchState", millis());

	// 次の状態（DROP）をSDカードに保存
	if (!_ctx.getSDLogger().writeState((int)State::DROP)) {
		_ctx.getSerialWriter().log("Failed to write state in SD");
	}

#ifdef USE_FLASH
	// USE_FLAGが定義されている場合はFlashにも保存
	if (!_ctx.getFlashIO().writeState((int)State::DROP)) {
		_ctx.getSerialWriter().log("Failed to write state in Flash");
	}
#endif // USE_FLASH

	// カメラが未初期化の場合、PHOTO_MODEで初期化
	if (!_ctx.isInitCamera()) {
		if (!_ctx.getCamera().begin(PHOTO_MODE)) {
			return;
		}

		if (!_ctx.getCamera().startStreaming(true)) {
			return;
		}
		_ctx.setInitCamera(true);
	}

	// 状態遷移前に1枚撮影してSDカードに保存
	void* imgBuff = nullptr;
    size_t imgSize = 0;
    if (_ctx.getCamera().takePicture(&imgBuff, &imgSize)) {
        _ctx.getSerialWriter().log("Save taken picture to SD card...");
        _ctx.getSDLogger().saveJPEGImage(imgBuff, imgSize);
    } else {
		_ctx.getSerialWriter().log("Failed to take picture");
    }

	// カメラを終了して次の状態に備える
	_ctx.getCamera().end();
	_ctx.setInitCamera(false);
}

State LaunchState::getState() const {
	return State::LAUNCH;
}

4. DROP

落下したことを検知する状態です。別パーツから動作できる状態をTWELITEで受信したら次の状態へ移行します


DropState.hpp
/**
 * @file DropState.hpp
 */

#pragma once
#include "Controller/ICansatState.hpp"

/**
 * @class DropState
 * @brief 降下状態：パラシュートで降下中の状態
 */
class DropState : public ICansatState {
public:
	/**
	 * @brief コンストラクタ
	 * @param ctx CansatControllerへの参照
	 */
	DropState(CansatController& ctx) : _ctx(ctx) {}

	/**
	 * @brief 状態開始時の初期化処理
	 */
	void onEnter() override;

	/**
	 * @brief 状態の更新処理（200msごとに呼ばれる）
	 */
	void onUpdate() override;

	/**
	 * @brief 状態終了時の処理
	 */
	void onExit() override;

	/**
	 * @brief 現在の状態を取得
	 * @return State::DROP
	 */
	State getState() const override;

private:
	CansatController& _ctx;        ///< CansatControllerへの参照
	unsigned long _startTime;      ///< 降下開始時刻（タイムアウト判定用）
};


DropState.cpp
/**
 * @file DropState.cpp
 */

#include "Controller/States/DropState.hpp"
#include "Controller/States/EscapeState.hpp"
#include "Controller/CansatController.hpp"

void DropState::onEnter() {
  _ctx.writeSystemLog("%lu: Entering DropState", millis());

	// 状態に対応したビープ音とLED点灯パターンを実行
	_ctx.getSpeaker().playState((int)State::DROP);
  _ctx.setLed((int)State::DROP);

  _ctx.writeSystemLog("%lu: CansatController: Twelite initialization started", millis());
  if (!_ctx.isConnectTwelite()) {
    _ctx.getTwelite().on();
    _ctx.setIsConnectTwelite(true);
  }

  _startTime = millis();  // タイムアウト判定用の開始時刻を記録
}

void DropState::onUpdate() {
	_ctx.getSerialWriter().log("Updating DropState");

	// タイムアウト判定（config.jsonで設定可能）
  unsigned long elapsedTime = millis() - _startTime;
  _ctx.getSerialWriter().logf("Elapsed time: %lu", elapsedTime);
  if (elapsedTime > _ctx.getUserConfig().dropStateTimeoutThreshold) {
    _ctx.writeSystemLog("%lu: Timeout. Change to EscapeState", millis());
    _ctx.changeState(std::make_unique<EscapeState>(_ctx));
    return;
  }

  twelite::Packet pkt;
  if (_ctx.getTwelite().receivePacket(pkt)) {
    if (twelite::TwelitePacket::match(pkt, twelite::C_PARTS, twelite::BROADCAST, twelite::DeployComplete)) {
      _ctx.writeSystemLog("%lu: DeployComplete received. Change to EscapeState", millis());
      _ctx.changeState(std::make_unique<EscapeState>(_ctx));
      return;
    }
  }
}

void DropState::onExit() {
  _ctx.writeSystemLog("%lu: Exiting DropState", millis());

	// 次の状態（ESCAPE）をSDカードに保存
  if (!_ctx.getSDLogger().writeState((int)State::ESCAPE)) {
		_ctx.getSerialWriter().log("Failed to write state in SD");
	}

#ifdef USE_FLASH
	// USE_FLAGが定義されている場合はFlashにも保存
  if (!_ctx.getFlashIO().writeState((int)State::ESCAPE)) {
		_ctx.getSerialWriter().log("Failed to write state in Flash");
  }
#endif // USE_FLASH

	// カメラが未初期化の場合、PHOTO_MODEで初期化
	if (!_ctx.isInitCamera()) {
		if (!_ctx.getCamera().begin(PHOTO_MODE)) {
			return;
		}

		if (!_ctx.getCamera().startStreaming(true)) {
			return;
		}
		_ctx.setInitCamera(true);
	}

	// 状態遷移前に1枚撮影してSDカードに保存
	void* imgBuff = nullptr;
    size_t imgSize = 0;
    if (_ctx.getCamera().takePicture(&imgBuff, &imgSize)) {
        _ctx.getSerialWriter().log("Save taken picture to SD card...");
        _ctx.getSDLogger().saveJPEGImage(imgBuff, imgSize);
    } else {
		_ctx.getSerialWriter().log("Failed to take picture");
    }

	// カメラを終了して次の状態に備える
	_ctx.getCamera().end();
	_ctx.setInitCamera(false);
}

State DropState::getState() const {
	return State::DROP;
}

5. ESCAPE

ケースから脱出、姿勢を整える状態です


EscapeState.hpp
/**
 * @file EscapeState.hpp
 */

#pragma once
#include "Controller/ICansatState.hpp"

/**
 * @class EscapeState
 * @brief 脱出状態：コンテナからの脱出を試みる
 */
class EscapeState : public ICansatState {
public:
	/**
	 * @brief コンストラクタ
	 * @param ctx CansatControllerへの参照
	 */
	EscapeState(CansatController& ctx) : _ctx(ctx) {}

	/**
	 * @brief 状態開始時の初期化処理
	 */
	void onEnter() override;

	/**
	 * @brief 状態の更新処理（200msごとに呼ばれる）
	 */
	void onUpdate() override;

	/**
	 * @brief 状態終了時の処理
	 */
	void onExit() override;

	/**
	 * @brief 現在の状態を取得
	 * @return State::ESCAPE
	 */
	State getState() const override;

private:
	CansatController& _ctx;        ///< CansatControllerへの参照
	unsigned long _startTime;      ///< 脱出開始時刻（タイムアウト判定用）
	int _count;                    ///< 脱出パターンのカウンタ（0-3をローテーション）

	/**
	 * @brief パルス前進：断続的に前進する
	 * @param pwm モーターのPWM値
	 * @param repeat 繰り返し回数
	 */
	void pulseForward(int pwm, int repeat);

	/**
	 * @brief ロッキング脱出：前後左右のロッキング動作
	 * @param pwm モーターのPWM値
	 * @param repeat 繰り返し回数
	 */
	void rockingEscape(int pwm, int repeat);

	/**
	 * @brief 位相脱出：S字カーブのような動作
	 * @param pwm モーターのPWM値
	 * @param repeat 繰り返し回数
	 */
	void phaseEscape(int pwm, int repeat);

	/**
	 * @brief 渦巻き脱出：渦巻き状の動作で周囲を探索
	 * @param pwm モーターのPWM値
	 * @param repeat 繰り返し回数
	 */
	void spiralEscape(int pwm, int repeat);
};


EscapeState.cpp
/**
 * @file EscapeState.cpp
 */

#include "Controller/States/EscapeState.hpp"
#include "Controller/States/DetectionState.hpp"
#include "Controller/CansatController.hpp"

void EscapeState::onEnter() {
	_ctx.writeSystemLog("%lu: Entering EscapeState", millis());

	_ctx.getSpeaker().playState((int)State::ESCAPE);
	_ctx.setLed((int)State::ESCAPE);

	_startTime = millis();  // タイムアウト判定用の開始時刻を記録
	_count = 0;             // 脱出パターンのカウンタを初期化
}

void EscapeState::onUpdate() {
    _ctx.getSerialWriter().log("Updating EscapeState");
    unsigned long elapsedTime = millis() - _startTime;
    _ctx.getSerialWriter().logf("Elapsed time: %lu", elapsedTime);

    // タイムアウト判定（config.jsonで設定可能）
    if (elapsedTime > _ctx.getUserConfig().escapeStateTimeoutThreshold) {
        _ctx.writeSystemLog("%lu: Timeout. Change to DetectionState", millis());
        _ctx.changeState(std::make_unique<DetectionState>(_ctx));
        return;
    }

    // 4つのダイナミックなパターンをローテーション
    switch (_count % 4) {
        case 0:
			pulseForward(100, 3);            // パルス前進
            break;
        case 1:
            rockingEscape(100, 2);           // 前後左右ロッキング
            break;
        case 2:
            phaseEscape(100, 2);             // S字カーブ
            break;
        case 3:
            spiralEscape(100, 2);            // 渦巻き動作
            break;
    }

    _count++;

	// 4パターン実行後、加速度センサで姿勢を確認してDetectionStateへ遷移
	if (_count >= 4) {
        delay(1000);

        // Z軸加速度が負（逆さま）の場合、前進して姿勢を正す
        if (_ctx.getBno055().getAcceleration().z < 0) {
            _ctx.getMotor().forward(150);
            delay(1500);
        }
        _ctx.getMotor().stop();

        _ctx.writeSystemLog("%lu: Timeout. Change to DetectionState", millis());
        _ctx.changeState(std::make_unique<DetectionState>(_ctx));
        return;
	}
}

void EscapeState::onExit() {
	_ctx.writeSystemLog("%lu: Exiting EscapeState", millis());

	// 次の状態（DETECTION）をSDカードに保存
	if (!_ctx.getSDLogger().writeState((int)State::DETECTION)) {
		_ctx.getSerialWriter().log("Failed to write state in SD");
	}

#ifdef USE_FLASH
	// USE_FLAGが定義されている場合はFlashにも保存
	if (!_ctx.getFlashIO().writeState((int)State::DETECTION)) {
		_ctx.getSerialWriter().log("Failed to write state in Flash");
	}
#endif // USE_FLASH

	// カメラが未初期化の場合、PHOTO_MODEで初期化
	if (!_ctx.isInitCamera()) {
		if (!_ctx.getCamera().begin(PHOTO_MODE)) {
			return;
		}

		if (!_ctx.getCamera().startStreaming(true)) {
			return;
		}
		_ctx.setInitCamera(true);
	}

	// 状態遷移前に1枚撮影してSDカードに保存
	void* imgBuff = nullptr;
    size_t imgSize = 0;
    if (_ctx.getCamera().takePicture(&imgBuff, &imgSize)) {
        _ctx.getSerialWriter().log("Save taken picture to SD card...");
        _ctx.getSDLogger().saveJPEGImage(imgBuff, imgSize);
    } else {
		_ctx.getSerialWriter().log("Failed to take picture");
    }

	// カメラを終了して次の状態に備える
	_ctx.getCamera().end();
	_ctx.setInitCamera(false);
}

State EscapeState::getState() const {
	return State::ESCAPE;
}

// パルス前進：断続的に前進と停止を繰り返す
void EscapeState::pulseForward(int pwm, int repeat) {
	for (int i = 0; i < repeat; ++i) {
		_ctx.getMotor().forward(pwm);
		delay(200);
		_ctx.getMotor().stop();
		delay(200);
	}
}

// ロッキング脱出：前後左右のロッキング動作でコンテナから脱出を試みる
void EscapeState::rockingEscape(int pwm, int repeat) {
    for (int i = 0; i < repeat; ++i) {
        // 前進
        _ctx.getMotor().forward(pwm);
        delay(250);
        _ctx.getMotor().stop();
        delay(100);

        // 右に旋回
        _ctx.getMotor().turnRight(pwm);
        delay(180);
        _ctx.getMotor().stop();
        delay(100);

        // 後退
        _ctx.getMotor().backward(pwm);
        delay(250);
        _ctx.getMotor().stop();
        delay(100);

        // 左に旋回（元の向きに戻す）
        _ctx.getMotor().turnLeft(pwm);
        delay(180);
        _ctx.getMotor().stop();
        delay(100);
    }
}

// 位相脱出：S字カーブのような動作で位置を変えながら最終的に戻る
void EscapeState::phaseEscape(int pwm, int repeat) {
    for (int i = 0; i < repeat; ++i) {
        // 右斜め前に移動
        _ctx.getMotor().rightForward(pwm);
        delay(200);
        _ctx.getMotor().leftForward(pwm * 0.7);  // 左を少し弱く
        delay(200);
        _ctx.getMotor().stop();
        delay(100);

        // 左斜め前に移動
        _ctx.getMotor().leftForward(pwm);
        delay(200);
        _ctx.getMotor().rightForward(pwm * 0.7);  // 右を少し弱く
        delay(200);
        _ctx.getMotor().stop();
        delay(100);

        // 右斜め後ろに移動（戻る）
        _ctx.getMotor().rightBackward(pwm);
        delay(200);
        _ctx.getMotor().leftBackward(pwm * 0.7);
        delay(200);
        _ctx.getMotor().stop();
        delay(100);

        // 左斜め後ろに移動（戻る）
        _ctx.getMotor().leftBackward(pwm);
        delay(200);
        _ctx.getMotor().rightBackward(pwm * 0.7);
        delay(200);
        _ctx.getMotor().stop();
        delay(100);
    }
}

// 渦巻き脱出：渦巻き状の動作で周囲を探索してから中心に戻る
void EscapeState::spiralEscape(int pwm, int repeat) {
    for (int i = 0; i < repeat; ++i) {
        int duration = 150 + (i * 50);  // 徐々に動作時間を延ばす

        // 前進しながら右旋回
        _ctx.getMotor().rightForward(pwm);
        delay(duration);
        _ctx.getMotor().leftForward(pwm * 0.5);
        delay(duration);
        _ctx.getMotor().stop();
        delay(100);

        // 90度回転
        _ctx.getMotor().turnLeft(pwm);
        delay(150);
        _ctx.getMotor().stop();
        delay(100);
    }

    _ctx.getMotor().stop();
}

6. DETECTION

カメラで画像を取り、物体検知を行う状態です。撮影対象となるパーツ（A-PARTS：浮遊ローバ）をEdge Impulseによる物体検出で捉えます（詳細は「Edge Impulseによる物体検出」セクション参照）。

検出処理の流れ

200msごとに画像を撮影し、推論を実行
A-PARTSを検出したら、その方向に旋回して接近
検出失敗が25回続いた場合は次の状態へ遷移
検出成功時は、検出した画像をSDカードに保存


DetectionState.hpp
/**
 * @file DetectionState.hpp
 */

#pragma once

#include <Arduino.h>
#include "Controller/ICansatState.hpp"

#define EI_CAMERA_RAW_FRAME_BUFFER_COLS 160  ///< Edge Impulse用の生フレームバッファの幅
#define EI_CAMERA_RAW_FRAME_BUFFER_ROWS 120  ///< Edge Impulse用の生フレームバッファの高さ
#define EI_CAMERA_RAW_FRAME_BUFFER_BYTES 2   ///< YUV422のピクセルあたりバイト数

#define OUTPUT_WIDTH 96   ///< ML推論用にリサイズ後の画像幅
#define OUTPUT_HEIGHT 96  ///< ML推論用にリサイズ後の画像高さ

#define ALIGN_PTR(p,a)   ((p & (a-1)) ?(((uintptr_t)p + a) & ~(uintptr_t)(a-1)) : p)  ///< ポインタを指定アラインメントに揃えるマクロ

/**
 * @struct DetectionObject
 * @brief 検出されたオブジェクトの情報
 */
struct DetectionObject {
	float value;     ///< 検出の信頼度（0.0-1.0）
	uint32_t x;      ///< オブジェクトのX座標
	uint32_t y;      ///< オブジェクトのY座標
};

/**
 * @struct DetectionResult
 * @brief 物体検出の結果を格納する構造体
 */
struct DetectionResult {
	bool has_detection;                    ///< 検出があったかどうか
	uint32_t detection_count;              ///< 検出されたオブジェクトの数
	DetectionObject detected_objects[10];  ///< 検出されたオブジェクトの配列（最大10個）
};

/**
 * @class DetectionState
 * @brief 検知状態：A-parts物体検知を行う
 */
class DetectionState : public ICansatState {
public:
	/**
	 * @brief コンストラクタ
	 * @param ctx CansatControllerへの参照
	 */
	DetectionState(CansatController& ctx) : _ctx(ctx), _result() {}

	/**
	 * @brief 状態開始時の初期化処理
	 */
	void onEnter() override;

	/**
	 * @brief 状態の更新処理（200msごとに呼ばれる）
	 */
	void onUpdate() override;

	/**
	 * @brief 状態終了時の処理
	 */
	void onExit() override;

	/**
	 * @brief 現在の状態を取得
	 * @return State::DETECTION
	 */
	State getState() const override;

private:
	CansatController& _ctx;           ///< CansatControllerへの参照
	int _failedCount = 0;             ///< 検出失敗回数のカウンタ
	bool _isInitCamera = false;       ///< カメラが初期化済みかどうか
	bool _isInitEdgeImpulse = false;  ///< Edge Impulseが初期化済みかどうか
	DetectionResult _result;          ///< 物体検出の結果
	int _succeedCount = 0;            ///< 検出成功回数のカウンタ

	/**
	 * @brief DETECTION_MODEでカメラを初期化
	 * @return 成功時true
	 */
	bool setDetectionMode();

	/**
	 * @brief カメラを終了
	 */
	void endDetectionMode();

	static uint8_t *_image_buffer_base;      ///< Edge Impulse用の画像バッファ（ベースアドレス）
	static uint8_t *_current_image_buffer;   ///< Edge Impulse用の画像バッファ（アラインメント済み）

	/**
	 * @brief Edge Impulseの推論用にピクセルデータを取得
	 * @param offset ピクセルオフセット
	 * @param length 取得するピクセル数
	 * @param out_ptr 出力先のfloat配列
	 * @return 常に0
	 */
	static int get_image_data(size_t offset, size_t length, float *out_ptr);

	/**
	 * @brief Edge Impulseの初期化（画像バッファ確保）
	 * @return 成功時true
	 */
	bool beginEdgeImpulse(void);

	/**
	 * @brief Edge Impulseの終了（画像バッファ解放）
	 */
	void endEdgeImpulse(void);

	/**
	 * @brief YUV422形式をRGB888形式に変換
	 * @param yuv_buffer YUV422画像データ
	 * @param yuv_size YUV422データのサイズ
	 * @return 成功時true
	 */
	bool convertYUV422ToRGB888(const uint8_t *yuv_buffer, size_t yuv_size);

	/**
	 * @brief 画像を96x96にリサイズ
	 * @return 常にtrue
	 */
	bool resizeImage(void);

	/**
	 * @brief Edge Impulseで物体検出を実行
	 * @return 成功時true
	 */
	bool detectObjects(void);
};


DetectionState.cpp
/**
 * @file DetectionState.cpp
 */

#include "Controller/States/DetectionState.hpp"
#include "Controller/States/RecordingState.hpp"
#include "Controller/CansatController.hpp"

#include "a_parts_blackrock_inferencing.h"
#include <edge-impulse-sdk/dsp/image/image.hpp>

// Edge Impulse用の画像バッファ（静的メンバ変数）
uint8_t *DetectionState::_image_buffer_base = nullptr;
uint8_t *DetectionState::_current_image_buffer = nullptr;

void DetectionState::onEnter() {
	_ctx.writeSystemLog("%lu: Entering DetectionState", millis());

	_ctx.getSpeaker().playState((int)State::DETECTION);
	_ctx.setLed((int)State::DETECTION);

	// DETECTION_MODE（QQVGA YUV422）でカメラを初期化
	if (!setDetectionMode()) {
		_ctx.writeSystemLog("%lu: Failed to set detection mode", millis());
	} else {
		_ctx.writeSystemLog("%lu: Detection mode set", millis());
		// Edge Impulse用の画像バッファを確保
		if (!beginEdgeImpulse()) {
			_ctx.writeSystemLog("%lu: Failed to initialize Edge Impulse", millis());
		} else {
			_ctx.writeSystemLog("%lu: Succeed to initialize Edge Impulse", millis());
		}
	}
}

void DetectionState::onUpdate() {
	_ctx.getSerialWriter().log("Updating DetectionState");

	// 失敗回数をカウントし、上限に達したらRecordingStateへ遷移
	_failedCount++;
	if (_failedCount >= _ctx.getUserConfig().detectionMaxFailedCount) {
		_ctx.writeSystemLog("%lu: Failed too many times. Change to RecordingState", millis());
		_ctx.changeState(std::make_unique<RecordingState>(_ctx));
		return;
	}

	if (_isInitEdgeImpulse) {
		// 画像を撮影する（QQVGA YUV422形式）
    	void* imgBuff = nullptr;
    	size_t imgSize = 0;
    	if (!_ctx.getCamera().takePicture(&imgBuff, &imgSize)) {
			_ctx.getSerialWriter().log("Failed to take picture");
		   return;
		}

		// YUV422をRGB888に変換
		if (!convertYUV422ToRGB888(imgBuff, imgSize)) {
			_ctx.getSerialWriter().log("Failed to convert YUV422 to RGB888");
			return;
		}

		// 96x96にリサイズ
		if (!resizeImage()) {
			_ctx.getSerialWriter().log("Failed to resize image");
			return;
		}

		// デバッグ用：リサイズ後の画像をSDカードに保存（コメントアウト中）
		// if (!_ctx.getSDLogger().savePPMImage(_current_image_buffer, OUTPUT_WIDTH * OUTPUT_HEIGHT * 3)) {
		// 	_ctx.getSerialWriter().log("Failed to save resized image");
		// 	return;
		// }

		// Edge Impulseで物体検出を実行
		if (!detectObjects()) {
			_ctx.getSerialWriter().log("Failed to detect objects");
			return;
		}

		// A-partsを検出した場合の処理
		if (_result.has_detection) {
			int x = _result.detected_objects[0].x;
			_ctx.writeSystemLog("A-parts detected %f x=%d y=%d", _result.detected_objects[0].value, _result.detected_objects[0].x, _result.detected_objects[0].y);

			// 検出した画像をSDカードに保存
			if (!_ctx.getSDLogger().savePPMImage(_current_image_buffer, OUTPUT_WIDTH * OUTPUT_HEIGHT * 3)) {
				_ctx.getSerialWriter().log("Failed to save resized image");
			 	return;
			}

			// 検出成功回数に応じた動作
			switch (_succeedCount) {
			case 0:
				// 1回目：180度回転してA-partsに接近
				_ctx.getMotor().turnLeft(150);
				delay(_ctx.getUserConfig().detectionTurn180delay);
				_ctx.getMotor().stop();
				delay(100);
				_ctx.getMotor().snakeForwardSmooth(150, 4000, 1000);
				delay(100);
				break;
			case 1:
				// 2回目：RecordingStateへ遷移（A-partsの近くに到達）
				_ctx.writeSystemLog("%lu: A-parts detected. Changing to RecordingState, millis()");
		 		_ctx.changeState(std::make_unique<RecordingState>(_ctx));
				return;
			default:
				_succeedCount = 0;
				break;
			}
			_succeedCount++;
		}
	}

	// 検出できなかった場合、左旋回して探索を続ける
	_ctx.getMotor().turnLeft(150);
	delay(200);
	_ctx.getMotor().stop();
}

void DetectionState::onExit() {
	_ctx.writeSystemLog("%lu: Exiting DetectionState", millis());

	// Edge Impulseの終了（画像バッファ解放）
	if (_isInitEdgeImpulse) {
		endEdgeImpulse();
	}

	// カメラの終了
	if (_isInitCamera) {
		endDetectionMode();
	}

	// 次の状態（RECORDING）をSDカードに保存
	if (!_ctx.getSDLogger().writeState((int)State::RECORDING)) {
		_ctx.getSerialWriter().log("Failed to write state in SD");
	}

#ifdef USE_FLASH
	// USE_FLAGが定義されている場合はFlashにも保存
	if (!_ctx.getFlashIO().writeState((int)State::RECORDING)) {
		_ctx.getSerialWriter().log("Failed to write state in Flash");
	}
#endif // USE_FLASH
}

State DetectionState::getState() const {
	return State::DETECTION;
}

// DETECTION_MODE（QQVGA YUV422）でカメラを初期化
bool DetectionState::setDetectionMode() {
	if (!_ctx.getCamera().begin(DETECTION_MODE)) {
		return false;
	}

	if (!_ctx.getCamera().startStreaming(true)) {
		return false;
	}

	_isInitCamera = true;
	return true;
}

// カメラの終了
void DetectionState::endDetectionMode() {
	_ctx.getCamera().startStreaming(false);
	_ctx.getCamera().end();
	_isInitCamera = false;
}

// Edge Impulseの初期化：RGB888画像用のバッファを確保（32バイトアラインメント）
bool DetectionState::beginEdgeImpulse(void) {
	_image_buffer_base = (uint8_t*)ei_malloc(EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS * 3 + 32);
	if (_image_buffer_base == nullptr) {
		return false;
	}
    _current_image_buffer = (uint8_t *)ALIGN_PTR((uintptr_t)_image_buffer_base, 32);

	_isInitEdgeImpulse = true;
	return true;
}

// Edge Impulseの終了：画像バッファを解放
void DetectionState::endEdgeImpulse(void) {
	if (_image_buffer_base) {
		ei_free(_image_buffer_base);
	}
	_image_buffer_base = nullptr;
	_current_image_buffer = nullptr;
	_isInitEdgeImpulse = false;
}

// YUV422形式（カメラ出力）をRGB888形式（ML入力）に変換
bool DetectionState::convertYUV422ToRGB888(const uint8_t *yuv_buffer, size_t yuv_size) {
	if (!yuv_buffer) {
		return false;
	}

	if (ei::EIDSP_OK != ei::image::processing::yuv422_to_rgb888(
			_current_image_buffer, yuv_buffer, yuv_size, ei::image::processing::BIG_ENDIAN_ORDER)) {
		return false;
	}

	return true;
}

// 160x120の画像を96x96にリサイズ（crop_and_interpolate）
bool DetectionState::resizeImage(void) {
	ei::image::processing::crop_and_interpolate_rgb888(
		_current_image_buffer,
		EI_CAMERA_RAW_FRAME_BUFFER_COLS,
		EI_CAMERA_RAW_FRAME_BUFFER_ROWS,
		_current_image_buffer,
		OUTPUT_WIDTH,
		OUTPUT_HEIGHT
	);

	return true;
}

// Edge Impulseで物体検出を実行（run_classifier）
bool DetectionState::detectObjects(void) {
	// 結果をリセット
	_result.has_detection = false;
	_result.detection_count = 0;
	memset(&_result.detected_objects, 0, sizeof(_result.detected_objects));

	// 画像データのシグナルを作成
	ei::signal_t signal;
	signal.total_length = OUTPUT_WIDTH * OUTPUT_HEIGHT;
	signal.get_data = &get_image_data;

	ei_impulse_result_t ei_result = { 0 };

	// Edge Impulseの推論を実行
	EI_IMPULSE_ERROR err = run_classifier(&signal, &ei_result, false);
	if (err != EI_IMPULSE_OK) {
		return false;
	}

	// 検出結果（bounding boxes）を_resultに格納
	for (uint32_t i = 0; i < ei_result.bounding_boxes_count && i < 10; i++) {
		ei_impulse_result_bounding_box_t bb = ei_result.bounding_boxes[i];
		if (bb.value > 0) {
			_result.detected_objects[_result.detection_count].value = bb.value;
			_result.detected_objects[_result.detection_count].x = bb.x;
			_result.detected_objects[_result.detection_count].y = bb.y;

			_result.has_detection = true;
			_result.detection_count++;
		}
	}

	return true;
}

// Edge Impulseの推論コールバック：RGB888データをfloat配列に変換
int DetectionState::get_image_data(size_t offset, size_t length, float *out_ptr) {
    size_t pixel_ix = offset * 3;
	size_t pixels_left = length;
	size_t out_ptr_ix = 0;

	// RGB888（3バイト）を24bitのfloat値に変換
	while (pixels_left != 0) {
		out_ptr[out_ptr_ix] = (_current_image_buffer[pixel_ix] << 16) + (_current_image_buffer[pixel_ix + 1] << 8) + _current_image_buffer[pixel_ix + 2];

		out_ptr_ix++;
		pixel_ix += 3;
		pixels_left--;
	}

    return 0;
}

7. RECORDING

浮遊ローバが発信する動画を撮影する状態です


RecordingState.hpp
/**
 * @file RecordingState.hpp
 */

#pragma once
#include "Controller/ICansatState.hpp"
#include <Camera.h>

/**
 * @class RecordingState
 * @brief 録画状態：A-parts発射の動画を撮影する
 */
class RecordingState : public ICansatState {
public:
	/**
	 * @brief コンストラクタ
	 * @param ctx CansatControllerへの参照
	 */
	RecordingState(CansatController& ctx) : _ctx(ctx) {}

	/**
	 * @brief 状態開始時の初期化処理
	 */
	void onEnter() override;

	/**
	 * @brief 状態の更新処理（200msごとに呼ばれる）
	 */
	void onUpdate() override;

	/**
	 * @brief 状態終了時の処理
	 */
	void onExit() override;

	/**
	 * @brief 現在の状態を取得
	 * @return State::RECORDING
	 */
	State getState() const override;

	/**
	 * @brief カメラコールバック関数（静的メソッド）
	 * @param img カメラ画像
	 */
	static void CamCB(CamImage img);

private:
	CansatController& _ctx;             ///< CansatControllerへの参照
	unsigned long _startTime;           ///< 録画開始時刻（タイムアウト判定用）
	bool _isInitCamera = false;         ///< カメラが初期化済みかどうか
	static RecordingState *_instance;   ///< 静的コールバック用のインスタンスポインタ
	bool _isRecordingOK = false;        ///< 録画開始可能フラグ

	// エラーハンドリング用メンバ変数
	bool _recordingError = false;       ///< 録画エラーフラグ
	bool _aviRecordingActive = false;   ///< AVI録画がアクティブかどうか
	uint32_t _frameCount = 0;           ///< 録画されたフレーム数

	/**
	 * @brief VIDEO_MODEでカメラを初期化
	 * @return 成功時true
	 */
	bool setRecordingMode();

	/**
	 * @brief AVI録画を実行
	 * @param time_ms 録画時間（ミリ秒）
	 * @return 成功時true
	 */
	bool record(int time_ms);

	/**
	 * @brief カメラを終了
	 */
	void endRecordingMode();

	/**
	 * @brief カメラ画像を処理してAVIファイルに追加
	 * @param img カメラ画像
	 */
	void handleCameraImage(CamImage img);
};


RecordingState.cpp
/**
 * @file RecordingState.cpp
 */

#include "Controller/States/RecordingState.hpp"
#include "Controller/States/ExploreState.hpp"
#include "Controller/CansatController.hpp"

// 静的コールバック用のインスタンスポインタ
RecordingState* RecordingState::_instance = nullptr;

void RecordingState::onEnter() {
	_instance = this;
	_ctx.writeSystemLog("%lu: Entering RecordingState", millis());

	_ctx.getSpeaker().playState((int)State::RECORDING);
	_ctx.setLed((int)State::RECORDING);

  	if (!_ctx.isConnectTwelite()) {
    	_ctx.getTwelite().on();
    	_ctx.setIsConnectTwelite(true);
  	}

	delay(200);

	// VIDEO_MODE（QVGA 30fps JPEG）でカメラを初期化
	if (!setRecordingMode()) {
		_ctx.writeSystemLog("%lu: Failed to set recording mode", millis());
		_recordingError = true;
	} else {
		_ctx.writeSystemLog("%lu: Recording mode set", millis());
	}

	_startTime = millis();  // タイムアウト判定用の開始時刻を記録
}

void RecordingState::onUpdate() {
	_ctx.getSerialWriter().log("Updating RecordingState");

	// エラー状態の場合は早期終了
	if (_recordingError) {
		_ctx.writeSystemLog("%lu: RecordingState in error state, transitioning to ExploreState", millis());
		_ctx.changeState(std::make_unique<ExploreState>(_ctx));
		return;
	}

	// Twelite通信でReadyForCaptureパケットを送信
	twelite::Packet pkt = twelite::TwelitePacket::makePacket(
		twelite::B_PARTS,
		twelite::A_PARTS,
		twelite::ReadyForCapture,
		0,
		NULL
	);
	_ctx.getTwelite().sendPacket(pkt);

	if (_ctx.getTwelite().receivePacket(pkt)) {
		if (twelite::TwelitePacket::match(pkt, twelite::A_PARTS, twelite::B_PARTS, twelite::ReadyForCaptureAck)) {
			_ctx.writeSystemLog("%lu: ReadyForCaptureAck received. Recording start", millis());
			_isRecordingOK = true;
		}
	}

	// タイムアウト判定（config.jsonで設定可能）
	unsigned long elapsedTime = millis() - _startTime;
	_ctx.getSerialWriter().logf("Elapsed time: %lu", elapsedTime);
	if (elapsedTime > _ctx.getUserConfig().recordingTimeoutThreshold) {
		_ctx.writeSystemLog("%lu: Timeout. Recording start", millis());
		_isRecordingOK = true;
	}

	// 録画開始フラグが立ったら録画を実行
	if (_isRecordingOK) {
		if (record(_ctx.getUserConfig().recordingTime)) {
			_ctx.writeSystemLog("%lu: Finished recording successfully. Changing to ExploreState", millis());
		} else {
			_ctx.writeSystemLog("%lu: Recording failed but partial video may be saved. Changing to ExploreState", millis());
		}
		_ctx.changeState(std::make_unique<ExploreState>(_ctx));
		return;
	}
}

void RecordingState::onExit() {
	_ctx.writeSystemLog("%lu: Exiting RecordingState", millis());

	// 録画エラーの緊急停止処理
	if (_aviRecordingActive) {
		_ctx.writeSystemLog("%lu: Emergency AVI stop during state exit", millis());
		_ctx.getSDLogger().aviEmergencyStop();
		_aviRecordingActive = false;
	}

	// カメラの終了
	if (_isInitCamera) {
		endRecordingMode();
	}

	// 次の状態（EXPLORE）をSDカードに保存
	if (!_ctx.getSDLogger().writeState((int)State::EXPLORE)) {
		_ctx.getSerialWriter().log("Failed to write state in SD");
	}

#ifdef USE_FLASH
	// USE_FLAGが定義されている場合はFlashにも保存
	if (!_ctx.getFlashIO().writeState((int)State::EXPLORE)) {
		_ctx.getSerialWriter().log("Failed to write state in Flash");
	}
#endif // USE_FLASH

	_instance = nullptr;
}

State RecordingState::getState() const {
	return State::RECORDING;
}

// VIDEO_MODE（QVGA 30fps JPEG）でカメラを初期化し、コールバックを設定
bool RecordingState::setRecordingMode() {
	if (!_ctx.getCamera().begin(VIDEO_MODE)) {
		_ctx.writeSystemLog("%lu: Camera begin failed", millis());
		return false;
	}

	if (!_ctx.getCamera().startStreaming(true, CamCB)) {
		_ctx.writeSystemLog("%lu: Camera streaming start failed", millis());
		return false;
	}

	_isInitCamera = true;
	return true;
}

// AVI録画を実行（指定時間録画してSDカードに保存）
bool RecordingState::record(int time_ms) {
	_ctx.writeSystemLog("%lu: Recording started", millis());

	// AVI初期化（QVGA解像度）
	if (!_ctx.getSDLogger().aviInit(CAM_IMGSIZE_QVGA_H, CAM_IMGSIZE_QVGA_V)) {
		_ctx.writeSystemLog("%lu: AVI initialization failed: %s", millis(),
			_ctx.getSDLogger().aviGetErrorMessage());
		return false;
	}

	// AVI録画開始
	if (!_ctx.getSDLogger().aviStart()) {
		_ctx.writeSystemLog("%lu: AVI recording start failed: %s", millis(),
			_ctx.getSDLogger().aviGetErrorMessage());
		return false;
	}

	_aviRecordingActive = true;
	_frameCount = 0;
	_recordingError = false;

	uint32_t start_time = millis();
	uint32_t last_status_time = start_time;

	// 録画ループ（指定時間またはエラー発生まで）
	while ((millis() - start_time) < time_ms) {
		// エラーチェック
		if (_ctx.getSDLogger().aviHasFailed()) {
			_ctx.writeSystemLog("%lu: AVI recording error detected: %s", millis(),
				_ctx.getSDLogger().aviGetErrorMessage());
			break;  // エラー発生時はループを抜ける
		}

		delay(10);
	}

	// 録画終了処理
	bool success = true;
	if (!_ctx.getSDLogger().aviEnd()) {
		_ctx.writeSystemLog("%lu: AVI recording end failed: %s", millis(),
			_ctx.getSDLogger().aviGetErrorMessage());
		success = false;
	}

	_aviRecordingActive = false;

	// カメラストリーミング停止
	if (!_ctx.getCamera().startStreaming(false)) {
		_ctx.writeSystemLog("%lu: Camera streaming stop failed", millis());
		success = false;
	}

	// 録画結果の報告
	uint32_t actual_duration = millis() - start_time;
	if (success) {
		_ctx.writeSystemLog("%lu: Recording finished successfully. Duration: %u ms, Frames: %u",
			millis(), actual_duration, _frameCount);
	} else {
		_ctx.writeSystemLog("%lu: Recording finished with errors. Duration: %u ms, Frames: %u",
			millis(), actual_duration, _frameCount);
	}

	return success;
}

// カメラの終了
void RecordingState::endRecordingMode() {
	_ctx.getCamera().end();
	_isInitCamera = false;
}

// 静的コールバック関数：カメラからフレームが到着したときに呼ばれる
void RecordingState::CamCB(CamImage img) {
	if (_instance) {
		_instance->handleCameraImage(img);
	}
}

// カメラ画像を処理してAVIファイルに追加
void RecordingState::handleCameraImage(CamImage img) {
	if (!img.isAvailable()) {
		_ctx.writeSystemLog("%lu: Invalid camera image received", millis());
		return;
	}

	// 録画がアクティブでない場合は処理しない
	if (!_aviRecordingActive || _recordingError) {
		return;
	}

	void* imgBuff = img.getImgBuff();
	size_t imgSize = img.getImgSize();

	// フレームをAVIファイルに追加
	if (!_ctx.getSDLogger().aviRecord(imgBuff, imgSize)) {
		_ctx.writeSystemLog("%lu: Frame recording failed: %s", millis(),
			_ctx.getSDLogger().aviGetErrorMessage());
		_recordingError = true;
		return;
	}

	_frameCount++;
}

8. EXPLORE

周辺を散策し、調査する状態です


ExploreState.hpp
/**
 * @file ExploreState.hpp
 */

#pragma once
#include "Controller/ICansatState.hpp"

/**
 * @class ExploreState
 * @brief 探索状態：自律移動と周辺撮影を行う
 */
class ExploreState : public ICansatState {
public:
	/**
	 * @brief コンストラクタ
	 * @param ctx CansatControllerへの参照
	 */
	ExploreState(CansatController& ctx) : _ctx(ctx) {}

	/**
	 * @brief 状態開始時の初期化処理
	 */
	void onEnter() override;

	/**
	 * @brief 状態の更新処理（200msごとに呼ばれる）
	 */
	void onUpdate() override;

	/**
	 * @brief 状態終了時の処理
	 */
	void onExit() override;

	/**
	 * @brief 現在の状態を取得
	 * @return State::EXPLORE
	 */
	State getState() const override;

private:
	CansatController& _ctx;        ///< CansatControllerへの参照
	bool _isInitCamera = false;    ///< カメラが初期化済みかどうか

	/**
	 * @brief PHOTO_MODEでカメラを初期化
	 * @return 成功時true
	 */
	bool setPhotoMode();

	/**
	 * @brief カメラを終了
	 */
	void endPhotoMode();
};


ExploreState.cpp
/**
 * @file ExploreState.cpp
 */

#include "Controller/States/ExploreState.hpp"
#include "Controller/CansatController.hpp"

void ExploreState::onEnter() {
	_ctx.writeSystemLog("%lu: Entering ExploreState", millis());

	_ctx.getSpeaker().playState((int)State::EXPLORE);
	_ctx.setLed((int)State::EXPLORE);

	// PHOTO_MODEでカメラを初期化
	if (!setPhotoMode()) {
		_ctx.writeSystemLog("%lu: Failed to set explore mode", millis());
	} else {
		_ctx.writeSystemLog("%lu: Explore mode set", millis());
	}
}

void ExploreState::onUpdate() {
	_ctx.getSerialWriter().log("Updating ExploreState");

	// 蛇行前進（PWM150、2秒間、周期1ms）
	// // 適当に移動
	_ctx.getMotor().snakeForwardSmooth(150, 2000, 1);
	// _ctx.getMotor().forward(150);
	// delay(random(500, 1500));
	// _ctx.getMotor().stop();
	delay(100);

	// ランダムに左右旋回（200-500ms）
	if (random(2) == 0) {
		_ctx.getMotor().turnLeft(150);
	} else {
		_ctx.getMotor().turnRight(150);
	}
	delay(random(200, 500));
	_ctx.getMotor().stop();
	delay(100);

	// 画像を撮影してSDカードに保存
    void* imgBuff = nullptr;
    size_t imgSize = 0;
    if (_ctx.getCamera().takePicture(&imgBuff, &imgSize)) {
        _ctx.getSerialWriter().log("Save taken picture to SD card...");
        _ctx.getSDLogger().saveJPEGImage(imgBuff, imgSize);
    } else {
		_ctx.getSerialWriter().log("Failed to take picture");
    }
}

void ExploreState::onExit() {
	_ctx.writeSystemLog("%lu: Exiting ExploreState", millis());

	// カメラの終了
	if (_isInitCamera) {
		endPhotoMode();
	}
}

State ExploreState::getState() const {
	return State::EXPLORE;
}

// PHOTO_MODEでカメラを初期化
bool ExploreState::setPhotoMode() {
	if (!_ctx.getCamera().begin(PHOTO_MODE)) {
		return false;
	}

	if (!_ctx.getCamera().startStreaming(true)) {
		return false;
	}

	_isInitCamera = true;
	return true;
}

// カメラの終了
void ExploreState::endPhotoMode() {
	_ctx.getCamera().end();
}

センサ・アクチュエータなどを制御するクラス

以下のクラスは汎用的なセンサ・アクチュエータの制御を行うものです。詳細なソースコードはGitHubリポジトリを参照してください。

アクチュエータ

内蔵LED制御: 状態表示やデバッグ用のLED制御
- Led.hpp / Led.cpp
タイヤ・モーター制御: TB6612モータードライバを使用した走行制御
- Motor.hpp / Motor.cpp
スピーカー制御: 状態通知用の音声出力
- Speaker.hpp / Speaker.cpp

センサ

カメラ制御: SPRESENSEカメラモジュールの制御
- CameraController.hpp / CameraController.cpp
CdS制御: 照度センサによる放出検知
- CdSSensor.hpp / CdSSensor.cpp
内蔵GNSS制御: GPS測位とデータ取得
- GnssSensor.hpp / GnssSensor.cpp

ユーティリティ

ファイル入出力: SD/Flashへのログ保存
電源管理: バッテリー電圧監視
- PowerController.hpp / PowerController.cpp
シリアル出力: デバッグ出力
- SerialWriter.hpp / SerialWriter.cpp

サブモジュール

9軸IMU（BNO055）制御: 姿勢・加速度・地磁気センサ
- BNO055Library.h / BNO055Library.cpp
動画の録画: AVI形式での動画録画
- PosixAvi.hpp / PosixAvi.cpp
TWELITE通信: 機体間無線通信
- TwelitePacket.h / TwelitePacket.cpp

コメント無し