76 lines
2.1 KiB
Python
76 lines
2.1 KiB
Python
'''
|
||
This demo show the communication interface of MR813 motion control board based on Lcm.
|
||
Dependency:
|
||
- robot_control_cmd_lcmt.py
|
||
- robot_control_response_lcmt.py
|
||
'''
|
||
import lcm
|
||
import sys
|
||
import os
|
||
import time
|
||
import threading
|
||
from threading import Thread, Lock
|
||
from task_3.task_3 import pass_stone
|
||
|
||
# from robot_control_cmd_lcmt import robot_control_cmd_lcmt
|
||
# from robot_control_response_lcmt import robot_control_response_lcmt
|
||
|
||
def circle2(ctrl, msg):
|
||
try:
|
||
#起立
|
||
msg.mode = 12
|
||
msg.gait_id = 0
|
||
msg.life_count += 1 # Command will take effect when life_count update
|
||
ctrl.Send_cmd(msg)
|
||
ctrl.Wait_finish(12, 0)
|
||
|
||
# #向左转
|
||
msg.mode = 11 # 旋转1
|
||
msg.gait_id = 3 # 自变频Trot步态
|
||
msg.vel_des = [0.0, 0.0, 0.4] # 顺时针旋转（负Z轴速度）
|
||
msg.duration = 4200 # 执行9.5秒后自动停止
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(10)
|
||
|
||
|
||
|
||
#直走
|
||
msg.mode = 11
|
||
msg.gait_id = 3
|
||
msg.vel_des = [0.3,0.0,0.0]
|
||
msg.duration = 2800
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(10)
|
||
|
||
# #向右转
|
||
msg.mode = 11 # 旋转1
|
||
msg.gait_id = 3 # 自变频Trot步态
|
||
msg.vel_des = [0.0, 0.0, -0.4] # 顺时针旋转（负Z轴速度）
|
||
msg.duration = 4200 # 执行9.5秒后自动停止
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(10)
|
||
|
||
|
||
# 过石板路
|
||
pass_stone(ctrl, msg, distance = 4.5, observe=False)
|
||
|
||
# 旋转至正面
|
||
msg.mode = 11 # 旋转1
|
||
msg.gait_id = 3 # 自变频Trot步态
|
||
msg.vel_des = [0.0, 0.0, 0.4] # 顺时针旋转（负Z轴速度）
|
||
msg.duration = 8400 # 执行9.5秒后自动停止
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(10)
|
||
|
||
except KeyboardInterrupt:
|
||
pass
|
||
ctrl.quit()
|
||
sys.exit() |