树莓派小车opencv循迹无法驱动电机，望指导

树莓派小车opencv循迹无法驱动电机，望大佬指教

import numpy as np
import cv2
import RPi.GPIO as GPIO
from time import sleep
cap = cv2.VideoCapture(2)
#设置摄像头采集视频宽640高360
cap.set(cv2.CAP_PROP_FRAME_WIDTH,640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,360)
#frame_height = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
#frame_width = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
#print(frame_height)
#print(frame_width)
servo_pin = 12                # 舵机信号线接树莓派12
moto_pin = 13     # 电调信号线接树莓派13

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(servo_pin, GPIO.OUT, initial=False)
GPIO.setup(moto_pin, GPIO.OUT, initial=False)
# 旋转角度转换到PWM占空比
def angleToDutyCycle(angle):
    return 2.5 + angle / 180.0 * 10
p = GPIO.PWM(servo_pin, 50)    # 初始频率为50HZ
p.start(angleToDutyCycle(90))  # 舵机初始化角度为90
x = GPIO.PWM(moto_pin, 200)   #电调初始频率为200HZ
sleep(0.5)
p.ChangeDutyCycle(0)           # 清空当前占空比，使舵机停止抖动
x.ChangeDutyCycle(0)     # 初始化占空比
kp = 0.2
kd = 0.4
last_diff = 0
while (True):
    ret, img = cap.read()
    img0 = cv2.cvtColor(img,cv2.COLOR_RGB2GRAY)        #图像灰度处理
    kernel = np.ones((5,5),np.uint8)        #设置卷积核大小  
#    img0 = cv2.erode(img0,kernel,iterations = 1)        #第一个参数：img0指需要腐蚀的图；第二个参数：kernel指腐蚀操作的内核；第三个参数：iterations指的是腐蚀次数，省略是默认为1
#    img0 = cv2.dilate(img0,kernel,iterations = 2)        #膨胀处理
#    img0 = cv2.erode(img0,kernel,iterations = 1)
#    img0 = cv2.dilate(img0,kernel,iterations = 2)
    img0 = cv2.GaussianBlur(img0,(3,3),0) #高斯降噪
#    cv2.namedWindow("capture0",0)        #得到的图像框就可以自行调整大小，可以拉伸进行自由调整
#    cv2.imshow("capture0",img0)            #创建窗口，图像展示
    yuzhi = 200 #越大越清晰 80
    img4 = cv2.getStructuringElement(cv2.MORPH_RECT, (1, (int)(img0.shape[0] / yuzhi)), (-1, -1))    #形态学处理，可以建立指定大小、形状的结构
    img5 = cv2.morphologyEx(img0, cv2.MORPH_OPEN, img4)        #进行开运算，指的是先进行腐蚀操作，再进行膨胀操作
    img5 = cv2.bitwise_not(img5)        #对图像取非操作，它的输入图像必须是二值图像
    img5 = cv2.erode(img5,kernel,iterations = 7)    #腐蚀
    img2 = cv2.Canny(img5, 100, 150)        #进行canny边缘检测；第一个参数：输入的图片；第二个参数：thresh1表示最小阈值；第三个参数：thresh2表示最大阈值；用于进一步删选边缘信息

    #提取左右边线数组 rows【370，395】
    t = 0
    left = [-1 for i in range(0,100)]
    right = [-1 for i in range(0,100)]
    mid = [-1 for i in range(0,100)]
    mid_sum = 0
    for i in range(300,200,-1):
        l_lost = 0
        r_lost = 0
        for j in range(300,10,-1):#左
            if(img2[i][j]==255):
                left[t] = j
                l_lost = 1
                break
        #丢点，左置为0
        if l_lost == 0:
            left[t] = 0
        for j in range(300,570):#右
            if(img2[i][j]==255):
                right[t] = j
                r_lost = 1
                break
        #丢点，右置为599
        if r_lost == 0:
            right[t] = 599
        #中线数组
        mid[t] = (left[t]+right[t])/2
        mid_sum = mid[t] + mid_sum
        t = t + 1
    mid_final = mid_sum/100
    print(mid_final)
    k = cv2.waitKey(1)
    if k == 27:
        break
    cv2.namedWindow("capture",0)
    cv2.imshow("capture",img2)
    diff = mid_final - 300
    #pd控制方向环
    angle = kp * diff + kd * (diff - last_diff)
    last_diff = diff
    p.ChangeDutyCycle(angleToDutyCycle(angle))
    sleep(0.1)
    p.ChangeDutyCycle(0) # 清空当前占空比，使舵机停止抖动
    #速度恒定 pwm给定 前进
    x.ChangeDutyCycle(4.5)
    sleep(1.5)
cap.release()