k210实现PID控制，云台追踪色块

还是一个小练习，刚了解了PID的用法，在之前云台追踪的基础上进行改进，目前只想出了闭环控制，其它更加精妙的控制还需要多加思考学习。

关于基础的云台实时追踪色块思路可以看这一篇：

K210云台追踪色块

这里仅对其中的一些代码进行了更进，其余基础逻辑仍然相同。

1. 首先先进行舵机的类定义和初始化两个舵机，这里已经开始定义PID的值了

kp, ki, kd

虽然这是简单的位置PID，我除了p，仍然设置了另外两个，以便查看测试结果

class Motor_Property():
    motor1_duty      = 0         # 舵机1 占空比
    motor2_duty      = 0         # 舵机2 占空比

    duty1_center   = 0      # 当物体移至画面中心时,占空比中间值,即舵机的位置
    duty2_center   = 0      # 也为pid中的初始值

    motor1_pin  = 0              # 舵机1 引脚
    motor2_pin  = 0              # 舵机2 引脚

    control_x   = 0            # 被控坐标 x
    control_y   = 0            # 被控坐标 y
    control_flag_x = 'x'       #打个向哪个地方转的标签
    control_flag_y = 'y'

    kp = 0      #比例值
    ki = 0      #积分值
    kd = 0      #微分, 暂时不用

    error_x = 0  # x轴方向上的误差
    error_y = 0  # y轴方向上的误差

    last_error_x = 0  # x轴 上一次的误差
    last_error_y = 0  #

    p_out_x = 0 #比例输出, 快速达到目标值附近
    i_out_x = 0 #积分输出, 减小稳定误差
    d_out_x = 0 #微分输出, 减小振荡

    p_out_y = 0 #比例输出, 快速达到目标值附近
    i_out_y = 0 #积分输出, 减小稳定误差
    d_out_y = 0 #微分输出, 减小振荡

    total_out_x = 0 #总输出, pid之和
    total_out_y = 0


# 实例化舵机类
motor = Motor_Property()     # 实例化舵机类 Motor_Property() 为 motor
motor.motor1_duty = 7.5          # 舵机1的占空比 初始设置, x轴是7.5%为中心,duty范围为2-13
motor.motor2_duty = 8            # 舵机2的占空比 初始设置, y轴是 8%为水平线

motor.duty1_center   = 7.5
motor.duty2_center   = 6     # 中心位置

motor.motor1_pin  = 11       # 舵机1的引脚 也就是x轴
motor.motor2_pin  = 12       # 舵机2的引脚 也就是y轴

motor.control_x = 160
motor.control_y = 120                        # 被控坐标

control_flag_x = 'x'                   #打个向哪个地方转的标签
control_flag_y = 'y'

motor.kp = 0.15
motor.ki = 0.015
motor.kd = 0.0012

2.创建舵机对象，输出PWM波

motor1 = PWM(tim_1_ch0, freq=timer1.freq,duty=motor.motor1_duty,pin=11)# 舵机1, x轴
motor2 = PWM(tim_1_ch1, freq=timer1.freq,duty=motor.motor2_duty,pin=12)# 舵机2, y轴

3.写出计算PID的函数，得出我们需要的duty占空比

通过这个我们可以多次调参，我的镜头像素太差，来回晃动严重，只好都用一下试试看，哪种最快最稳当。

#定义舵机占空比控制函数
def motor_control_pid(motor,x,y):           # 比如控制舵机也可以在这里写

    motor.error_x = motor.control_x - x # Error 误差值, 现在的中心点-目标点, 换算成duty的误差值
    motor.error_y = motor.control_y - y

    motor.p_out_x = motor.kp * motor.error_x  # x轴中输出的p, 快速到达目标点
    motor.i_out_x += motor.ki * motor.error_x # x轴中输出的i, 累计误差, 只涉及到位置, 故不需要
    motor.d_out_x = motor.kd * (motor.error_x - motor.last_error_x) # d, 减弱振荡

    motor.p_out_y = motor.kp * motor.error_y
    motor.i_out_y += motor.ki * motor.error_y
    motor.d_out_y = motor.kd * (motor.error_y - motor.last_error_y)

    motor.total_out_x = (motor.p_out_x + motor.i_out_x + motor.d_out_x) *0.034375 # 总输出
    motor.total_out_y = (motor.p_out_y + motor.i_out_y + motor.d_out_y) *0.016665


    if x != motor.control_x:     # 如果不在画面中心, 则
        motor.motor1_duty = motor.duty1_center + motor.total_out_x
        if motor.motor1_duty<0 :
            motor.motor1_duty = - motor.motor1_duty
        print("x_duty:", motor.motor1_duty)
        # print(motor.motor1_duty)

    if y != motor.control_y:
        motor.motor2_duty = motor.duty2_center - motor.total_out_y
        if motor.motor1_duty<0 :
            motor.motor1_duty = - motor.motor1_duty
        print("y_duty:", motor.motor2_duty)
        # print(motor.motor2_duty)

    motor.last_error_x = motor.error_x
    motor.last_error_y = motor.error_y

4.主函数

简单定义一下极限值，防止duty过值程序报错。

要实现保持物体在画面中心，需要实时更改我们的中心坐标。

while(True):
    clock.tick()
    img = sensor.snapshot()
    lcd.display(img)

    opv_find_blobs(black,1)  #找线！
    motor_control_pid(motor, black.cx, black.cy)

    if (motor.motor1_duty >=13 ):
        motor.motor1_duty = 13
    if (motor.motor2_duty >=8):
        motor.motor2_duty = 8
    if (motor.motor1_duty <=3):
        motor.motor1_duty = 3
    if (motor.motor2_duty <=3):
        motor.motor2_duty = 3       # 3是向上抬头

    motor1.duty(motor.motor1_duty)  # 将获取到的电机1占空比 装载,x轴
    motor2.duty(motor.motor2_duty)
    print("x_duty:", motor.motor1_duty)
    print("y_duty:", motor.motor2_duty)

    if (black.cx==motor.control_x and black.cy==motor.control_y):
        motor.duty1_center = motor.motor1_duty
        motor.duty2_center = motor.motor2_duty
        break