以下の実験を行った。「実機における PID 制御と電流 PI 制御の統合実装: 実機ロボット上で、設計した PID 制御とモータの電流 PI 制御を統合し実装する。ゲイン調整を行いながら、シミュレーション
結果との比較や理論的考察を交え、制御性能（安定性、応答速度等）を評価・検証する。実機を用いて、内側の電流PIゲインと外側の角度PIDゲインを試行錯誤により調整し、目標角度(（60度）への良好な追従性能を実現する。調整の基本サイクル:
➀【角度PID調整-まず動かす】‘angle_kp'を調整し、モーターが目標角度に向かって動き出すことを目指す(P制御)。
➁【電流PI調整-追従性改善】角度制御が動作している状態で、グラフの電流応答
(I_ref VS I_means)を見て電流誤差/遅延が最小になるように`desired_current_Kp'と`desired_current_Ki`を調整する。
➂【角度PID調整-性能向上】電流ループがある程度良好な状態で、グラフの角度応答(〖 θ〗_ref  VS  θ_means )を見て、 望ましい動作(速応性、安定性、定常偏差ゼロ)になるように`angle_kp','angle_kd`,'angle_ki`を調整する(PD制御、PID制御)。
④【繰り返し】ステップ3の調整でI_refの挙動が変わり電流追従が悪化したら、ステップ➁に戻ってPIゲインを再調整する。目標性能に達するまで、ステップ➁と➂を繰り返す。
」以下のコードで行った。import math
import random
import struct
import time
import os
import csv
import matplotlib
matplotlib.use("Agg")  # 非対話型バックエンドを設定
import matplotlib.pyplot as plt

# HID ライブラリのインポートを試みる（エラー処理付き）
try:
    import hid
    HID_AVAILABLE = True
except ImportError:
    print("警告: HIDライブラリが見つかりません。シミュレーションモードで実行します。")
    HID_AVAILABLE = False

# メモリマップ構造（レジスタアドレス）
class MemoryMap:
    M_CURRENT_L_OFFSET = 4   # 左電流測定値 (mA)
    M_CURRENT_R_OFFSET = 6   # 右電流測定値 (mA)
    T_CURRENT_L_OFFSET = 8   # 左電流指令値
    T_CURRENT_R_OFFSET = 10  # 右電流指令値
    WHEEL_ANGLE_L_OFFSET = 56     # 左車輪角度 (rad)
    WHEEL_ANGLE_R_OFFSET = 58     # 右車輪角度 (rad)
    WHEEL_ANGULAR_SPD_L_OFFSET = 40 # 左車輪角速度 (rad/s)
    WHEEL_ANGULAR_SPD_R_OFFSET = 42 # 右車輪角速度 (rad/s)
    # 電流PI制御ゲイン (double, 8 bytes)
    GAIN_CURRENT_P_OFFSET = 288
    GAIN_CURRENT_I_OFFSET = 296

# ハードウェアメモリ読み取り関数
def read_memory(h, addr, fmt, size):
    command = b"r" + addr.to_bytes(2, "little") + size.to_bytes(1, "little")
    if HID_AVAILABLE and isinstance(h, hid.device):
        try:
            if h.write(command) != len(command):
                print(f"読み取りコマンドの送信に失敗しました: {addr}")
                return None
            response = h.read(size + 2)
            # 応答がNoneまたは空でないか確認
            if not response or len(response) < 2:
                 print(f"無効な応答（短すぎるかNone）: {addr}")
                 return None
            if response[0] != ord(b"r") or response[1] != size:
                print(f"無効な応答ヘッダ: {addr}, 受信: {response[:2]}, 期待値: ('r', {size})")
                return None
            data = bytes(response[2:])
            # データの長さが期待通りか確認
            expected_data_len = struct.calcsize("<" + fmt)
            if len(data) < expected_data_len:
                 print(f"データ長が不十分: {addr}, 受信: {len(data)}, 期待値: {expected_data_len}")
                 return None
            value = struct.unpack("<" + fmt, data)[0]
            return value
        except hid.HIDException as e:
             # HID固有のエラーを具体的に処理
             print(f"HIDエラー（メモリ読み取り中）({addr}): {e}")
             return None
        except Exception as e:
            print(f"メモリ読み取り中にエラーが発生しました ({addr}): {e}")
            return None
    elif not HID_AVAILABLE: # シミュレーションモード処理
         # print(f"メモリ読み取りをシミュレート: {addr}") # オプションのデバッグ情報
         return None # メインループにシミュレーションデータを使用させる
    else:
         print("エラー：無効なデバイスハンドルです。")
         return None


# ハードウェアメモリ書き込み関数
def write_memory(h, addr, value, fmt, size):
    if not HID_AVAILABLE: # シミュレーションモードでは書き込みを行わない
        # print(f"メモリ書き込みをシミュレート: {addr} = {value}") # オプションのデバッグ情報
        return True
    if not isinstance(h, hid.device):
        print("エラー：無効なデバイスハンドルです。")
        return False

    data = struct.pack("<" + fmt, value)
    command = b"w" + addr.to_bytes(2, "little") + size.to_bytes(1, "little") + data
    try:
        if h.write(command) != len(command):
            print(f"書き込みコマンドの送信に失敗しました: {addr}")
            return False
        response = h.read(3)
        # 応答がNoneまたは空でないか確認
        if not response or len(response) < 3:
            print(f"無効な応答（短すぎるかNone）: {addr}")
            return False
        if response[0] != ord(b"w") or response[1] != addr % 256 or response[2] != size:
            print(f"無効な応答ヘッダ: {addr}, 受信: {response[:3]}, 期待値: ('w', {addr % 256}, {size})")
            return False
        return True
    except hid.HIDException as e:
        # HID固有のエラーを具体的に処理
        print(f"HIDエラー（メモリ書き込み中）({addr}): {e}")
        return False
    except Exception as e:
        print(f"メモリ書き込み中にエラーが発生しました ({addr}): {e}")
        return False

# 両方のモーターに電流指令値を書き込む
def write_targets(h, left, right):
    left = max(-0x7FFF, min(0x7FFF, int(left)))
    right = max(-0x7FFF, min(0x7FFF, int(right)))
    if not write_memory(h, MemoryMap.T_CURRENT_L_OFFSET, left, "h", 2):
        return False
    if not write_memory(h, MemoryMap.T_CURRENT_R_OFFSET, right, "h", 2):
        return False
    return True

# PID 制御パラメータ
desired_current_Kp = 0.00023  # サンプル値
desired_current_Ki = 0.0012   # サンプル値

angle_kp = 1700    # 比例ゲイン（外側ループ）
angle_ki = 90  # 積分ゲイン
angle_kd = 110   # 微分ゲイン

target_angle_rad = math.radians(60) # 目標車輪角度: 60度

dt = 0.005  # 制御周期 (秒)

# データ記録用リスト（単位は元データと同じ）
time_list = []
angle_ref_list = []   # 目標角度 (rad)
angle_meas_list = []  # 測定角度 (rad)
I_ref_list = []       # 電流指令値 (mA)
I_meas_list = []      # 測定電流 (mA)

g_plot_dir = "plots" # プロット保存ディレクトリ

if not os.path.exists(g_plot_dir):
    os.makedirs(g_plot_dir)

# ==============================================================================
# プロット関連
# ==============================================================================
def plot_final_results(time_list, angle_ref_list, angle_meas_list, I_ref_list, I_meas_list):
    """Generate and save the final summary plot."""
    params_text = (
        f"Control Parameters:\n"
        f"Inner Loop: Current PI (Kp={desired_current_Kp}, Ki={desired_current_Ki})\n"
        f"Outer Loop: Angle PID (Kp={angle_kp}, Ki={angle_ki}, Kd={angle_kd})\n"
        f"Target Angle: {math.degrees(target_angle_rad):.1f}°"
    )
    angle_ref_deg_list = [math.degrees(a) for a in angle_ref_list]
    angle_meas_deg_list = [math.degrees(a) for a in angle_meas_list]

    plt.figure(figsize=(14, 10))

    # Angle subplot
    plt.subplot(2, 1, 1)
    plt.plot(time_list, angle_ref_deg_list, "r-", linewidth=2, label="Reference Angle (°)")
    plt.plot(time_list, angle_meas_deg_list, "b--", linewidth=2, label="Measured Angle (°)")
    plt.ylabel("Angle (°)", fontsize=12)
    plt.title("PID Control Results Summary", fontsize=16)
    plt.legend(fontsize=10)
    plt.grid(True)

    # Current subplot
    plt.subplot(2, 1, 2)
    plt.plot(time_list, I_ref_list, "r-", linewidth=2, label="Current Command (mA)")
    plt.plot(time_list, I_meas_list, "b--", linewidth=2, label="Measured Current (mA)")
    plt.xlabel("Time (s)", fontsize=12)
    plt.ylabel("Current (mA)", fontsize=12)
    plt.legend(fontsize=10)
    plt.grid(True)

    # Add parameter text
    plt.figtext(0.5, 0.01, params_text, fontsize=10, bbox=dict(facecolor="lightgray", alpha=0.5), horizontalalignment="center")
    plt.tight_layout(rect=[0, 0.05, 1, 0.95]) # Adjust layout for title and text

    # Save the figure
    filepath = os.path.join(g_plot_dir, "final_summary.png") # English filename
    plt.savefig(filepath, dpi=150)
    print(f"\nSummary plot saved to {filepath}") # English message
    plt.close() # Close the figure to free memory

# ==============================================================================
# データ保存関連
# ==============================================================================
def save_experiment_data(filename, time_list, angle_ref_list, angle_meas_list, I_ref_list, I_meas_list):
    """Save experiment data to a CSV file using the csv module."""
    filepath = os.path.join(g_plot_dir, filename) # Save file in plots directory
    try:
        with open(filepath, mode="w", newline="", encoding="utf-8") as f: # Keep encoding for safety
            writer = csv.writer(f)
            # Write header (English)
            writer.writerow(["Time(s)", "Angle_Ref(rad)", "Angle_Meas(rad)", "I_ref(mA)", "I_meas(mA)"])
            # Write data rows
            for t, a_ref, a_meas, i_ref, i_meas in zip(time_list, angle_ref_list, angle_meas_list, I_ref_list, I_meas_list):
                writer.writerow([t, a_ref, a_meas, i_ref, i_meas])
        print(f"Experiment data saved to {filepath}") # English message
    except IOError as e:
        # 日本語のエラーメッセージは維持
        print(f"ファイル書き込みエラー ({filepath}): {e}")
    except Exception as e:
        print(f"CSV保存中に予期せぬエラーが発生しました: {e}")

# ==============================================================================
# メイン処理
# ==============================================================================
def main():
    h = None # デバイスハンドルを初期化
    is_demo_mode = False

    if HID_AVAILABLE:
        try:
            h = hid.device()
            h.open(0x1962, 0x2080)  # デバイスVID/PID
            h.set_nonblocking(False)
            print("デバイスに接続しました")
            # 電流ループPIゲインを設定
            if not write_memory(h, MemoryMap.GAIN_CURRENT_P_OFFSET, desired_current_Kp, "d", 8):
                 print("警告: 電流Pゲインの設定に失敗した可能性があります。")
            if not write_memory(h, MemoryMap.GAIN_CURRENT_I_OFFSET, desired_current_Ki, "d", 8):
                 print("警告: 電流Iゲインの設定に失敗した可能性があります。")
            print(f"電流PIゲインを設定しました: Kp={desired_current_Kp}, Ki={desired_current_Ki}")
        except hid.HIDException as e:
            print(f"デバイス接続中にエラーが発生しました: {e}")
            print("シミュレーションモードを開始します（シミュレートされたデータを使用）")
            is_demo_mode = True
            if h:
                try: h.close()
                except: pass # 閉じる際のエラーは無視
            h = None # シミュレーションモードではハンドルをNoneに設定
        except Exception as e: # 他のエラー（権限など）もキャッチ
             print(f"デバイス初期化中に不明なエラーが発生しました: {e}")
             print("シミュレーションモードを開始します（シミュレートされたデータを使用）")
             is_demo_mode = True
             if h:
                 try: h.close()
                 except: pass
             h = None
    else:
        print("シミュレーションモードを開始します（HIDライブラリが利用不可）")
        is_demo_mode = True


    # 以前のデータをクリア
    global time_list, angle_ref_list, angle_meas_list, I_ref_list, I_meas_list
    time_list.clear()
    angle_ref_list.clear()
    angle_meas_list.clear()
    I_ref_list.clear()
    I_meas_list.clear()

    print(f"\n角度PID制御テストを開始します (目標 {math.degrees(target_angle_rad):.1f}°)")
    print("内側ループ: 電流PI制御, 外側ループ: 角度PID制御")
    start_time = time.time()
    angle_integral = 0.0
    last_wheel_angle = 0.0 # シミュレーションモードで使用

    try:
        run_duration = 3.0 # 実験実行時間（秒）
        print(f"実験時間: {run_duration}秒")

        while True:
            current_time = time.time() - start_time
            if current_time > run_duration:
                 print(f"\n予定実行時間 ({run_duration}s) に到達しました。制御を終了します。")
                 break

            # --- データ収集 ---
            wheel_angle = None
            wheel_angular_spd = None
            I_meas_L = None
            I_meas_R = None

            if not is_demo_mode and h:
                wheel_angle = read_memory(h, MemoryMap.WHEEL_ANGLE_L_OFFSET, "d", 8)
                wheel_angular_spd = read_memory(h, MemoryMap.WHEEL_ANGULAR_SPD_L_OFFSET, "d", 8)
                I_meas_L = read_memory(h, MemoryMap.M_CURRENT_L_OFFSET, "h", 2)
                I_meas_R = read_memory(h, MemoryMap.M_CURRENT_R_OFFSET, "h", 2)

                # 読み取り失敗をチェック (Noneが返ってきた場合)
                if wheel_angle is None or wheel_angular_spd is None or I_meas_L is None or I_meas_R is None:
                    print("警告: ハードウェアからのデータ読み取りに失敗しました。この周期はスキップされる可能性があります。")
                    # 必要に応じてエラー処理や待機を追加
                    time.sleep(dt * 0.5) # 短い待機
                    continue # この制御ループをスキップするか、代替処理を行う

            # --- シミュレーションデータ生成 (必要な場合) ---
            simulating_angle = is_demo_mode or wheel_angle is None
            simulating_current = is_demo_mode or I_meas_L is None or I_meas_R is None

            if simulating_angle:
                # 簡易な角度一次システムシミュレーション
                target = target_angle_rad
                time_constant_angle = 0.5 # 角度応答の時定数（シミュレーション用）
                noise_angle = (random.random() - 0.5) * math.radians(0.5) # 角度ノイズ (±0.25°)
                if len(angle_meas_list) > 0:
                     prev_angle = angle_meas_list[-1]
                     # 指令値ではなく目標値に向かう簡易シミュレーション
                     wheel_angle = prev_angle + (target - prev_angle) * (1 - math.exp(-dt / time_constant_angle)) + noise_angle
                else:
                     wheel_angle = 0.0 + noise_angle # 初期角度

                # 角速度の計算
                if len(angle_meas_list) > 0:
                     wheel_angular_spd = (wheel_angle - angle_meas_list[-1]) / dt
                else:
                     wheel_angular_spd = 0.0
                # print(f"[シミュレーション] 角度={math.degrees(wheel_angle):.2f}°, 角速度={wheel_angular_spd:.2f} rad/s")

            # --- PID 制御計算 ---
            angle_ref = target_angle_rad
            error = angle_ref - wheel_angle
            angle_integral += error * dt
            # 積分値の発散防止（アンチワインドアップ） - 必要であれば追加
            # angle_integral = max(min(angle_integral, INTEGRAL_LIMIT), -INTEGRAL_LIMIT)
            derivative = -wheel_angular_spd # 角速度を入力として使用

            I_ref = angle_kp * error + angle_ki * angle_integral + angle_kd * derivative # 計算された電流指令値 (mA)

            # --- 制御指令の適用 ---
            current_cmd = int(max(min(I_ref, 32767), -32768)) # ハードウェア指令形式に変換
            if not is_demo_mode and h:
                if not write_targets(h, current_cmd, current_cmd): # 左右輪に同じ指令を送信
                    print("電流指令の書き込みに失敗しました")
                    time.sleep(dt) # 次の周期まで待機
                    continue
            # else:
                # print(f"[シミュレーション] 電流指令書き込み: {current_cmd}")

            # --- 電流測定/シミュレーション ---
            if simulating_current:
                 # 簡易な電流一次システムシミュレーション
                 time_constant_current = 0.05 # 電流応答の時定数（シミュレーション用）
                 noise_current = (random.random() - 0.5) * 20 # 電流ノイズ (±10 mA)
                 if len(I_meas_list) > 0:
                      prev_current = I_meas_list[-1]
                      target_current = I_ref # 計算された指令値を目標とする
                      I_meas = prev_current + (target_current - prev_current) * (1 - math.exp(-dt / time_constant_current)) + noise_current
                 else:
                      I_meas = 0.0 + noise_current # 初期電流
                 # print(f"[シミュレーション] 測定電流={I_meas:.1f} mA")
            else:
                 # 実測値の平均を使用
                 I_meas = (I_meas_L + I_meas_R) / 2.0

            # --- データ記録 ---
            time_list.append(current_time)
            angle_ref_list.append(angle_ref)
            angle_meas_list.append(wheel_angle)
            I_ref_list.append(I_ref) # 計算された指令値を記録
            I_meas_list.append(I_meas) # 測定/シミュレートされた電流を記録

            # --- 状態表示 (日本語メッセージ維持) ---
            wheel_angle_deg = math.degrees(wheel_angle)
            error_deg = math.degrees(error)
            # \r を使って行を上書き表示
            print(f"時間: {current_time:.3f}s | 角度: {wheel_angle_deg:.2f}° | 目標: {math.degrees(target_angle_rad):.1f}° | "
                  f"誤差: {error_deg:.2f}° | 指令電流: {I_ref:.1f} mA | 測定電流: {I_meas:.1f} mA", end='\r')

            # --- 次の周期まで待機 ---
            # 正確な周期を保つための待機時間計算
            computation_time = time.time() - start_time - current_time
            sleep_time = dt - computation_time
            if sleep_time > 0:
                 time.sleep(sleep_time)
            # else:
            #      print(f"警告: 制御周期遅延 {abs(sleep_time):.4f} s") # 周期遅延の警告（オプション）


    except KeyboardInterrupt:
        print("\nユーザー割り込み。PID制御を終了します。")
    except Exception as e:
        print(f"\n予期せぬエラーが発生しました: {e}")
        import traceback
        traceback.print_exc() # 詳細なトレースバックを表示
    finally:
        print("\n" + "="*20 + " クリーンアップ " + "="*20)
        # モーターを停止
        if not is_demo_mode and h:
            print("停止コマンドを送信中...")
            try:
                write_targets(h, 0, 0)
            except Exception as e:
                 print(f"停止コマンド送信中にエラー: {e}")
        # デバイス接続を閉じる
        if h:
            try:
                h.close()
                print("デバイス接続を閉じました。")
            except Exception as e:
                 print(f"デバイス切断中にエラーが発生しました: {e}")

        # データが収集されていればプロットと保存を行う
        if time_list:
            # 日本語メッセージは維持
            print("\n最終分析プロットを生成中...")
            plot_final_results(time_list, angle_ref_list, angle_meas_list, I_ref_list, I_meas_list) # 英語ラベルでプロット

            # 日本語メッセージは維持
            print("\n実験データを保存中...")
            csv_filename = "experiment_data.csv" # 英語ファイル名
            save_experiment_data(csv_filename, time_list, angle_ref_list, angle_meas_list, I_ref_list, I_meas_list) # 英語ヘッダーで保存
        else:
            print("\nデータが収集されなかったため、プロットと保存をスキップします。")

        print("\nプログラム終了。")


if __name__ == "__main__":
    main()　　　　　　　　　　　このコードのdesired_current_Kp = 0.00023  # サンプル値
desired_current_Ki = 0.0012   # サンプル値

angle_kp = 1700    # 比例ゲイン（外側ループ）
angle_ki = 90  # 積分ゲイン
angle_kd = 110   # 微分ゲイン　を調整するのだが、desired_current_Kp = 0.00023  # サンプル値
desired_current_Ki = 0.0012   # サンプル値　の部分はサンプル値から変更させなかった。これの結果から得られるグラフのCurrent CommandとMeasured Currentは何を表し、どのように見たらよいのか？