ドキュメントUSB-PS2 コントローラーによるロボットアームの動作制御実験
実験の目的：
- ロボットアームの運動学アルゴリズムを理解します。2. 運動学アルゴリズムを組み合わせたプログラミングにより、コントローラーでロボットアームを制御します。
基本的な動作原理：
-
3 軸ロボットアームの先端に旋回モジュールを追加し、エンドエフェクタのグリッパーの向きを制御できるようにしています。
-
取り付け位置は、回転軸を鉛直下向き、x 軸方向、y 軸方向などにすることができます。ただし、第 4 軸は一般に 3 軸構成では先端を平行に保てないという問題を補うために使われるため、直列 4 軸の第 4 関節は通常、第 2・第 3 関節と同じ回転方向にします。
-
運動学アルゴリズムのダウンロード：直列4自由度ロボットアームの運動学計算.pdf
ハードウェアの準備
ディスプレイ × 1、Bluetooth キーボード・マウス × 1、メインコンソール × 1、4 自由度ロボットアーム × 1、USB-PS2 コントローラー × 1（レシーバーと電池を含む）
回路接続：
ハードウェアモジュール
ヒント：ハードウェアモジュールの使用説明
ハードウェアシステムの全体組立
電子モジュールの紹介
物理配線
ロボットアームのベースをメインコンソールに固定します。具体的な取り付け方法はハードウェアシステムの全体組立を参照してください。
USB ワイヤレスレシーバーをメインコントローラーに挿し込みます。
クイックスタート
-
上記を参照して回路接続を完了します：
-
ctrl+alt+tでターミナルを開き、リモート接続します。ターミナルに次のコマンドを入力して Enter を押します：
ssh sunrise@192.168.8.111
sunrise
パスワードを入力して Enter を押すとログインが完了します。
- 指定のディレクトリに移動してターミナルを開き、次のように入力します：
cd ~/Desktop/Arm_Experiment/module_control_servo
上記で開いたターミナルで次のように入力し、PS2 コントローラーによるロボットアーム制御プログラムを実行します：
python ps2_control_arm.py
- PS2 の対応するキーを押して、ロボットアームが正常に動作を開始するかを観察します。PS2 によるロボットアーム操作の説明と照らし合わせ、対応する PS2 のキーを押してロボットアームが指示どおりに動くかを確認します。ターミナルには次のように表示されます
--------------------------------------------------------------------------------------
_ _ _ _______ _
| | | | (_) |__ __| | |
| | | |_ __ _ | | ___ _ __ __ _ ___| |__ _ _ __ _ _ __ __ _
| | | | '_ \| | | |/ _ \| '_ \ / _` |/ __| '_ \| | | |/ _` | '_ \ / _` |
| |__| | | | | | | | (_) | | | | (_| | (__| | | | |_| | (_| | | | | (_| |
\____/|_| |_|_| |_|\___/|_| |_|\__, |\___|_| |_|\__,_|\__,_|_| |_|\__, |
__/ | __/ |
|___/ |___/
--------------------------------------------------------------------------------------
---------------------
自定义字体初始化完成
---------------------
---------------------
机械臂初始化完成
---------------------
---------------------
PS2初始化完成
---------------------
---------------------------------------------------------------------------------------
PS2手柄平放俯视图
._______________________________________________________________________________________.
| L2: R2: |
| L1: R1: |
| |
| Up Y |
| | | |
|Left----+----Right X----+----B |
| | SELECT START | |
| Down A |
| MODE |
| (-32767) (-32767) |
| | | |
| | | |
| | | |
|(-32767)--------+-------> X(32767) (-32767)-------+--------> X方向(32767) |
| | | |
| | | |
| | | |
| v v |
| Y方向(32767) Y方向(32767) |
| |
| 左侧摇杆 右侧摇杆 |
|_______________________________________________________________________________________|
左侧摇杆X方向向左拨动: 机械臂向左移动
左侧摇杆X方向向右拨动: 机械臂向右移动
左侧摇杆Y方向向上拨动: 机械臂向前移动
左侧摇杆Y方向向下拨动: 机械臂向后移动
右侧摇杆Y方向向上拨动: 机械臂向上移动
右侧摇杆Y方向向下拨动: 机械臂向下移动
左上角L1: 控制机械臂爪子闭合
右上角R1: 控制机械臂爪子张开
---------------------------------------------------------------------------------------
実験結果：
コントローラーのボタンを押して、ロボットアームの動きを観察します。
ソフトウェアのデバッグ
ソースコードのダウンロード
（HumansX サポートより提供）
プログラムコード
コードの構成：
| ファイル名 | 機能説明 |
|---|---|
| ps2_control_arm.py | メインプログラム。コントローラーのスティックとボタンでロボットアームの動きとグリッパーの開閉を制御します。 |
| custom_font.py | ターミナル出力の装飾ツール |
| manager.py | 制御の中枢。すべての下位モジュールを統合管理します |
| serial_section.py | シリアル通信の管理 |
| servo_section.py | サーボ制御プロトコルの生成 |
| user_arm.py | ロボットアームの逆運動学アルゴリズム。直線補間と円弧補間を実現できます |
具体的なコード：
プロジェクトの完全なコードは（ソースコードのリンク）からダウンロードしてください。
"""
--------------------------------------------------------------------------------
功能:PS2手柄控制机械臂程序
日期:2025-06-06
--------------------------------------------------------------------------------
"""
import time
import queue
import threading
from src.joy import joy_module
from src.arm_control import manager
from src.arm_control import custom_font
#=======================================加载自定义字体
time.sleep(1)
font = custom_font.CustomFont()
print(font.color_text(font.compony_msg, color="cyan", bold=True))
print(font.color_text(font.font_load_success, color="green", bold=True))
#=======================================创建机械臂实例,以及机械臂相关参数
arm = manager.RobotManager()
arm.arm_init_action()#机械臂执行初始化动作
print(font.color_text(font.arm_load_success, color="green", bold=True))
arm_x_step = 5
arm_y_step = 5
arm_z_step = 5
joystick_name = ''
joystick_data = 0
joystick_action_move = False
joystick_current_numbers = 0
#=======================================创建线程
arm_command_queue = queue.Queue(maxsize=1) # 限制队列大小防止积压
def arm_worker():
"""机械臂控制线程"""
while True:
command = arm_command_queue.get() # 获取包含两个参数的元组
try:
arm.save_ang[0], arm.save_ang[1], arm.save_ang[2] = command
arm.arm_move(arm.save_ang[0], arm.save_ang[1], arm.save_ang[2], 1)
except Exception as e:
print(f"[ARM ERROR] 机械臂控制错误: {e}")
# 启动机械臂控制线程
arm_thread = threading.Thread(target=arm_worker, daemon=True)
arm_thread.start()
#=======================================创建监听器实例
gamepad = joy_module.GamepadListener(device_path='/dev/input/js0')
gamepad.start() #启动监听
print(font.color_text(font.ps2_load_success, color="green", bold=True))
AXIS_THRESHOLD = 30000 # 摇杆阈值 - 小于此值视为未移动
# 摇杆状态字典 - 跟踪摇杆的模拟值
axis_states = {'Left Stick X', 'Left Stick Y', 'Right Stick X', 'Right Stick Y'}
# 按键状态字典. 跟踪所有按键的当前状态
button_states = {
'A': False,'B': False,'X': False,'Y': False, #带有字母的按钮
'Start': False,'Select': False,'Mode/Xbox': False, #中间三个小按钮
'Left': False,'Right': False,'Up': False,'Down': False, #左侧上，下，左，右四个按键
'Left Shoulder': False,'Right Shoulder': False,'L2': False, 'R2': False} #L1,L2,R1,R2按键
# 主程序循环
try:
print(font.color_text(font.ps2_arm_commond, color="cyan", bold=True))
while True:
# 获取所有未处理的事件
events = gamepad.return_status(clear_queue=True)
# 更新按键状态
for event in events:
if event['type'] == 'button':
button_name = event['name']
# 更新按钮状态
button_states[button_name] = bool(event['value'])
elif event['type'] == 'axis':
# 处理方向键
if event['name'] == 'D-pad X':
button_states['Left'] = (event['value'] == -32767)
button_states['Right'] = (event['value'] == 32767)
elif event['name'] == 'D-pad Y':
button_states['Up'] = (event['value'] == -32767)
button_states['Down'] = (event['value'] == 32767)
# 处理L2/R2触发键
elif event['name'] == 'LT':
button_states['L2'] = (event['value'] == 32767)
elif event['name'] == 'RT':
button_states['R2'] = (event['value'] == 32767)
print(button_states['R2'], event['value'])
#获取摇杆状态
elif event['name'] in axis_states:
joystick_name = event['name']
joystick_data = event['value']
if joystick_data != 0:
joystick_action_move = True
else:
joystick_action_move = False
joystick_current_numbers = 0
#将摇杆值映射到机械臂控制量
if joystick_action_move == True:
joystick_current_numbers += 1
if joystick_name == 'Left Stick X':
if joystick_data <= -AXIS_THRESHOLD:
arm.save_ang[1] -= arm_y_step
if joystick_data >= AXIS_THRESHOLD:
arm.save_ang[1] += arm_y_step
elif joystick_name == 'Left Stick Y':
if joystick_data <= -AXIS_THRESHOLD:
arm.save_ang[0] += arm_x_step
if joystick_data >= AXIS_THRESHOLD:
arm.save_ang[0] -= arm_x_step
elif joystick_name == 'Right Stick Y':
if joystick_data <= -AXIS_THRESHOLD:
arm.save_ang[2] += arm_z_step
if joystick_data >= AXIS_THRESHOLD:
arm.save_ang[2] -= arm_z_step
# 检查所有按键状态，执行持续动作
if button_states['A']: #持续按下A
pass
if button_states['B']: #持续按下B
pass
if button_states['X']: #持续按下X
pass
if button_states['Y']: #持续按下Y
pass
if button_states['Start']: #持续按下Start
pass
if button_states['Select']: #持续按下Select
pass
if button_states['Mode/Xbox']: #持续按下Mode
pass
if button_states['Left']: #持续按下Left
pass
if button_states['Right']: #持续按下Right
pass
if button_states['Up']: #持续按下Up
pass
if button_states['Down']: #持续按下Down
pass
if button_states['L2']: #持续按下L2
pass
if button_states['R2']: #持续按下R2
pass
if button_states['Left Shoulder']: #持续按下L1
# print(font.color_text('爪子打开', color="green", bold=True))
arm.catch_sth()
if button_states['Right Shoulder']: #持续按下R1
# print(font.color_text('爪子闭合', color="green", bold=True))
arm.release_sth()
#机械臂运动学限位
arm.save_ang[0], arm.save_ang[1], arm.save_ang[2] = arm.three_axis_data_limit(arm.save_ang[0], arm.save_ang[1], arm.save_ang[2])
if arm_command_queue.empty():
# 添加位移数据到队列
arm_command_queue.put_nowait((
arm.save_ang[0], arm.save_ang[1], arm.save_ang[2]
))
# 控制循环速度
time.sleep(0.0001)
except KeyboardInterrupt:
print("程序退出")
finally:
# 确保停止监听器
gamepad.stop()
操作手順：
デバッグ手順
- プロジェクトのあるシステムに入り、ターミナルからリモート接続します。ターミナルに次のコマンドを入力して Enter を押します：
ssh sunrise@192.168.8.111
sunrise
パスワードを入力して Enter を押すとログインが完了します。
- ロボットアームを初期化角度に調整します。
上腕と前腕の長さを測定して記録します。例えばテスト時には、上腕の実測長は 109cm、前腕の長さは 126cm でした。
プログラムを開き、赤枠部分のコードを修正します：
- ロボットアーム初期化状態でのサーボの角度を記録します。
ロボットアームが初期状態にあるときのサーボ値を記録し、次の表に記入します。以下はテスト時のデータ値です：
| ハードウェア | ロボットアーム初期状態に対応するサーボ値 |
|---|---|
| 旋回台サーボ | 83 |
| 上腕関節 | 100.9 |
| 前腕関節 | 95 |
| グリッパー | 141 |
- 運動学関数 void get_coordinate(float x1,float y1,float z1) の初期化パラメータを同期して更新します。パラメータ 83,100.9,95,141 を図の赤枠部分に反映してください。
- ロボットアームの可動範囲から離れた状態でスイッチを入れ、プログラムを実行します。
注意：ロボットアームの動作が速いと先端の慣性が大きくなります。起動時は、ぶつからないようロボットアームの球面状の可動範囲に入らないようにしてください。
再現時の注意点
-
再現する際は、各関節サーボの出力軸に取り付けるサーボホーンの向きが、実物写真やデモ動画と一致していることに注意してください。これはロボットアーム各関節の回転方向に影響し、運動学式における方向と関係します。向きが間違っていると、ある関節の回転角度や動作方向が大きくずれることになります。
-
ロボットアームの初期化状態では、各関節の角度をできるだけサーボの中央付近の範囲（80～150）に収めることを推奨します。この範囲外の場合は、構造の分解・組み直しによって調整することを推奨します。これはロボットアームの可動範囲に影響します。サーボの回転範囲は 0～270 度で中央値は 135 のため、この付近にすると関節サーボの可動範囲が最大になります。中央値の範囲が不適切だとロボットアームの到達可能な点が減少し、本来到達できるはずの点が運動学式で計算した結果解なしとなり、理論上の到達点に届かなくなります。
インターフェースドキュメント
さらに詳しくは、以下の API インターフェースドキュメントを参照して二次開発や最適化を行うことができます。
ロボット本体インターフェースの全体紹介
お問い合わせ
資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。