K210的MicroPython扩展例程——自动驾驶例程（视觉循迹）

前言

该例程实现的功能是循迹功能，可为想拿K210做视觉循迹开发作为参考
例程使用前需要搭建好MicroPython的开发环境
K210开发板MicroPython开发环境搭建

一、将K210连接好后打开CanMV IDE，连接成功后，三角形变成绿色

二、然后要把小车驱动库和PID控制库下载到内存卡的根目录上。

1、现在官方资料包找到下面三个源码，然后复制到文档/CanMV这个文件夹里

2、点这里，先把这两个库存进内存卡（前提是插上了内存卡）

3、点打开，依次写入这两个库

4、写入成功即可

三、在这个位置打开循迹例程

四、源码

import sensor, image, time, lcd

#导入所需模块
from modules import ybserial #串口通信模块
from robot_Lib import Robot  #机器人控制模块
from simplePID import PID    #PID控制器模块

#FollowLinePID = (22, 0, 2)
FollowLinePID = (15, 0, 2)  #循迹PID参数
SCALE = 1000.0         #PID控制器缩放比例

#初始化PID控制器
PID_controller = PID(
    160,
    FollowLinePID[0] / 1.0 / (SCALE),
    FollowLinePID[1] / 1.0 / (SCALE),
    FollowLinePID[2] / 1.0 / (SCALE))

#初始化串口通信
ser = ybserial()


#初始化机器人控制
bot = Robot(ser)

#设置机器人声音和运动状态
bot.set_beep(50)
bot.set_car_motion(0, 0, 0)

#初始化相机传感器
lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565) #设置图像格式为RGB565
sensor.set_framesize(sensor.QVGA)   #设置图像帧大小为QVGA
sensor.skip_frames(time = 100)      #跳过100ms的帧
sensor.set_auto_gain(False)         #关闭自动增益
sensor.set_auto_whitebal(True)      #打开自动白平衡

#初始化时间时钟
clock = time.clock()

#提示用户在摄像头前放置要跟踪的对象，并学习其颜色阈值
print("Hold the object you want to track in front of the camera in the box.")
print("MAKE SURE THE COLOR OF THE OBJECT YOU WANT TO TRACK IS FULLY ENCLOSED BY THE BOX!")

# Capture the color thresholds for whatever was in the center of the image.
# 50x50 center of QVGA.
#学习颜色阈值的中心图片区域
BOX = 30
r = [(320//2)-(BOX//2), (240//2)-(BOX//2), BOX, BOX]
for i in range(50):
    img = sensor.snapshot()
    img.draw_rectangle(r)
    lcd.display(img)

#学习颜色阈值
print("Learning thresholds...")
threshold = [BOX, BOX, 0, 0, 0, 0] # Middle L, A, B values.
for i in range(50):
    img = sensor.snapshot()
    hist = img.get_histogram(roi=r)
    lo = hist.get_percentile(0.01) # 获取直方图在1%范围内的累积分布函数值
    hi = hist.get_percentile(0.99) # 获取直方图在99%范围内的累积分布函数值
     # 取百分位值的平均值
    threshold[0] = (threshold[0] + lo.l_value()) // 2
    threshold[1] = (threshold[1] + hi.l_value()) // 2
    threshold[2] = (threshold[2] + lo.a_value()) // 2
    threshold[3] = (threshold[3] + hi.a_value()) // 2
    threshold[4] = (threshold[4] + lo.b_value()) // 2
    threshold[5] = (threshold[5] + hi.b_value()) // 2
    # 绘制目标物体的边界框和中心交叉点
    for blob in img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())
        img.draw_rectangle(r, color=(0,255,0))# 绘制颜色学习的区域
    lcd.display(img)

print("Thresholds learned...")# 学习阈值完成
print("Start Color Recognition...")# 开始颜色识别

state = 0# 初始化状态
while(True):
    clock.tick()
    img = sensor.snapshot() # 获取图像
    fps = clock.fps()       # 计算帧率
    data_in = 0
    index = 0
    for blob in img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10):
        #img.draw_rectangle(blob.rect())
        #img.draw_cross(blob.cx(), blob.cy())
        index = index + 1
        state = 1
        if index == 1:
            area_max = blob.w()*blob.h()
            area = blob
        else:
            temp_area = blob.w()*blob.h()
            if temp_area > area_max:
                area_max = temp_area
                area = blob
    if state == 1:
        print("area:", index, area.w(), area.h())
        value = PID_controller.incremental(area.cx())# 计算PID控制器输出值
        img.draw_rectangle(area.rect())# 绘制目标物体的边界框
        img.draw_cross(area.cx(), area.cy()) # 绘制目标物体的中心交叉点
        bot.set_car_motion(0.2, 0, value)# 设置机器人运动
        state = 0

    img.draw_string(0, 0, "%2.1ffps" %(fps), color=(0, 60, 128), scale=2.0)
    lcd.display(img)# 在LCD上显示帧率
    #print("FPS:s", fps)

五、点击下载运行即可

六、现象

等待系统初始化完成后，LCD显示摄像头画面，并且屏幕中间有一个白色的方框，请将要识别的颜色放到白色方框内，等待白色方框变绿则开始采集颜色，采集完成绿框消失，开始运行程序。

颜色识别的功能主要是分析颜色的LAB值，先把要识别的颜色放方框内，然后系统会根据方框内读取到的颜色的LAB值，再与摄像头采集到的颜色的LAB值作为分析对比，如果符合要求则画出方框，表示识别到该颜色，并将识别到的颜色的位置信息传输给PID控制器进行计算，判断出识别到的颜色与小车中间的偏移量，根据偏移量来修改小车前进的方向，从而达到小车视觉巡线的功能。

附加

1、PID库源码

# encoding: utf-8

import time

# simplePID.py
class PID(object):
    def __init__(self, target, P, I, D):

        self.Kp = P
        self.Ki = I
        self.Kd = D
        self.setPoint = target
        self.err = 0
        self.err_next = 0
        self.err_last = 0
        self.last_result = 0


    def __del__(self):
        print("DEL PID")

    # 重新设置目标值
    def reset_target(self, target):
        self.setPoint = target


    # 增量式PID计算方式
    def incremental(self, current_value, limit=0):
        self.err = self.setPoint - current_value
        result = self.last_result + self.Kp * (self.err - self.err_next) + self.Ki * self.err + self.Kd * (self.err - 2 * self.err_next + self.err_last)
        self.err_last = self.err_next
        self.err_next = self.err
        if limit > 0:
            if result > limit:
                result = limit
            if result < -limit:
                result = -limit
        self.last_result = result
        return result

2、小车驱动库源码

# coding: utf-8
import struct
import time

# robot_Lib.py
class Robot(object):

    def __init__(self, ser, delay=.002, debug=False):

        self.__delay_time = delay
        self.__debug = debug

        self.__HEAD = 0xFF
        self.__DEVICE_ID = 0xFC
        self.__COMPLEMENT = 257 - self.__DEVICE_ID
        self.__CAR_TYPE = 4
        self.__CAR_ADJUST = 0x80

        self.FUNC_AUTO_REPORT = 0x01
        self.FUNC_BEEP = 0x02
        self.FUNC_PWM_SERVO = 0x03
        self.FUNC_PWM_SERVO_ALL = 0x04
        self.FUNC_RGB = 0x05
        self.FUNC_RGB_EFFECT = 0x06

        self.FUNC_REPORT_SPEED = 0x0A
        self.FUNC_REPORT_IMU_RAW = 0x0B
        self.FUNC_REPORT_IMU_ATT = 0x0C
        self.FUNC_REPORT_ENCODER = 0x0D
        
        self.FUNC_RESET_STATE = 0x0F

        self.FUNC_MOTOR = 0x10
        self.FUNC_CAR_RUN = 0x11
        self.FUNC_MOTION = 0x12
        self.FUNC_SET_MOTOR_PID = 0x13
        self.FUNC_SET_YAW_PID = 0x14
        self.FUNC_SET_CAR_TYPE = 0x15

        self.FUNC_UART_SERVO = 0x20
        self.FUNC_UART_SERVO_ID = 0x21
        self.FUNC_UART_SERVO_TORQUE = 0x22
        self.FUNC_ARM_CTRL = 0x23
        self.FUNC_ARM_OFFSET = 0x24

        self.FUNC_AKM_DEF_ANGLE = 0x30
        self.FUNC_AKM_STEER_ANGLE = 0x31


        self.FUNC_REQUEST_DATA = 0x50
        self.FUNC_VERSION = 0x51

        self.FUNC_RESET_FLASH = 0xA0

        self.CARTYPE_X3 = 0x01
        self.CARTYPE_X3_PLUS = 0x02
        self.CARTYPE_X1 = 0x04
        self.CARTYPE_R2 = 0x05

        self.ser = ser

        time.sleep(.002)

    def __del__(self):
        print("serial Close!")



    # 蜂鸣器开关，on_time=0：关闭，on_time=1：一直响，
    # on_time>=10：响xx毫秒后自动关闭（on_time是10的倍数）。
    # Buzzer switch. On_time =0: the buzzer is off. On_time =1: the buzzer keeps ringing
    # On_time >=10: automatically closes after xx milliseconds (on_time is a multiple of 10)
    def set_beep(self, on_time):
        try:
            if on_time < 0:
                print("beep input error!")
                return
            value = bytearray(struct.pack('h', int(on_time)))

            cmd = [self.__HEAD, self.__DEVICE_ID, 0x05, self.FUNC_BEEP, value[0], value[1]]
            checksum = sum(cmd, self.__COMPLEMENT) & 0xff
            cmd.append(checksum)
            self.ser.send_bytearray(cmd)
            if self.__debug:
                print("beep:", cmd)
            time.sleep(self.__delay_time)
        except:
            print('---set_beep error!---')
            pass



    # 控制电机PWM脉冲，从而控制速度（未使用编码器测速）。speed_X=[-100, 100]
    # Control PWM pulse of motor to control speed (speed measurement without encoder). speed_X=[-100, 100]
    def set_motor(self, speed_1, speed_2, speed_3, speed_4):
        try:
            t_speed_a = bytearray(struct.pack('b', self.__limit_motor_value(speed_1)))
            t_speed_b = bytearray(struct.pack('b', self.__limit_motor_value(speed_2)))
            t_speed_c = bytearray(struct.pack('b', self.__limit_motor_value(speed_3)))
            t_speed_d = bytearray(struct.pack('b', self.__limit_motor_value(speed_4)))
            cmd = [self.__HEAD, self.__DEVICE_ID, 0x00, self.FUNC_MOTOR,
                   t_speed_a[0], t_speed_b[0], t_speed_c[0], t_speed_d[0]]
            cmd[2] = len(cmd) - 1
            checksum = sum(cmd, self.__COMPLEMENT) & 0xff
            cmd.append(checksum)
            self.ser.send_bytearray(cmd)
            if self.__debug:
                print("motor:", cmd)
            time.sleep(self.__delay_time)
        except:
            print('---set_motor error!---')
            pass


    # 控制小车向前、向后、向左、向右等运动。
    # state=[0, 7],=0停止,=1前进,=2后退,=3向左,=4向右,=5左旋,=6右旋,=7停车
    # speed=[-100, 100]，=0停止。
    # adjust=True开启陀螺仪辅助运动方向。=False则不开启。(此功能未开通)
    # Control the car forward, backward, left, right and other movements.
    # State =[0~6],=0 stop,=1 forward,=2 backward,=3 left,=4 right,=5 spin left,=6 spin right
    # Speed =[-100, 100], =0 Stop.
    # Adjust =True Activate the gyroscope auxiliary motion direction.  If =False, the function is disabled.(This function is not enabled)
    def set_car_run(self, state, speed, adjust=False):
        try:
            car_type = self.__CAR_TYPE
            if adjust:
                car_type = car_type | self.__CAR_ADJUST
            t_speed = bytearray(struct.pack('h', int(speed)))
            cmd = [self.__HEAD, self.__DEVICE_ID, 0x00, self.FUNC_CAR_RUN, \
                car_type, int(state&0xff), t_speed[0], t_speed[1]]
            cmd[2] = len(cmd) - 1
            checksum = sum(cmd, self.__COMPLEMENT) & 0xff
            cmd.append(checksum)
            self.ser.send_bytearray(cmd)
            if self.__debug:
                print("car_run:", cmd)
            time.sleep(self.__delay_time)
        except:
            print('---set_car_run error!---')
            pass

    # 小车运动控制,
    # Car movement control
    def set_car_motion(self, v_x, v_y, v_z):
        '''
        输入范围 input range:
        X3: v_x=[-1.0, 1.0], v_y=[-1.0, 1.0], v_z=[-5, 5]
        X3PLUS: v_x=[-0.7, 0.7], v_y=[-0.7, 0.7], v_z=[-3.2, 3.2]
        R2/R2L: v_x=[-1.8, 1.8], v_y=[-0.045, 0.045], v_z=[-3, 3]
        '''
        try:
            vx_parms = bytearray(struct.pack('h', int(v_x*1000)))
            vy_parms = bytearray(struct.pack('h', int(v_y*1000)))
            vz_parms = bytearray(struct.pack('h', int(v_z*1000)))
            cmd = [self.__HEAD, self.__DEVICE_ID, 0x00, self.FUNC_MOTION, self.__CAR_TYPE, \
                vx_parms[0], vx_parms[1], vy_parms[0], vy_parms[1], vz_parms[0], vz_parms[1]]
            cmd[2] = len(cmd) - 1
            checksum = sum(cmd, self.__COMPLEMENT) & 0xff
            cmd.append(checksum)
            self.ser.send_bytearray(cmd)
            if self.__debug:
                print("motion:", cmd)
            time.sleep(self.__delay_time)
        except:
            print('---set_car_motion error!---')
            pass


    # PID 参数控制，会影响set_car_motion函数控制小车的运动速度变化情况。默认情况下可不调整。
    # kp ki kd = [0, 10.00], 可输入小数。
    # forever=True永久保存，=False临时作用。
    # 由于永久保存需要写入芯片flash中，操作时间较长，所以加入delay延迟时间，避免导致单片机丢包的问题。
    # 临时作用反应快，单次有效，重启单片后数据不再保持。
    # PID parameter control will affect the set_CAR_motion function to control the speed change of the car.  This parameter is optional by default.
    # KP ki kd = [0, 10.00]
    # forever=True for permanent, =False for temporary.
    # Since permanent storage needs to be written into the chip flash, which takes a long time to operate, delay is added to avoid packet loss caused by MCU.
    # Temporary effect fast response, single effective, data will not be maintained after restarting the single chip
    def set_pid_param(self, kp, ki, kd, forever=False):
        try:
            state = 0
            if forever:
                state = 0x5F
            cmd = [self.__HEAD, self.__DEVICE_ID, 0x0A, self.FUNC_SET_MOTOR_PID]
            if kp > 10 or ki > 10 or kd > 10 or kp < 0 or ki < 0 or kd < 0:
                print("PID value must be:[0, 10.00]")
                return
            kp_params = bytearray(struct.pack('h', int(kp * 1000)))
            ki_params = bytearray(struct.pack('h', int(ki * 1000)))
            kd_params = bytearray(struct.pack('h', int(kd * 1000)))
            cmd.append(kp_params[0])  # low
            cmd.append(kp_params[1])  # high
            cmd.append(ki_params[0])  # low
            cmd.append(ki_params[1])  # high
            cmd.append(kd_params[0])  # low
            cmd.append(kd_params[1])  # high
            cmd.append(state)
            checksum = sum(cmd, self.__COMPLEMENT) & 0xff
            cmd.append(checksum)
            self.ser.send_bytearray(cmd)
            if self.__debug:
                print("pid:", cmd)
            time.sleep(self.__delay_time)
            if forever:
                time.sleep(.1)
        except:
            print('---set_pid_param error!---')
            pass

    # 舵机控制，servo_id：对应ID编号，angle：对应舵机角度值
    def set_pwm_servo(self, servo_id, angle):
        try:
            if servo_id < 1 or servo_id > 4:
                if self.__debug:
                    print("set_pwm_servo input invalid")
                return
            if angle > 180:
                angle = 180
            elif angle < 0:
                angle = 0
            cmd = [self.__HEAD, self.__DEVICE_ID, 0x00, self.FUNC_PWM_SERVO, int(servo_id), int(angle)]
            cmd[2] = len(cmd) - 1
            checksum = sum(cmd, self.__COMPLEMENT) & 0xff
            cmd.append(checksum)
            self.ser.send_bytearray(cmd)
            if self.__debug:
                print("pwmServo:", cmd)
            time.sleep(self.__delay_time)
        except:
            print('---set_pwm_servo error!---')
            pass

    # 同时控制四路PWM的角度，angle_sX=[0, 180]
    # At the same time control four PWM Angle, angle_sX=[0, 180]
    def set_pwm_servo_all(self, angle_s1, angle_s2, angle_s3=255, angle_s4=255):
        try:
            if angle_s1 < 0 or angle_s1 > 180:
                angle_s1 = 255
            if angle_s2 < 0 or angle_s2 > 180:
                angle_s2 = 255
            if angle_s3 < 0 or angle_s3 > 180:
                angle_s3 = 255
            if angle_s4 < 0 or angle_s4 > 180:
                angle_s4 = 255
            cmd = [self.__HEAD, self.__DEVICE_ID, 0x00, self.FUNC_PWM_SERVO_ALL, \
                   int(angle_s1), int(angle_s2), int(angle_s3), int(angle_s4)]
            cmd[2] = len(cmd) - 1
            checksum = sum(cmd, self.__COMPLEMENT) & 0xff
            cmd.append(checksum)
            self.ser.send_bytearray(cmd)
            if self.__debug:
                print("all Servo:", cmd)
            time.sleep(self.__delay_time)
        except:
            print('---set_pwm_servo_all error!---')
            pass


    # RGB可编程灯带控制，可单独控制或全体控制，控制前需要先停止RGB灯特效。
    # led_id=[0, 13]，控制对应编号的RGB灯；led_id=0xFF, 控制所有灯。
    # red,green,blue=[0, 255]，表示颜色RGB值。
    # RGB programmable light belt control, can be controlled individually or collectively, before control need to stop THE RGB light effect.
    # Led_id =[0, 13], control the CORRESPONDING numbered RGB lights;  Led_id =0xFF, controls all lights.
    # Red,green,blue=[0, 255], indicating the RGB value of the color.
    def set_colorful_lamps(self, led_id, red, green, blue, clear=0):
        try:
            id = int(led_id) & 0xff
            r = int(red) & 0xff
            g = int(green) & 0xff
            b = int(blue) & 0xff
            if clear != 0:
                clear = 1
            cmd = [self.__HEAD, self.__DEVICE_ID, 0x00, self.FUNC_RGB, id, r, g, b, clear]
            cmd[2] = len(cmd) - 1
            checksum = sum(cmd, self.__COMPLEMENT) & 0xff
            cmd.append(checksum)
            self.ser.send_bytearray(cmd)
            if self.__debug:
                print("rgb:", cmd)
            time.sleep(self.__delay_time)
        except:
            print('---set_colorful_lamps error!---')
            pass


if __name__ == '__main__':

    bot = Robot(debug=True)
    time.sleep(.1)
    bot.set_beep(50)
    time.sleep(.1)