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()
|