havoc420ubuntu
49a6a10f63
- Add new functions for turning, going straight, and circling - Implement standing up and lying down movements - Update task_1 to use new movement functions - Decode QR code information from image processor - Refactor main.py to run task_5 instead of task_1
83 lines
1.8 KiB
Python
83 lines
1.8 KiB
Python
import time
|
|
import cv2
|
|
|
|
def turn_90(ctrl, msg, direction="right"):
|
|
msg.mode = 11
|
|
msg.gait_id = 10
|
|
msg.vel_des = [1.1, 0, -1.5 if direction == "right" else 1.5]
|
|
msg.duration = 1500
|
|
msg.step_height = [0.06, 0.06]
|
|
msg.life_count += 1
|
|
ctrl.Send_cmd(msg)
|
|
time.sleep(1.5)
|
|
|
|
def turn_right_180(ctrl, msg):
|
|
msg.mode = 11
|
|
msg.gait_id = 3
|
|
msg.vel_des = [0.55, 0, -1.0]
|
|
msg.duration = 3500
|
|
msg.step_height = [0.06, 0.06]
|
|
msg.life_count += 1
|
|
ctrl.Send_cmd(msg)
|
|
time.sleep(3.5)
|
|
|
|
def go_straight(ctrl, msg, time_ms, fb=True):
|
|
msg.mode = 11
|
|
msg.gait_id = 26
|
|
msg.vel_des = [1 if fb else -1, 0, 0]
|
|
msg.duration = time_ms
|
|
msg.life_count += 1
|
|
ctrl.Send_cmd(msg)
|
|
time.sleep(time_ms / 1000)
|
|
|
|
def stand_up(ctrl, msg):
|
|
msg.mode = 12 # Recovery stand
|
|
msg.gait_id = 0
|
|
msg.life_count += 1
|
|
ctrl.Send_cmd(msg)
|
|
time.sleep(1.5)
|
|
|
|
def down_and_back(ctrl, msg):
|
|
# # TAG 趴下
|
|
msg.mode = 7
|
|
msg.gait_id = 1
|
|
msg.duration = 2000
|
|
msg.life_count += 1
|
|
ctrl.Send_cmd(msg)
|
|
ctrl.Wait_finish(7, 1)
|
|
|
|
stand_up(ctrl, msg)
|
|
|
|
go_straight(ctrl, msg, 1500, False)
|
|
|
|
def circle_90(ctrl, msg, direction="right"):
|
|
msg.mode = 11
|
|
msg.gait_id = 10
|
|
msg.duration = 1000
|
|
msg.vel_des = [0, 0, -2 if direction == "right" else 2]
|
|
msg.life_count += 1
|
|
ctrl.Send_cmd(msg)
|
|
time.sleep(1)
|
|
|
|
def run_task_1(ctrl, msg, image_processor):
|
|
print('Running task 1...')
|
|
|
|
# 右前方
|
|
turn_90(ctrl, msg)
|
|
|
|
# TAG take photo
|
|
qrcode_info = image_processor.decode_qrcode()
|
|
print(qrcode_info)
|
|
|
|
# TODO 不同的走向
|
|
turn_right_180(ctrl, msg)
|
|
|
|
go_straight(ctrl, msg, 200)
|
|
|
|
down_and_back(ctrl, msg)
|
|
|
|
circle_90(ctrl, msg)
|
|
|
|
# turn_90(ctrl, msg, 'left')
|
|
|