【树莓派4B】二自由度舵机云台、PC端遥控小车（丝滑遥控无卡顿）

很久之前（大约一年多）的课设内容，很多细节遗忘了

一、硬件

1.树莓派4B

2.二自由度舵机云台、树莓派配套摄像头

（图中摄像头换为下图）
3.L298N（驱动模块）

4.18650 4000mAh 锂电池两节 + 电池盒

5.充电模块
从尸体车上锯下来的，不放图了，可用电池充电盒代替

6.车体+电机
（实验室的垃圾堆捡的）

7.外观

其中充电宝参数为5V 3A标准树莓派供电要求

二、连接

树莓派4B io口如下图

需要完成的连接有电源(电池盒)与电机、驱动(L298N)、摄像头、树莓派以及充电模块的连接；电机与驱动(L298N)的连接；驱动(L298N)与树莓派连接；摄像头与树莓派连接；舵机云台与树莓派连接。
这里只介绍重点的连接：驱动与树莓派和电机的连接，云台与树莓派连接
L298N的输出端连上四个电机，电源线对应相连，输入端IN1~4连接如下：
IN1——GPIO 16
IN2——GPIO 18
IN3——GPIO 36
IN4——GPIO 38
panpin——GPIO 11 // 摄像头左右
fanpin—— GPIO 12 // 摄像头上下
千万别把红黑线接反！千万别把红黑线接反！千万别把红黑线接反！
不要连AB口的使能端！不要连AB口的使能端！不要连AB口的使能端！
据实践，AB口的使能端地和电源地不是同一个地

可以先尝试将驱动和电机连起来，给IN1加高电平IN2加低电平，观察左轮是否转动，再测试右轮，全部测试好之后再连其他线。注意接线的时侯不要短路，特别是软线线芯。

三、软件

目标：PC端键盘遥控，摄像头实时图传 WASD控制小车行走，IJKL控制摄像头角度移动

首先考虑cv库，有现成代码：

ret, frame = camera.read
k = cv2.waitKey(1) //获取按键
cv2.imshow('camera', frame) //显示图像(视频由一帧一帧图像构成)

可判断k的值为WASD/IJKL则实现车体/摄像头的运动

完整代码（python）：

import RPi.GPIO as GPIO
import cv2
import timepygame.init()IN1 = 16
IN2 = 18
IN3 = 36
IN4 = 38GPIO.setmode(GPIO.BOARD)
GPIO.setup(IN1, GPIO.OUT)
GPIO.setup(IN2, GPIO.OUT)
GPIO.setup(IN3, GPIO.OUT)
GPIO.setup(IN4, GPIO.OUT)p1 = GPIO.PWM(IN1,50)
p2 = GPIO.PWM(IN2,50)
p3 = GPIO.PWM(IN3,50)
p4 = GPIO.PWM(IN4,50)
p1.start(0)
p2.start(0)
p3.start(0)
p4.start(0)panPin = 11
fanPin = 12
GPIO.setup(panPin,GPIO.OUT)
GPIO.setup(fanPin,GPIO.OUT)
pan = GPIO.PWM(panPin,100)
fan = GPIO.PWM(fanPin,100)
pan.start(0)
fan.start(0)
pan.ChangeDutyCycle(2)
fan.ChangeDutyCycle(2)def forward(freq):p2.ChangeDutyCycle(0)p3.ChangeDutyCycle(0)p1.ChangeDutyCycle(freq)GPIO.output(IN2,GPIO.LOW)p4.ChangeDutyCycle(freq)GPIO.output(IN3,GPIO.LOW)def back(freq):p1.ChangeDutyCycle(0)p4.ChangeDutyCycle(0)p2.ChangeDutyCycle(freq)GPIO.output(IN1,GPIO.LOW)p3.ChangeDutyCycle(freq)GPIO.output(IN4,GPIO.LOW)def run(left,right):p1.ChangeDutyCycle(0)p2.ChangeDutyCycle(0)p3.ChangeDutyCycle(0)p4.ChangeDutyCycle(0)if left>=0:p1.ChangeDutyCycle(left)else:p2.ChangeDutyCycle(-left)if right>=0:p4.ChangeDutyCycle(right)else:p3.ChangeDutyCycle(-right)camera = cv2.VideoCapture(0)
p = 0.0
f = 0.0while camera.isOpened():ret, frame = camera.read()cv2.imshow('camera', frame)if p>10:p=10if p<2:p=2if f>10:f=10if f<3:f=3pan.ChangeDutyCycle(0)fan.ChangeDutyCycle(0)k = cv2.waitKey(1)if k==27:camera.release()cv2.destroyAllWindows()breakif k == ord('i'):f+=0.3fan.ChangeDutyCycle(f)if k == ord('k'):f-=0.3fan.ChangeDutyCycle(f)if k == ord('j'):p+=0.3pan.ChangeDutyCycle(p)if k == ord('l'):p-=0.3pan.ChangeDutyCycle(p)if k == ord('w'):forward(50)elif k == ord('s'):back(50)elif k == ord('a'):run(20,100)elif k == ord('d'):run(100,20) else:run(0,0)GPIO.cleanup()

然而实际效果差强人意，只要按键一直按着，树莓派连续响应按键，视频画面就会卡住不动
根本原因在于k = cv2.waitKey(1) 这一句，打开底层代码发现

我认为是delay和sleep搞的鬼，还有按下按键不放手是连续检测按键，这使得树莓派几乎将所有时间都用来检测按键-执行操作

另寻他法
查阅了很多资料后找到一个神奇的库：pygame
pygame.KEYDOWN表示按键按下事件，pygame.KEYUP表示按键松开事件
于是我想到用列表的方法使控制按键WASD/IJKL的按下顺序形成一个序列，按下某个按键之后即将此按键添加至待处理按键列表，松开按键则将按键从列表中删除，视频刷新穿插于按键检测之中，根据序列中的顺序对车体控制，若按键没松开，那么就不会触发按按键检测的事件，小车即专心的完成动作，比如一直按着W，小车只在按下一瞬间接收到前进的命令，其他时间全部用来刷新图传视频。
这种方法比cv库连续检测按键好的非常多

另外运动模型做了大幅度修整
设置变量cha表示左右轮速度差值，检测到A键cha+=4，检测到D键cha-=4，前进时车轮左右速度为(50+cha,50-cha)，后退时(-50-cha,-50+cha)
相较于上一版的简单前进后退左右转，这一模型比较合理适用。

完整代码（python）：

import pygame.camera
import RPi.GPIO as GPIO
import cv2
import time
import picamera
import sysIN1 = 16
IN2 = 18
IN3 = 36
IN4 = 38GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(IN1, GPIO.OUT)
GPIO.setup(IN2, GPIO.OUT)
GPIO.setup(IN3, GPIO.OUT)
GPIO.setup(IN4, GPIO.OUT)p1 = GPIO.PWM(IN1,50)
p2 = GPIO.PWM(IN2,50)
p3 = GPIO.PWM(IN3,50)
p4 = GPIO.PWM(IN4,50)
p1.start(0)
p2.start(0)
p3.start(0)
p4.start(0)panPin = 11
fanPin = 12
GPIO.setup(panPin,GPIO.OUT)
GPIO.setup(fanPin,GPIO.OUT)
pan = GPIO.PWM(panPin,50)
fan = GPIO.PWM(fanPin,50)
pan.start(0)
fan.start(0)def forward(freq):p2.ChangeDutyCycle(0)p3.ChangeDutyCycle(0)p1.ChangeDutyCycle(freq)GPIO.output(IN2,GPIO.LOW)p4.ChangeDutyCycle(freq)GPIO.output(IN3,GPIO.LOW)def back(freq):p1.ChangeDutyCycle(0)p4.ChangeDutyCycle(0)p2.ChangeDutyCycle(freq)GPIO.output(IN1,GPIO.LOW)p3.ChangeDutyCycle(freq)GPIO.output(IN4,GPIO.LOW)def run(left,right):p1.ChangeDutyCycle(0)p2.ChangeDutyCycle(0)p3.ChangeDutyCycle(0)p4.ChangeDutyCycle(0)if left>=0:p1.ChangeDutyCycle(left)else:p2.ChangeDutyCycle(-left)if right>=0:p4.ChangeDutyCycle(right)else:p3.ChangeDutyCycle(-right)key_list=[]
cha = 0
p = 0.0
f = 0.0pygame.camera.init()
cameras = pygame.camera.list_cameras()
cam = pygame.camera.Camera(cameras[0])
cam.start()img = cam.get_image()WIDTH = img.get_width()
HEIGHT = img.get_height()screen = pygame.display.set_mode( ( WIDTH, HEIGHT ) )
pygame.display.set_caption("xuxian's smartcar")while True :if p>10:p=10if p<2:p=2if f>10:f=10if f<3:f=3for event in pygame.event.get():if event.type == pygame.QUIT :sys.exit()if event.type == pygame.KEYDOWN:  # 将按下的按键添加至列表if event.key == pygame.K_d:key_list.append('d')if event.key == pygame.K_a:key_list.append('a')if event.key == pygame.K_w:key_list.append('w')if event.key == pygame.K_s:key_list.append('s')if event.key == pygame.K_l:key_list.append('l')if event.key == pygame.K_j:key_list.append('j')if event.key == pygame.K_i:key_list.append('i')if event.key == pygame.K_k:key_list.append('k')if event.type == pygame.KEYUP:  # 将松开的按键从列表删去if event.key == pygame.K_d:key_list.remove('d')cha = 0if event.key == pygame.K_a:key_list.remove('a')cha = 0if event.key == pygame.K_w:key_list.remove('w')if event.key == pygame.K_s:key_list.remove('s')if event.key == pygame.K_i:key_list.remove('i')if event.key == pygame.K_k:key_list.remove('k')if event.key == pygame.K_j:key_list.remove('j')if event.key == pygame.K_l:key_list.remove('l')#print(key_list)for i in key_list:if i == 'd':cha += 4if i == 'a':cha -= 4if cha>50:cha=50if cha<-50:cha=-50if i == 'w':run(50+cha,50-cha)if i == 's':run(-50-cha,-50+cha)if i == 'i':f += 0.3fan.ChangeDutyCycle(f)if i == 'k':f -= 0.3fan.ChangeDutyCycle(f)if i == 'j':p += 0.3pan.ChangeDutyCycle(p)if i == 'l':p -= 0.3pan.ChangeDutyCycle(p)if not key_list:run(0,0)pan.ChangeDutyCycle(0)fan.ChangeDutyCycle(0)# draw framescreen.blit(img, (0,0))pygame.display.flip()# grab next frame    img = cam.get_image()

完结，想起细节再更新