ドキュメントジェスチャーによるパンチルト雲台の動作制御実験
実験の目的：
- パンチルト雲台の構造を設計できるようになります。2. パンチルト雲台の首振りを制御できるようになります。
主な技術ポイント：
- Socket 通信サービス：9000 番ポートで待ち受け、Arduino ジャイロスコープモジュールのリアルタイム動作データを受信します
- データ解析：JSON 形式の加速度センサーとジャイロスコープのデータを処理します
- 姿勢計算：デバイスのロール（roll）、ピッチ（pitch）、ヨー（yaw）角を計算します
- パンチルト雲台制御：姿勢角に応じてロボットアームのパンチルト雲台の動きを制御します
- マルチスレッドアーキテクチャ：データ受信とロボットアーム制御を分離し、リアルタイム性を確保します
基本原理：
2 自由度のパンチルト雲台は 2 つの旋回モジュールで構成され、下部は鉛直軸まわりに回転し、上部は X 軸または Y 軸まわりに回転します。
応用シーン：
移動ロボットによる観光地での撮影やイベントのライブ中継などに適しています。ロボットにパンチルト雲台とカメラを搭載し、多角度から撮影します。
ハードウェアの準備：
ディスプレイ × 1、Bluetooth キーボード・マウス × 1、メインコンソール × 1、パンチルト雲台モジュール × 1、装着型ジャイロスコープモジュール × 1
構造設計：
回路接続：
2 自由度パンチルト雲台をメインコンソールに接続します。姿勢センサー（ESP32 ジャイロスコープ）は手首に固定します。
ハードウェアモジュール
ヒント：ハードウェアモジュールの使用説明
ハードウェアシステムの全体組立
電子モジュールの紹介
物理配線
接続ケーブルの一端をパンチルト雲台に、もう一端をメインコンソールに接続します。具体的な取り付け方法はハードウェアシステムの全体組立を参照してください。
装着型ジャイロスコープモジュールを手首に固定します。
ソフトウェア設計
設計フローチャート
ソースコードのダウンロード
ソースコードのリンク：RAI-P4 具身ロボットタスクプランニング総合実習プラットフォーム コース用オープンソースリポジトリ
プログラムコード
コードの構成：
| ファイル名 | 機能説明 |
|---|---|
| control_gimbal.py | メインプログラム。Socket サーバーとしてジャイロスコープのデータを受信し、パンチルト雲台の動きを制御します |
| custom_font.py | ターミナル出力の装飾ツール |
| manager.py | 制御の中枢。すべての下位モジュールを統合管理します |
| process_gyro_data.py | ジャイロスコープのデータ処理と姿勢計算 |
| serial_section.py | シリアル通信の管理 |
| servo_section.py | サーボ制御プロトコルの生成 |
具体的なコード：
プロジェクトの完全なコードは（ソースコードのリンク）からダウンロードしてください。
"""
--------------------------------------------------------------------------------
功能:通过socket获取手持陀螺仪模块数据,并控制云台运动
日期:2025-06-03
--------------------------------------------------------------------------------
"""
import socket
import json
import threading
import queue
import math
import time
import numpy as np
from src.arm_control import custom_font
from src.arm_control import manager
from src.arm_control import process_gyro_data
process_gyro = process_gyro_data.ProcessGyro()
# 创建线程间通信的队列
arm_command_queue = queue.Queue(maxsize=1) # 限制队列大小防止积压
def arm_worker():
global roll_windows, pitch_windows, last_yaw_data, loop_left, loop_right, loop_shows
global left_numbers,gyro_turn_left_flag,gyro_turn_right_flag, right_numbers, loop_numbers
"""机械臂控制线程"""
node = manager.RobotManager()
input_point = 1
while True:
command = arm_command_queue.get() # 获取包含两个参数的元组
try:
pitch, roll = command
# 原有控制逻辑
if roll >= 35:
node.gimbal_ang[0] -= 4
input_point = 1
elif roll <= -35:
node.gimbal_ang[0] += 4
input_point = 1
elif pitch >= 30:
node.gimbal_ang[1] += 4
input_point = 1
elif pitch <= -30:
node.gimbal_ang[1] -= 4
input_point = 1
node.gimbal_move(node.gimbal_ang[0], node.gimbal_ang[1], input_point)
except Exception as e:
print(f"[ARM ERROR] 机械臂控制错误: {e}")
# 启动机械臂控制线程
arm_thread = threading.Thread(target=arm_worker, daemon=True)
arm_thread.start()
# 主线程处理陀螺仪数据
node = manager.RobotManager()
font = custom_font.CustomFont()
print(font.color_text(font.compony_msg, color="cyan", bold=True))
HOST = '0.0.0.0'
PORT = 9000
print(f"[INFO] 启动Socket服务端, 监听端口 {PORT} ...")
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind((HOST, PORT))
s.listen(1)
print("[INFO] 等待Arduino客户端连接...")
conn, addr = s.accept()
with conn:
print(f"[INFO] 已连接: {addr}")
buffer = b''
while True:
data = conn.recv(1024)
if not data:
print("[WARN] 客户端断开连接")
break
buffer += data
while b'\n' in buffer:
line, buffer = buffer.split(b'\n', 1)
try:
sensor_data = json.loads(line.decode('utf-8'))
# 原有角度计算
if not process_gyro.calibration_complete:
process_gyro.calibration_gyro(sensor_data['accel'])
else:
data = time.time()
roll, pitch, yaw, time_delay= process_gyro.calculate_angles_back(sensor_data['accel'], sensor_data['gyro'], data)
# 非阻塞方式发送命令到机械臂线程
if arm_command_queue.empty():
# 添加位移数据到队列
arm_command_queue.put_nowait((
pitch, roll
))
except Exception as e:
print(f"[ERROR] 解码失败: {e}")
操作手順
回路接続が完了したら、ターミナルで操作します：
ターミナルからリモート接続します。ターミナルに次のコマンドを入力して Enter を押します
ssh sunrise@192.168.8.111
sunrise
パスワードを入力して Enter を押すとログインが完了します。
- 指定のディレクトリに移動してターミナルを開き、次のように入力します：
cd ~/Desktop/Arm_Experiment/module_control_servo
上記で開いたターミナルで次のように入力し、ジャイロモジュールによるパンチルト雲台動作制御プログラムを実行します：
python imu_control_arm.py
テスト時の操作コマンドは下図のとおりです。必要に応じて参照してください。
- ハンドヘルドジャイロスコープモジュールを手で回転させ、パンチルト雲台が正常に回転・起動するかを観察します。ターミナルには次のように出力されます：
--------------------------------------------------------------------------------------
_ _ _ _______ _
| | | | (_) |__ __| | |
| | | |_ __ _ | | ___ _ __ __ _ ___| |__ _ _ __ _ _ __ __ _
| | | | '_ \| | | |/ _ \| '_ \ / _` |/ __| '_ \| | | |/ _` | '_ \ / _` |
| |__| | | | | | | | (_) | | | | (_| | (__| | | | |_| | (_| | | | | (_| |
\____/|_| |_|_| |_|\___/|_| |_|\__, |\___|_| |_|\__,_|\__,_|_| |_|\__, |
__/ | __/ |
|___/ |___/
--------------------------------------------------------------------------------------
[INFO] 启动Socket服务端, 监听端口 9000 ...
[INFO] 等待Arduino客户端连接...
[INFO] 已连接: ('192.168.8.235', 56950)
陀螺仪模块标定中..................................................
.----------------------------.
| |
| |
| 陀螺仪校准完成 |
| |
| |
.----------------------------.
滚转角初始值:-0.51°
俯仰角初始值:-3.27°
実験結果
手を振るジェスチャーをすると、パンチルト雲台がジェスチャーに合わせて回転することを確認できます。
インターフェースドキュメント
さらに詳しくは、以下の API インターフェースドキュメントを参照して二次開発や最適化を行うことができます。
ロボット本体インターフェースの全体紹介
お問い合わせ
資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。