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

最新推荐文章于 2023-03-08 16:45:42 发布

xuxian_

最新推荐文章于 2023-03-08 16:45:42 发布

阅读量2.3k

点赞数 5

文章标签：单片机嵌入式硬件 python 硬件工程 raspberry pi

本文链接：https://blog.csdn.net/xuxian_/article/details/122434750

版权

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

一、硬件

1.树莓派4B

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

（图中摄像头换为下图）
3.L298N（驱动模块）
L298N
4.18650 4000mAh 锂电池两节 + 电池盒

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

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

7.外观
请添加图片描述

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

二、连接

树莓派4B io口如下图
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 time

pygame.init()

IN1 = 16
IN2 = 18
IN3 = 36
IN4 = 38

GPIO.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.0

while camera.isOpened():
    ret, frame = camera.read()
    cv2.imshow('camera', frame)
    if p>10:
        p=10
    if p<2:
        p=2
    if f>10:
        f=10
    if f<3:
        f=3
    pan.ChangeDutyCycle(0)
    fan.ChangeDutyCycle(0)
    
    
    k = cv2.waitKey(1)
    
    if k==27:
        camera.release()
        cv2.destroyAllWindows()
        break
    if k == ord('i'):
        f+=0.3
        fan.ChangeDutyCycle(f)
    if k == ord('k'):
        f-=0.3
        fan.ChangeDutyCycle(f)
    if k == ord('j'):
        p+=0.3
        pan.ChangeDutyCycle(p)
    if k == ord('l'):
        p-=0.3
        pan.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 sys

IN1 = 16
IN2 = 18
IN3 = 36
IN4 = 38

GPIO.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.0

pygame.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=10
    if p<2:
        p=2
    if f>10:
        f=10
    if f<3:
        f=3
    
    for 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 = 0
            if event.key == pygame.K_a:
                key_list.remove('a')
                cha = 0
            if 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 += 4
        if i == 'a':
            cha -= 4
            
        if cha>50:
            cha=50
        if cha<-50:
            cha=-50
            
        if i == 'w':
            run(50+cha,50-cha)
        if i == 's':
            run(-50-cha,-50+cha)
        
        if i == 'i':
            f += 0.3
            fan.ChangeDutyCycle(f)
        if i == 'k':
            f -= 0.3
            fan.ChangeDutyCycle(f)
        if i == 'j':
            p += 0.3
            pan.ChangeDutyCycle(p)
        if i == 'l':
            p -= 0.3
            pan.ChangeDutyCycle(p)
          

    if not key_list:
        run(0,0)
        pan.ChangeDutyCycle(0)
        fan.ChangeDutyCycle(0)
        
    # draw frame
    screen.blit(img, (0,0))
    pygame.display.flip()
    # grab next frame    
    img = cam.get_image()