百度智能车竞赛丝绸之路1——智能车设计与编程实现控制
百度智能车竞赛丝绸之路2——手柄控制
一、机器人设计
二、实现原理
本教程使用Python的Serial库和Struct二进制数据解析库去实现Xbox手柄+百度大脑学习开发板（上位机）和机器人控制器（下位机）之间的通讯。
Xbox通过蓝牙模块与上位机进行通讯，上位机用Python中的Struct库去将蓝牙模块接收到的数据解析成四个元素的元组（时间、数据、类型、编号）。时间数据表示手柄与上位机每次发送字节数据进行通讯的时间戳，数据则表示手柄传给上位机的数据（value），其中button数据为{1,0}（离散值）、axis数据为{-32767,32768}（同样是离散值：2的16次方，也是代表两个字节数），类型数据包括{1,0}（0代表包含的数据为button值可以用int类型变量接收，1代表包含的数据为axis值可以用float类型变量接收），编号数据是对于手柄不同按键的排序。通过解析出来的手柄数据进而使上位机对机器人下位机进行通讯，实现了上位机对于机器人各个关节及其功能的控制。与下位机的通讯是基于485串口通讯协议进行控制的，机器人控制器只有一个串口，而与串口之间通讯同一时间只能通讯一次，也是后续项目开发过程中的难题所在。
手柄数据用button（int）列表和axis（float）列表接收，用joy_listenner-_thread线程实时对手柄数据进行解析并赋值给类成员变量self.button与self.axis。之后在手柄控制程序中实例化对象并开启类内成员线程。将与下位机通讯的数据与手柄数据相联系即可。手柄功能描述如下所示：
其中，更换模式可以更换手柄控制模式和自主巡检（做任务）模式，并且当手柄更换模式的时候蜂鸣笛响一声，RGB灯光切换灯光模式（闪烁和绿色）。而当程序退出时RGB等则呈现红色。当手柄更换只巡航模式是，灯光与蜂鸣笛状态都会发生改变，这是为了方便控制者的控制。
手柄控制模式中，防止各个机器人关节发生卡死现象（电机卡死不转动），需要对机器人各个关节电机做出限位，如下表所示：
三、编程实现
手柄解析joystick_struct.py
class JoyStick:
def __init__(self):
print('avaliable devices')
for fn in os.listdir('/dev/input'):
if fn.startswith('js'):
print('/dev/input/%s' % fn)
self.fn = '/dev/input/js0'
self.datas = [0] * 4
self.ev_buf = None
self.js_dev = None
self.done = True
self.flag = True
self.button = [0] * 15
self.axis = [0.0] * 15
self.listener_thread = Thread(target=self.read, daemon=True, name='read')
self.logs = False
def open(self):
self.js_dev = open(self.fn, 'rb')
# time, value, type_, number
def read(self):
while self.done and self.flag:
self.ev_buf = self.js_dev.read(8)
self.datas = struct.unpack('IhBB', self.ev_buf)
self.put_data()
def put_data(self):
if self.datas[2] & 0x01:
self.button[self.datas[3]] = self.datas[1]
if self.datas[2] & 0x02:
self.axis[self.datas[3]] = self.datas[1] / 32767
if self.logs:
print(self.button)
print(self.axis)
def __del__(self):
self.done = False
多线程检测thread_detection.py
from detector.detectors import Cruiser, TaskDetector, SignDetector
import threading
import cv2
class Mylane(threading.Thread):
def __init__(self, front_camera, cruiser: Cruiser, signdetector: SignDetector):
super().__init__() # 要先调用父类的init方法否则会报错
self.Done = True
# self.front_camera = cv2.VideoCapture(1) # 主摄像头为1
self.front_camera = front_camera
self.front_camera.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
self.front_camera.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
self.signdetector = signdetector
self.cruiser = cruiser
self.return_ = 0.0
self.landmark = 0
self.land_mark_thread = threading.Thread(target=self.land_mark, daemon=True, name='land_mark')
ret, self.front_image = self.front_camera.read()
self.land_mark_thread.start()
def land_mark(self):
while self.Done:
get_landmark = self.signdetector.detect(self.front_image)
# self.landmark = 0
print("get_landmark",get_landmark)
# print("总地标识别：", get_landmark)ssss
if len(get_landmark) == 0: # 未识别到结果 程序一直执行
self.landmark = 0
if len(get_landmark) != 0 and (
(str(get_landmark[0])[13:21] == "castle s" and str(get_landmark[0])[26:30] >= "0.80") or (str(get_landmark[0])[13:21] == "friendsh" and str(get_landmark[0])[30:34] >= "0.80")):
# print("get_landmark", str(get_landmark[0])[13:21])
self.landmark = 1
def run(self):
while self.Done:
ret, self.front_image = self.front_camera.read()
# print("front_image.shape:",front_image.shape)
self.return_ = self.cruiser.infer_cnn(self.front_image)
def __del__(self):
self.land_mark_thread.join()
class MyDetect(threading.Thread):
def __init__(self, side_camera, detector: TaskDetector):
super().__init__() # 要先调用父类的init方法否则会报错
self.detector = detector
# self.side_camera = cv2.VideoCapture(0) # 侧面摄像头为0
self.side_camera = side_camera
self.side_camera.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
self.side_camera.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
self.Done = True
self.have_done = [False] * 5
self.return_ = 0
def run(self):
while self.Done:
ret, side_image = self.side_camera.read()
# print("side_image.shape:", side_image.shape)
res_img = self.detector.detect(side_image)
# self.res_img=["dunhuang"]
# print(res_img)
result = res_img
print("***********************")
print(result)
print("***************************")
# print("result:",str(result[0])[13:21])
if len(result) == 0: # 未识别到结果 程序一直执行
self.return_ = 0
if len(result) != 0 and str(result[0])[13:21] == "alamutu ": # 识别到阿拉图 跳出循环 返回1
self.return_ = 1
# break
if len(result) != 0 and str(result[0])[13:21] == "dunhuang": # 识别到敦煌 跳出循环 返回2
self.return_ = 2
# break
if len(result) != 0 and str(result[0])[13:21] == "jstdb sc": # 识别到君士坦丁堡 跳出循环 返回3
self.return_ = 3
# break
if len(result) != 0 and str(result[0])[13:21] == "friendsh": # 识别到圣歌 跳出循环 返回4
# print(str(result[0])[13:21])
self.return_ = 4
手柄控制chassis_control.py
class RobotController:
def __init__(self, joy: pyjoy.JoyStick, _serial: serial.Serial):
self._serial = _serial
self.controller = MechainArmController(self._serial)
self.chassis_ = ChassisController(self._serial)
self.sensor_ = SensorController(self._serial)
self.compensator_ = CompensatorController(self._serial)
self.raise_flag_ = RaiseFlag(self._serial)
self.joy = joy
self.Break = False
def begin_control(self):
while True:
if self.joy.button[8] or self.joy.button[9]:
break
if self.joy.button[4] == 1:
self.controller.servo_control(90, 30)
if self.joy.button[5] == 1:
self.controller.servo_control(135, 30)
if self.joy.button[0] == 1:
self.raise_flag_.servo_control(self.raise_flag_.angle[0], 50)
elif self.joy.button[1] == 1:
self.raise_flag_.servo_control(self.raise_flag_.angle[1], 50)
elif self.joy.button[2] == 1:
self.raise_flag_.servo_control(self.raise_flag_.angle[2], 50)
elif self.joy.button[3] == 1:
self.raise_flag_.servo_control(self.raise_flag_.angle[3], 50)
else:
pass
if self.joy.axis[5] == -1:
self.controller.motor_rotate(40)
elif self.joy.axis[5] == 1:
self.controller.motor_rotate(-30)
else:
self.controller.motor_rotate(0)
try:
if self.joy.button[12] == 1 and not self.sensor_.buzzer_thread.is_alive():
self.sensor_.buzzer_thread.start()
self.sensor_.buzzer_thread.join()
except RuntimeError:
self.sensor_.buzzer_thread = threading.Thread(target=self.sensor_.Buzzers, daemon=True, name='Buzzer')
if self.joy.button[6] == 1:
self.compensator_.servo_control(135, 100)
elif self.joy.button[7] == 1:
self.compensator_.servo_control(45, 100)
cmd_vel = [0.0] * 4
if abs(self.joy.axis[1]) >= MIN_SENSITIVITY:
endwise_cmd = -self.joy.axis[1] * SPEED_MAX_PER
else:
endwise_cmd = 0.0
if abs(self.joy.axis[2]) >= MIN_SENSITIVITY:
infeed_cmd = -self.joy.axis[2] * SPEED_MAX_ANG
else:
infeed_cmd = 0.0
cmd_vel[0] = endwise_cmd - infeed_cmd
cmd_vel[1] = endwise_cmd + infeed_cmd
cmd_vel[2] = endwise_cmd - infeed_cmd
cmd_vel[3] = endwise_cmd + infeed_cmd
cmd_vel = speed_limit(cmd_vel)
self.chassis_.move(cmd_vel)
# print(sensor_.Groy_Sensor(2, 4))
self.sensor_.Nixie_Tube(self.sensor_.Groy_Sensor(2, 4))
print("over!!!")
def speed_limit(speeds):
i = 0
for speed in speeds:
if speed > 50:
speed = 50
elif speed < -50:
speed = -50
speeds[i] = speed
i = i + 1
return speeds
class Serial:
def __init__(self):
portx = "/dev/ttyUSB0"
if CONTROLLER == "mc601":
bps = 380400
elif CONTROLLER == "wobot":
bps = 115200
else:
bps = 115200
self.serial_ = serial.Serial(portx, int(bps), timeout=0.0005, parity=serial.PARITY_NONE, stopbits=1)
四、 不足之处
对于任务来讲，其中最大的问题应在于寻航任务控制过程由于串口通讯繁忙而不灵敏，使得其通讯频率降低，导致机器人最终实现效果呈现经常出现左右摆头的现象。从第一次验收与第二次验收结果都可以看出来。
对于机器人本身来讲，侧边摄像头是一个固定关节，以至于其只能对机器人单侧进行检测，并且刀片也是只能对单侧进行挥砍，这也是导致地图上的功能无法做完的原因之一。
五、 技术展望
针对于本小组设计的这款智能车，用很多改进的地方：
1.加长车体，使得车体空间增加。
2.加长刀片，使得同时兼顾左右方向，打击坏人。
3.侧边摄像头增加Z轴自由度，使得可以兼顾机器人左右方向的识别检测。
4.增加车体控制线程，使得机器人控制频率不受进程运行时间的干扰。并在线程中设立线程锁，使得多个线程之间不可以同时向驱动器写入数据。
