113 lines
3.3 KiB
Python
113 lines
3.3 KiB
Python
'''
|
||
This demo show the communication interface of MR813 motion control board bactrlsed 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 go_straight_with_enhanced_calibration
|
||
|
||
# from robot_control_cmd_lcmt import robot_control_cmd_lcmt
|
||
# from robot_control_response_lcmt import robot_control_response_lcmt
|
||
|
||
def circle1(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 = 4600 # 执行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 = 3400
|
||
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)
|
||
|
||
#上坡
|
||
go_straight_with_enhanced_calibration(ctrl, msg, distance = 5, speed=0.5, observe=False, mode=11, gait_id=3, step_height=[0.21, 0.21])
|
||
|
||
#停五秒
|
||
msg.mode = 12
|
||
msg.gait_id = 0
|
||
msg.vel_des = [0.0,0.0,0.0]
|
||
msg.duration = 6000
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(7)
|
||
|
||
#直走
|
||
msg.mode = 11
|
||
msg.gait_id = 3
|
||
msg.vel_des = [0.3,0.0,0.0]
|
||
msg.duration = 9000
|
||
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 = 4000 # 执行9.5秒后自动停止
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(5)
|
||
|
||
#直走进入扫码区
|
||
msg.mode = 11
|
||
msg.gait_id = 3
|
||
msg.vel_des = [0.3,0.0,0.0]
|
||
msg.duration = 3400
|
||
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 = 4600 # 执行9.5秒后自动停止
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(5)
|
||
|
||
except KeyboardInterrupt:
|
||
pass
|
||
ctrl.quit()
|
||
sys.exit()
|