Function-call による雲台（ジンバル）制御システムのタスクプランニング

実験の目的：

LLM から Function-call インターフェースを呼び出し、雲台の回転を制御します。

基本原理：

2 自由度雲台は 2 つの揺動モジュールで構成され、下部は鉛直方向の回転軸、上部は X 軸または Y 軸を回転軸とします。

活用シーン：

移動ロボットによる観光地での撮影やイベントのライブ配信などに適しています。ロボットに雲台とカメラを搭載し、さまざまな角度から撮影します。

ハードウェアの準備

No.	名称	数量	備考
1	RDK X5	1	組み込みメインボード（32GB メモリーカード付き）。上位機システムを実行します
2	カードリーダー	1	イメージシステムの書き込みに使用します
3	電源ケーブル	1	給電用
4	LAN ケーブル	1	ネットワーク接続用
5	ディスプレイ（各自で用意）	1	操作用
6	Bluetooth キーボード・マウス（各自で用意）	1	メインボードの操作用
7	Arduino Mega2560 メインボード	1	ロボットアームの駆動用
8	拡張ボード	1	Arduino メインボードに積み重ね、周辺回路を接続します
9	3S バッテリーボックス	1	Arduino メインボードおよび RDK への給電用
10	XT30 電源ケーブル	1	バッテリーボックスと拡張ボードを接続します
11	メス－メスジャンパーワイヤー	3	RDK と Arduino のシリアル通信接続用
12	Type-C 電源ケーブル	1	バッテリーボックスの USB ポートと RDK の Type-C 電源端子を接続します

構造設計：

STP ファイル：云台STP（包含摄像头支架）.rar（雲台 STP、カメラブラケットを含む）

回路接続：

2 自由度雲台を RDK X5 に接続します。（今後は ASR、TTS 技術と組み合わせて、音声で制御し結果を音声で再生する音声対話アシスタントの実験も可能です）

ハードウェアモジュール

ヒント：ハードウェアモジュールの使用説明

1.1 ハードウェアインターフェースで RDK X5 のインターフェースをすぐに把握できます

実機の配線

接続ケーブルの一端を雲台に、もう一端をメインコントローラーに接続します。

ソフトウェア設計

設計フローチャート

雲台制御の Function Calling インターフェース

雲台制御の実装

ソースコードのダウンロード

（ダウンロード済みの場合、この手順は不要です）ソースファイル：llm.rar

（リンクをクリックしてオンラインの説明を見ることもできます）（HumansX サポートより提供）

プログラムコード

コードの関係：

フォルダ	コード	機能
function_call	gimbal_functions.py	LLM に雲台制御用の Function Calling インターフェースを提供します
servo_control		インターフェースを実装し、雲台を制御します
	_init.py	雲台制御用モジュールをインポートします
	serial_protocol.py	シリアル通信の管理
	servo_controller.py	すべてのサーボ制御機能を提供します
	servo_protocol.py	サーボ制御プロトコルの生成

具体的なコード：

プロジェクトの完全なコードは（ソースファイル）をクリックしてダウンロードしてください。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
云台控制功能函数
Gimbal Control Functions

为LLM提供云台控制的Function Calling接口
基于现有的SimpleServoController实现
"""

import logging
from typing import Dict, Any, Optional

try:
    from ..servo_control.servo_controller import SimpleServoController
    from .function_registry import FunctionRegistry
except ImportError:
    import sys
    import os
    # 当直接运行时添加路径
    current_dir = os.path.dirname(os.path.abspath(__file__))
    servo_control_dir = os.path.join(current_dir, '..', 'servo_control')
    sys.path.insert(0, servo_control_dir)

    from servo_controller import SimpleServoController
    from function_registry import FunctionRegistry

logger = logging.getLogger(__name__)

# 云台限位设置 (单位: 度)
GIMBAL_LIMIT_HOR_MIN = 30
GIMBAL_LIMIT_HOR_MAX = 200
GIMBAL_LIMIT_VER_MIN = 30
GIMBAL_LIMIT_VER_MAX = 200

# 全局云台控制器实例
_gimbal_controller: Optional[SimpleServoController] = None

def validate_gimbal_angles(pan: float, tilt: float) -> tuple[bool, str]:
    """
    验证云台角度是否在限位范围内

    Args:
        pan: 水平角度 (度)
        tilt: 垂直角度 (度)

    Returns:
        tuple: (是否有效, 错误信息)
    """
    if not (GIMBAL_LIMIT_HOR_MIN <= pan <= GIMBAL_LIMIT_HOR_MAX):
        return False, f"云台水平角度超出限位范围 {pan}° (有效范围: {GIMBAL_LIMIT_HOR_MIN}-{GIMBAL_LIMIT_HOR_MAX}°)"
    if not (GIMBAL_LIMIT_VER_MIN <= tilt <= GIMBAL_LIMIT_VER_MAX):
        return False, f"云台垂直角度超出限位范围 {tilt}° (有效范围: {GIMBAL_LIMIT_VER_MIN}-{GIMBAL_LIMIT_VER_MAX}°)"
    return True, ""

def get_gimbal_controller() -> SimpleServoController:
    """获取云台控制器实例"""
    global _gimbal_controller
    if _gimbal_controller is None:
        raise RuntimeError("云台控制器未初始化，请先调用initialize_gimbal函数")
    return _gimbal_controller

def initialize_gimbal_controller(port: str = '/dev/ttyS1', baudrate: int = 115200,
                               pan_pin: int = 3, tilt_pin: int = 4) -> bool:
    """
    初始化云台控制器

    Args:
        port: 串口设备路径
        baudrate: 波特率
        pan_pin: 水平舵机引脚
        tilt_pin: 垂直舵机引脚

    Returns:
        bool: 是否成功初始化
    """
    global _gimbal_controller

    try:
        _gimbal_controller = SimpleServoController(
            port=port,
            baudrate=baudrate,
            pan_pin=pan_pin,
            tilt_pin=tilt_pin
        )
        logger.info(f"云台控制器初始化成功: {port}")
        return True
    except Exception as e:
        logger.error(f"云台控制器初始化失败: {e}")
        _gimbal_controller = None
        return False

@FunctionRegistry.register_function('gimbal_control', '控制云台转动到指定的绝对角度位置')
def gimbal_control_func(pan: float, tilt: float) -> str:
    """
    控制云台到指定绝对角度

    这个工具函数可以精确控制二自由度云台的水平和垂直角度。

    **重要限位信息：**
    - 实际可用角度范围：水平30-200°，垂直30-200°（安全限位）
    - 机械角度范围：0-270°（仅供参考，请使用限位范围）
    - 中心位置：135°（水平和垂直）

    **角落位置建议（在限位范围内）：**
    - 左上角：推荐 pan=50°, tilt=170°
    - 左下角：推荐 pan=50°, tilt=50°  
    - 右上角：推荐 pan=180°, tilt=170°
    - 右下角：推荐 pan=180°, tilt=50°

    Args:
        pan: 水平角度，**必须在30-200度范围内**（30度=偏左，135度=中心，200度=偏右）
        tilt: 垂直角度，**必须在30-200度范围内**（30度=偏下，135度=中心，200度=偏上）

    Returns:
        str: 执行结果描述

    使用示例：
        - 转向左上角：gimbal_control(50, 170)
        - 回到中心位置：gimbal_control(135, 135)
        - 转向右下角：gimbal_control(180, 50)
    """
    try:
        # 参数验证
        if not isinstance(pan, (int, float)) or not isinstance(tilt, (int, float)):
            return "错误：角度参数必须是数字"

        # 限位检查 - 使用更严格的限位范围
        is_valid, error_msg = validate_gimbal_angles(pan, tilt)
        if not is_valid:
            return f"限位错误：{error_msg}。请调整目标角度到安全范围内。"

        # 原有的0-270度范围检查作为额外保护
        if not (0 <= pan <= 270):
            return f"错误：水平角度超出机械范围 {pan}° (机械范围: 0-270度)"

        if not (0 <= tilt <= 270):
            return f"错误：垂直角度超出机械范围 {tilt}° (机械范围: 0-270度)"

        controller = get_gimbal_controller()

        # 如果未连接，尝试连接
        if not controller.connected:
            if not controller.connect():
                return "错误：云台连接失败，请检查硬件连接和串口配置"

        # 执行控制
        success = controller.move_to_angle(float(pan), float(tilt))

        if success:
            current_pan, current_tilt = controller.get_current_angles()
            return f"云台控制成功：已移动到 水平={current_pan:.1f}°, 垂直={current_tilt:.1f}°"
        else:
            return "错误：云台移动失败，请检查硬件状态"

    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"云台控制异常: {e}")
        return f"云台控制过程中出现错误：{str(e)}"

@FunctionRegistry.register_function('gimbal_move_relative', '相对移动云台，在当前位置基础上调整角度')
def gimbal_move_relative_func(pan_delta: float = 0, tilt_delta: float = 0) -> str:
    """
    相对移动云台

    这个工具函数用于微调云台位置，在当前角度基础上进行相对移动。
    使用直观的方向语义，正值表示增加角度方向。

    Args:
        pan_delta: 水平角度变化量（正值向右，负值向左）
        tilt_delta: 垂直角度变化量（正值向上，负值向下）

    Returns:
        str: 执行结果描述

    使用示例：
        - 向右上微调：gimbal_move_relative(10, 10)
        - 向左下微调：gimbal_move_relative(-10, -10)
        - 只向右调整：gimbal_move_relative(15, 0)
    """
    try:
        # 参数验证
        if not isinstance(pan_delta, (int, float)) or not isinstance(tilt_delta, (int, float)):
            return "错误：角度变化量参数必须是数字"

        controller = get_gimbal_controller()

        # 如果未连接，尝试连接
        if not controller.connected:
            if not controller.connect():
                return "错误：云台连接失败，请检查硬件连接和串口配置"

        # 获取当前角度并计算目标角度
        current_pan, current_tilt = controller.get_current_angles()
        target_pan = current_pan + pan_delta  # 正值向右，负值向左（直观语义）
        target_tilt = current_tilt + tilt_delta  # 正值向上，负值向下

        # 限位检查
        is_valid, error_msg = validate_gimbal_angles(target_pan, target_tilt)
        if not is_valid:
            return f"限位错误：{error_msg}。当前位置：水平={current_pan:.1f}°, 垂直={current_tilt:.1f}°，无法执行该移动。"

        # 机械范围检查
        if not (0 <= target_pan <= 270) or not (0 <= target_tilt <= 270):
            return f"错误：目标角度超出机械范围。目标位置：水平={target_pan:.1f}°, 垂直={target_tilt:.1f}° (机械范围: 0-270°)"

        # 使用绝对移动执行相对移动（避免双重方向转换）
        success = controller.move_to_angle(target_pan, target_tilt)

        if success:
            current_pan, current_tilt = controller.get_current_angles()
            return f"云台相对移动成功：当前位置 水平={current_pan:.1f}°, 垂直={current_tilt:.1f}°"
        else:
            return "错误：云台相对移动失败，请检查硬件状态"

    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"云台相对移动异常: {e}")
        return f"云台相对移动过程中出现错误：{str(e)}"

@FunctionRegistry.register_function('gimbal_reset', '重置云台到初始中心位置')
def gimbal_reset_func() -> str:
    """
    重置云台到初始中心位置

    这个工具函数会将云台重置到预设的初始位置（135°, 135°），
    这是云台的机械中心位置，提供最大的运动范围。

    Returns:
        str: 执行结果描述

    使用场景：
        - 系统初始化时的位置校准
        - 控制异常后的位置复位
        - 手动控制后的快速回中
    """
    try:
        controller = get_gimbal_controller()

        # 如果未连接，尝试连接
        if not controller.connected:
            if not controller.connect():
                return "错误：云台连接失败，请检查硬件连接和串口配置"

        # 执行重置
        success = controller.reset_position()

        if success:
            current_pan, current_tilt = controller.get_current_angles()
            return f"云台重置成功：已回到中心位置 水平={current_pan:.1f}°, 垂直={current_tilt:.1f}°"
        else:
            return "错误：云台重置失败，请检查硬件状态"

    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"云台重置异常: {e}")
        return f"云台重置过程中出现错误：{str(e)}"

@FunctionRegistry.register_function('gimbal_status', '获取云台当前状态信息')
def gimbal_status_func() -> str:
    """
    获取云台当前状态信息

    这个工具函数返回云台的详细状态信息，包括：
    - 当前的水平和垂直角度
    - 云台连接状态
    - 配置信息
    - 控制统计数据

    Returns:
        str: 状态信息描述
    """
    try:
        controller = get_gimbal_controller()

        # 获取状态
        status = controller.get_status()

        # 构建友好的状态描述
        result_parts = []

        # 连接状态
        connection_status = "已连接" if status['connected'] else "未连接"
        result_parts.append(f"云台连接状态：{connection_status}")

        # 当前角度
        angles = status['current_angles']
        result_parts.append(f"当前角度：水平={angles['pan']:.1f}°, 垂直={angles['tilt']:.1f}°")

        # 限位信息
        result_parts.append(f"限位范围：水平={GIMBAL_LIMIT_HOR_MIN}-{GIMBAL_LIMIT_HOR_MAX}°, 垂直={GIMBAL_LIMIT_VER_MIN}-{GIMBAL_LIMIT_VER_MAX}°")
        result_parts.append(f"机械范围：水平/垂直=0-270°")

        # 配置信息
        config = status['config']
        result_parts.append(f"配置信息：串口={config['serial_port']}, 波特率={config['baudrate']}")
        result_parts.append(f"舵机引脚：水平={config['pan_pin']}, 垂直={config['tilt_pin']}")

        # 统计信息
        stats = status['stats']
        total_commands = stats['total_commands']
        successful_commands = stats['successful_commands']
        failed_commands = stats['failed_commands']

        if total_commands > 0:
            success_rate = (successful_commands / total_commands) * 100
            result_parts.append(f"控制统计：总命令={total_commands}, 成功={successful_commands}, 失败={failed_commands}")
            result_parts.append(f"成功率：{success_rate:.1f}%")
        else:
            result_parts.append("控制统计：暂无执行记录")

        return "\n".join(result_parts)

    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"获取云台状态异常: {e}")
        return f"获取云台状态过程中出现错误：{str(e)}"

@FunctionRegistry.register_function('gimbal_connect', '连接云台控制器')
def gimbal_connect_func() -> str:
    """
    连接云台控制器

    这个工具函数会尝试连接到云台硬件设备，建立串口通信。

    Returns:
        str: 连接结果描述

    使用场景：
        - 系统启动时的硬件连接
        - 连接断开后的重新连接
        - 硬件状态检查
    """
    try:
        controller = get_gimbal_controller()

        if controller.connected:
            return "云台已连接"

        success = controller.connect()

        if success:
            return "云台连接成功"
        else:
            return "云台连接失败，请检查硬件连接和串口配置"

    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"云台连接异常: {e}")
        return f"云台连接过程中出现错误：{str(e)}"

@FunctionRegistry.register_function('gimbal_disconnect', '断开云台控制器连接')
def gimbal_disconnect_func() -> str:
    """
    断开云台控制器连接

    这个工具函数会安全地断开与云台硬件设备的连接。

    Returns:
        str: 断开结果描述

    使用场景：
        - 系统关闭前的安全断开
        - 硬件维护前的断开连接
        - 连接异常后的强制断开
    """
    try:
        controller = get_gimbal_controller()

        if not controller.connected:
            return "云台未连接"

        controller.disconnect()

        if not controller.connected:
            return "云台连接已断开"
        else:
            return "云台断开连接失败"

    except RuntimeError as e:
        return f"错误：{str(e)}"
    except Exception as e:
        logger.error(f"云台断开连接异常: {e}")
        return f"云台断开连接过程中出现错误：{str(e)}"