feat: 実機ロボット制御挙動の再現機能を追加

[HansRobo]

概要

grSimシミュレータで実機ロボット（Orion）と同じ制御アルゴリズム・挙動を再現する機能を実装しました。

背景

チーム（Orion）の実機ロボットとgrSimシミュレータで、以下の差異がありました：

項目	grSim	実機 (G474_Orion)
ホイール角度	60°, 135°, 225°, 300°	30°, 315°, 225°, 150°
ホイール半径	0.027m	0.028m
ロボット半径	0.09m	0.08m
最大速度	5 m/s	7 m/s
逆運動学	標準式	Orion独自式
加速度制御	固定 4 m/s²	方向依存
滑り補正	なし	X:1.1倍, Y:1.3倍

主な変更

1. RobotSettings構造体の拡張 (include/configwidget.h)

高度な制御パラメータを追加：

• AccelDeccelRatio: 減速時加速度倍率（デフォルト: 1.0）
• AccelBackSideRatio: 後方加速時倍率（デフォルト: 1.0）
• SlipCorrectionX/Y: 滑り補正係数（デフォルト: 1.0）
• KinematicsMode: 逆運動学モード（0:標準, 1:Orion, デフォルト: 0）

2. 設定読み込み処理 (src/configwidget.cpp)

AdvancedControlセクションから新パラメータを読み込み、デフォルト値で後方互換性を確保。

3. Robot::setSpeed()の改修 (src/robot.cpp)

実機のcontrol_speed.cを参考に以下を実装：

滑り補正

dReal corrected_vx = vx * cfg->robotSettings.SlipCorrectionX;
dReal corrected_vy = vy * cfg->robotSettings.SlipCorrectionY;

方向依存の加速度制御

• 後方加速時: 上限クリップ処理（実機の実装に忠実）
• 減速時: vel_now[i] * accel[i] <= 0 による減速判定で係数適用

2つの逆運動学モード

• モード0（標準）: (R*ω - vx*sin(α) + vy*cos(α)) / R_wheel
• モード1（Orion）: (vy*sin(α) + vx*cos(α) + R*ω) / (π*D)

4. Orion.ini設定ファイルの追加 (config/Orion.ini)

実機パラメータを設定：

[Geometery]
Radius = 0.080
WheelRadius = 0.028
Wheel1Angle = 30
Wheel2Angle = 315
Wheel3Angle = 225
Wheel4Angle = 150

[Physics]
VelAbsoluteMax=7.0

[AdvancedControl]
AccelDeccelRatio = 1.8
AccelBackSideRatio = 0.7
SlipCorrectionX = 1.1
SlipCorrectionY = 1.3
KinematicsMode = 1

実機コードとの整合性

実機（G474_Orion）のCore/Src/control_speed.cの実装を参考に、以下を忠実に再現：

• 後方加速時の上限クリップ処理（line 46-48）
• 速度と加速度の符号による減速判定（line 52-54）
• X/Y方向それぞれでの減速判定

後方互換性

✅ 既存の設定ファイル（Parsian.ini等）は変更不要
✅ 新パラメータはすべてデフォルト値で標準動作を維持
✅ 既存チームの動作に影響なし

テスト方法

1. grSimを起動
2. GUI設定でBlue TeamまたはYellow TeamをOrionに設定
3. 実機と同じコマンドを送信して挙動を比較

検証結果

逆運動学の動作を検証プログラムで確認済み：

• 標準モードとOrionモードで異なるホイール速度が出力される
• ホイール角度の違いが正しく反映される

🤖 Generated with Claude Code