制御モードと4WS通信の設計案¶
この文書は未実装の設計案を扱う。現在の送受信仕様は制御パケット、フィードバックパケットを参照する。
適用範囲と実装状況¶
| mode | 用途 | Orion | 4WS |
|---|---|---|---|
| 3 | 極座標速度目標 | 実装済み | 未実装 |
| 4 | 位置目標・終端速度 | CM4でmode 3に変換 | 未実装 |
| 5 | 各輪の駆動周速度と操舵角 | 対象外 | 未実装 |
| 6 | 各輪の駆動周速度 | 未実装 | 対象外 |
| 7 | ボール基準の相対速度 | 未実装 | 未実装 |
| 8 | ボール基準の相対位置 | 未実装 | 未実装 |
指令本体は64バイトを共用する案である。mode 4の出力をOrionではmode 3または6、4WSではmode 3または5から選ぶ規則と座標変換は未定義。未対応モードの拒否・安全停止も実装課題であり、現在のブリッジが拒否を保証するものではない。現在の実装にmode 5～8を送って動作確認しない。
モード別の配置案¶
FOUR_WHEEL_STEERING_TARGET_MODE (5)¶
4WS Mainへ直接送る4輪駆動・4輪操舵の目標である。指令本体byte 23を5とし、byte 0..23の共通フィールドを使用する。
| 指令本体のbyte | 内容 |
|---|---|
| 24..25 | モジュール0の駆動周速度 [m/s] |
| 26..27 | モジュール0の操舵角 [rad] |
| 28..31 | モジュール1の駆動周速度、操舵角 |
| 32..35 | モジュール2の駆動周速度、操舵角 |
| 36..39 | モジュール3の駆動周速度、操舵角 |
| 40..63 | 予約、0 |
各値は2バイトで上位バイトから格納する。raw = uint16_t(32767 × (x / R) + 32767)、x = (raw - 32767) × R / 32767とする。速度のRは32.767 m/s、操舵角のRは10π rad。0x7FFFは0、0xFFFFは未使用値として拒否する。範囲外の値は黙って飽和させず拒否する。車輪番号と配置、回転・操舵の正方向、機構上の許容範囲は実装前に定義する。STOP_EMERGENCYは輪の目標より優先する。
mode 5のbyte 32..37は輪の目標であり、mode 4の位置目標として復号しない。crane、CM4、4WS Mainにmode 5用のシリアライザ・デシリアライザが必要である。4WS Mainの通信・制御は未実装であり、対応するまでmode 5を走行指令として送らない。
OMNI_WHEEL_SPEED_TARGET_MODE (6)¶
OrionMainへ直接送る4輪の駆動周速度目標である。指令本体byte 23を6とする。mode 5から操舵角を除いた配置を使う。
| 指令本体のbyte | 内容 |
|---|---|
| 24..25 | 車輪0の駆動周速度 [m/s] |
| 26..27 | 車輪1の駆動周速度 [m/s] |
| 28..29 | 車輪2の駆動周速度 [m/s] |
| 30..31 | 車輪3の駆動周速度 [m/s] |
| 32..63 | 予約、0 |
各速度はmode 5と同じ2バイト符号化を使用する。符号化範囲は±32.767 m/sで、上位バイト、下位バイトの順に格納する。0x7FFFは0、0xFFFFは無効値とする。機構上の速度上限、車輪番号と配置、正回転方向は実装前に定義する。STOP_EMERGENCYは車輪目標より優先する。
mode 6ではbyte 32..37をmode 4の位置目標として復号しない。送信側とCM4側にはmode 6専用のシリアライザとデシリアライザが必要である。OrionMain側の受信・制御も未実装である。
BALL_RELATIVE_VELOCITY_MODE (7)・BALL_RELATIVE_POSITION_MODE (8)¶
mode 7・8はCM4内部の制御にローカルカメラ観測値を反映する。mode 7はボール基準の相対速度、mode 8はボール基準の相対位置を指示する。ボール基準の目標と、ボール未検出時に使う通常の目標は別フィールドに入れる。STOP_EMERGENCYは検出状態と目標値より優先する。
| 指令本体のbyte | mode 7：ボール基準の相対速度 | mode 8：ボール基準の相対位置 |
|---|---|---|
| 24..25 | 未検出時のmode 3 target_global_velocity_r [m/s] |
未検出時のmode 4 terminal_velocity_x [m/s] |
| 26..27 | 未検出時のmode 3 target_global_velocity_theta [rad] |
未検出時のmode 4 terminal_velocity_y [m/s] |
| 28..29 | 検出時のボール基準相対速度 relative_velocity_x [m/s] |
検出時のボール基準相対目標位置 relative_target_x [m] |
| 30..31 | 検出時のボール基準相対速度 relative_velocity_y [m/s] |
検出時のボール基準相対目標位置 relative_target_y [m] |
| 32..33 | 予約、0 | 未検出時のmode 4 TARGET_GLOBAL_POS_X [m] |
| 34..35 | 予約、0 | 未検出時のmode 4 TARGET_GLOBAL_POS_Y [m] |
| 36..37 | 予約、0 | 未検出時のmode 4 TERMINAL_VELOCITY [m/s] |
| 38..63 | 予約、0 | 予約、0 |
各2バイト値は上位バイト先行の符号化を使う。位置と並進速度の符号化範囲は±32.767 mまたはm/s、mode 7の未検出時の方向角はmode 3と同じ±32.767 radとする。実際に許す速度・距離は別途制限する。共通フィールドの目標方位はグローバル方位とし、ボール基準にするのは並進2軸である。
CM4はローカルカメラUDPの最新7バイトから有効なボール観測の有無を判定する。候補条件はradius > 0かつ最終受信から100 ms以内である。カメラが未起動で受信が一度もない場合や、停止・通信断により最終受信から100 msを超えた場合は条件を満たさない。x・yは画像座標、radiusは画像上の半径であり、そのままメートル単位の目標と比較しない。有効な観測がある場合、CM4内部の制御に位置・半径と観測の鮮度を反映し、ボール基準の位置・速度を求め、機体向けのmode 3・5・6指令へ変換する。mode 8の相対目標に到達したときの目標速度は0とする。
有効なボール観測がないときは、mode 7のbyte 24..27からmode 3相当の指令を生成し、mode 8のbyte 24..27と32..37からmode 4相当の指令を生成する。mode 8ではCM4の位置制御を通した後、Orionならmode 3または6、4WSならmode 3または5へ変換する。検出状態の切り替え時はボール追従側の制御状態をリセットし、保存したボール位置を有効な新規観測として使わない。crane指令や機体feedbackが失効した場合は、カメラが見えていても安全停止を優先する。
カメラが出すのは画像座標と半径であり、ボール基準のメートル座標や速度ではない。実装前にカメラ較正、距離推定、軸の向き、ボール速度の推定方法、検出の信頼度と制御周期を決める。変換が未定義のままmode 7・8を走行指令として採用しない。mode 7・8の処理は未実装である。
4WS MainへのSPI転送（仮仕様）¶
4WS MainとはSPIで通信し、CM4がSPIマスター、Mainがスレーブになる。制御指令本体には上記の64バイトRobotCommandSerializedV2を使用する。4WS Mainはmode 3・5を直接受け、mode 4・7・8はCM4がmode 3または5へ変換してから送る。mode 6を4WS Mainへ送らない。
SPIのmode、クロック、CS配線、転送周期、全二重転送の手順は実機で決める。指令本体のbyte 0はOrionMain向けと同じ254とする。転送単位やダミーバイトが必要でも、指令本体に4WS固有のヘッダ、メッセージ種別、長さ、CRC、別の制御ペイロードを追加しない。4WS MainのSPI通信は未実装である。Main→CM4のSPI受信でも共通のフィードバックパケットを使用する。
実装前に確定する事項¶
以下の機器間で共有する未決事項は、この設計案で管理する。4WS Mainのピン・周辺機能の現在設定、実装制約、実機結果はFW実装文書で管理する。
- 輪番号と配置、駆動・操舵の正方向、機構上の許容範囲。
- カメラ較正、画像から距離への変換、ボール速度推定と観測の信頼度。
- モード切替時の制御状態と、指令・feedback・カメラ断の停止条件。
- SPIの通信条件、フレーム境界、全二重転送と再同期。
- crane、CM4、Main、シミュレータを揃えて配布する検証手順。
- CM4とMain、アクチュエーター基板間の制御分担と、指令更新・帰還・制御の各周期。Main内のタイマー設定だけで機器間の周期契約を確定しない。
- 通常操作とボタンによる出力デバッグのモード選択・遷移、指令の優先順位、出力上限・継続条件、切替・停止後の復帰条件。
- 4WSのCANノード・バス・IDと駆動／操舵の対応。既存のOrion CAN割当を4WSへ転用するか、機器側FWと照合して決める。
- 共通feedbackの各項目の取得元、機体軸・原点、校正、有効性・鮮度、未取得時の扱い。配置が共通でも、値の生成と停止判断の実装は別途必要である。