树莓派 python 小车巡线行驶

学习参考：https://www.jianshu.com/p/9f8915caca13  （OpenCV定位轮廓的中点）

最终效果 代码在上面链接中

树莓派 python 小车巡线行驶代码注释

import numpy as np
import cv2
import Adafruit_PCA9685 # 舵机
import  RPi.GPIO as GPIO
import time  

video_capture = cv2.VideoCapture(0)  # 打开摄像头
video_capture.set(3,160) # 设置窗口大小，太大运算慢
video_capture.set(4,120) 

# 树莓派小车电机驱动初始化
PWMA = 18
AIN1   =  22
AIN2   =  27

PWMB = 23
BIN1   = 25
BIN2  =  24

BtnPin  = 19
Gpin    = 5
Rpin    = 6

#初始化舵机  实例化
pwm = Adafruit_PCA9685.PCA9685()
# Configure min and max servo pulse lengths
# 定义舵机控制范围150-600
servo_min = 150  # Min pulse length out of 4096
servo_max = 600  # Max pulse length out of 4096
# 辅助功能，使设置舵机脉冲宽度更简单。
def set_servo_pulse(channel, pulse):
    pulse_length = 1000000    # 1,000,000 us per second
    pulse_length //= 60       # 60 Hz
    print('{0}us per period'.format(pulse_length))
    pulse_length //= 4096     # 12 bits of resolution
    print('{0}us per bit'.format(pulse_length))
    pulse *= 1000
    pulse //= pulse_length
    pwm.set_pwm(channel, 0, pulse)

# 舵机脉宽转化为角度
def set_servo_angle(channel,angle):
    angle=4096*((angle*11)+500)/20000
    pwm.set_pwm(channel,0,int(angle))

# 频率设置为50hz，适用于舵机系统。
pwm.set_pwm_freq(50)
#　巡线摄像头向下  垂直坐标系
set_servo_angle(5,90)  # 底座舵机 90 
set_servo_angle(4,145)  # 顶部舵机 145

time.sleep(0.5)

# 树莓派小车运动函数
def t_up(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,True) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,True) #BIN1
        time.sleep(t_time)
        
def t_stop(t_time):
        L_Motor.ChangeDutyCycle(0)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(0)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)
        
def t_down(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,True)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,True)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)

def t_left(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,True)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,True) #BIN1
        time.sleep(t_time)

def t_right(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,True) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,True)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)  

def keysacn():
    val = GPIO.input(BtnPin)
    while GPIO.input(BtnPin) == False:
        val = GPIO.input(BtnPin)
    while GPIO.input(BtnPin) == True:
        time.sleep(0.01)
        val = GPIO.input(BtnPin)
        if val == True:
            GPIO.output(Rpin,1)
            while GPIO.input(BtnPin) == False:
                GPIO.output(Rpin,0)
        else:
            GPIO.output(Rpin,0)

GPIO.setwarnings(False) 
GPIO.setmode(GPIO.BCM)　
GPIO.setup(Gpin, GPIO.OUT)     # 设置绿色Led引脚模式输出
GPIO.setup(Rpin, GPIO.OUT)     # 设置红色Led引脚模式输出
GPIO.setup(BtnPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)    # 设置输入BtnPin模式，拉高至高电平(3.3V) 

GPIO.setup(AIN2,GPIO.OUT)
GPIO.setup(AIN1,GPIO.OUT)
GPIO.setup(PWMA,GPIO.OUT)

GPIO.setup(BIN1,GPIO.OUT)
GPIO.setup(BIN2,GPIO.OUT)
GPIO.setup(PWMB,GPIO.OUT)

L_Motor= GPIO.PWM(PWMA,100)
L_Motor.start(0)

R_Motor = GPIO.PWM(PWMB,100)
R_Motor.start(0)

keysacn()
while 1:
    # Capture the frames
    ret, frame = video_capture.read()
    # Crop the image　  
    crop_img =frame[60:120, 0:160]
    # Convert to grayscale 二值图像处理，灰度化
    gray = cv2.cvtColor(crop_img, cv2.COLOR_BGR2GRAY)
    # Gaussian blur  高斯模糊:减少图像噪声以及降低细节层次
    blur = cv2.GaussianBlur(gray,(5,5),0)
    '''
     # Color thresholding 阈值处理
     src：表示的是图片源
     thresh：表示的是阈值（起始值）
     maxval：表示的是最大值
     type：表示的是这里划分的时候使用的是什么类型的算法**，常用值为0（cv2.THRESH_BINARY）**
    '''
    ret,thresh1 = cv2.threshold(blur,60,255,cv2.THRESH_BINARY_INV)
    # Erode and dilate to remove accidental line detections
    # 形态学处理 消除噪音
    mask = cv2.erode(thresh1, None, iterations=2) # 腐蚀
    mask = cv2.dilate(mask, None, iterations=2) #膨胀
    
   # Find the contours of the frame　寻找所有轮廓
    image,contours,hierarchy = cv2.findContours(mask.copy(), 1, cv2.CHAIN_APPROX_NONE)
    # Find the biggest contour (if detected)　最大轮廓
    if len(contours) > 0:
        c = max(contours, key=cv2.contourArea)# cv2.contourArea轮廓面积
        M = cv2.moments(c) # cv2.moments()是用于计算图像矩，然后通过图像矩计算质心
        　
        # 求轮廓的中心位置
        cx = int(M['m10']/M['m00'])　
        cy = int(M['m01']/M['m00'])
        
        '''
        cv2.line(plot,(0,y),(int(h * mul),y),(255,0,0),w)
        第一个参数 img：要划的线所在的图像;
        第二个参数 pt1：直线起点
        第三个参数 pt2：直线终点
        第四个参数 color：直线的颜色 
        第五个参数 thickness=1：线条粗细
        '''
        # 过质心的两条线
        cv2.line(crop_img,(cx,0),(cx,720),(255,0,0),1)
        cv2.line(crop_img,(0,cy),(1280,cy),(255,0,0),1)
        
        cv2.drawContours(crop_img, contours, -1, (0,255,0), 1)
        # 偏右 朝左边
        if cx >= 120:
            t_right(50,0)                 
            print "Turn Left!"
        # 50-120范围一直前进
        if cx < 120 and cx > 50:
            t_up(50,0)
            print "On Track!"
        if cx <= 50:
        # 偏左，向右
            t_left(50,0)   
            print "Turn Right"
    else:
        print "I don't see the line"   
    #Display the resulting frame
    cv2.imshow('frame',crop_img)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        L_Motor.stop()
        R_Motor.stop()
        GPIO.cleanup()    
        break