基于ARDUINO的PID巡线小车

文章结构

l298n的使用
传感器数据的获得和处理
Pid的使用
差速转向的方法

第一部分 l298n的使用

Part1 PWM(脉宽调制信号）的认识与arduino的实现

PWM是一种对模拟信号电平进行数字编码的方法。通过高分辨率计数器的使用，方波的占空比被调制用来对一个具体模拟信号的电平进行编码。PWM信号仍然是数字的，因为在给定的任何时刻，满幅值的直流供电要么完全有（ON），要么完全无（OFF）。电压或电流源是以一种通（ON）或断（OFF）的重复脉冲序列被加到模拟负载上去的。通的时候即是直流供电被加到负载上的时候，断的时候即是供电被断开的时候。只要带宽足够，任何模拟值都可以使用PWM进行编码。

在本例中，主要的作用是用于控制L298N输出的电压来从而控制电机的转速

Part2:L298N的使用

关于怎么接线的问题，在本例中，我们对于上图的A/B相跳帽就不要拔下来，对输入1/2认为是控制通道A的电机转速及其方向，那么转速可以理解为用占空比控制，方向呢？

方向控制采用对1/2输入来输入pwm来控制正转抑或是反转，请对一个输入针脚输入信号的时候务必保留另一个针脚为低电平状态，（比如控制通道A的电机反转，给到输入2pwm,给到输入1低电平）对于通道B亦然。

以下是打包好的电机驱动例子

//设置电机转速
void Run_motor(int speed_l, int speed_r) {
  if(speed_l >= 0 && speed_r >= 0){
    if(speed_l > 210){
      speed_l =210;
    }
    else if(speed_l < 45){
      speed_l = 45;
    }
    if(speed_r > 240){
      speed_r =240;
    }
    else if(speed_r < 45){
      speed_r = 45;
    }
    analogWrite(motor_l, speed_l);
    analogWrite(motor_r, speed_r);
    analogWrite(motor_l_f, 0);
    analogWrite(motor_r_f, 0);
  }
  else if(speed_l < 0 && speed_r >= 0){
    speed_l = -speed_l;
    if(speed_l > 210){
      speed_l =210;
    }
    else if(speed_l < 45){
      speed_l = 45;
    }
    if(speed_r > 240){
      speed_r =240;
    }
    else if(speed_r < 45){
      speed_r = 45;
    }
    analogWrite(motor_l_f, speed_l);
    analogWrite(motor_r, speed_r);
    analogWrite(motor_l, 0);
    analogWrite(motor_r_f, 0);
  }
  else if(speed_l >= 0 && speed_r < 0){
    speed_r = - speed_r;
    if(speed_l > 210){
      speed_l =210;
    }
    else if(speed_l < 45){
      speed_l = 45;
    }
    if(speed_r > 240){
      speed_r =240;
    }
    else if(speed_r < 45){
      speed_r = 45;
    }
    analogWrite(motor_l, speed_l);
    analogWrite(motor_r_f, speed_r);
    analogWrite(motor_l_f, 0);
    analogWrite(motor_r, 0);
  }
  else if(speed_l < 0 && speed_r < 0){
    speed_l = -speed_l;
    speed_r = - speed_r;
    if(speed_l > 210){
      speed_l =210;
    }
    else if(speed_l < 45){
      speed_l = 45;
    }
    if(speed_r > 240){
      speed_r =240;
    }
    else if(speed_r < 45){
      speed_r = 45;
    }
    analogWrite(motor_l_f, speed_l);
    analogWrite(motor_r_f, speed_r);
    analogWrite(motor_l, 0);
    analogWrite(motor_r, 0);
  }
}

（通道B的接线可能接反了，懒得改了，车还在学校

第二部分：传感器数据的获得与处理

Part1:红外对管的使用

可以简略地认为红外对管地作用就是检测其前面有无物体遮挡，对于这个例子而言，我们是检测赛道上面是否存在黑线。当其检测到黑线，输出高电平。

所以为了防止传感器互相之间出现干扰，我们选择把传感器拉开，如果碰巧你有一台激光切割机的话，可以切一块半圆形的亚克力来固定传感器。如果不巧你没有，可以用3d打印机打一块出来。如果你都没有，~~给我写邮件我v你一块。~~

Part2:数据的处理方式

对于一大堆传感器，想要实现pid的闭环控制，数据的处理至关重要。对于这一对传感器我们使用如下方式对数据进行处理

为什么要歪歪的：为了处理各种不可靠的十字路口
如果车冲出赛道如何回来：马上讲到

那么我们尝试用代码实现以上的例子

double Value_count(int *value) {
    double res = 0;
    int count = 0;
    double return_value;
    for (int i = -3; i < 4; i++) {
        //Serial.print(i);
        //Serial.print(":");
        //Serial.println(value[i + 3]);
        res = res + i * value[i + 3];  //检测到是1，没检测到是0
        count = count + value[i + 3];
    }
    if (count == 0){
      res = last_value;
      count = 1;
    }
    else{
      last_value = res;
    }
    //Serial.print("now value count(with out /):");
    //Serial.println(res);
    //Serial.print("count:");
    //Serial.println(count);
    return_value = (res / count);
    return return_value;
}

这样就得到了处理过的数据辣

下一步是考虑如何处理冲出赛道的情况了，在上面已经有体现，即未检测到任何值时保留上一次数据不变。这样就能让车的鲁棒性起飞。

第三部分：PID的使用

Part1:初识PID（不是进程号

PID控制器（比例-积分-微分控制器），由比例单元（Proportional）、积分单元（Integral）和微分单元（Derivative）组成。可以透过调整这三个单元的增益 $K_{p}$ ， $K_{i}$ 和 $K_{d}$ 来调定其特性。PID控制器主要适用于基本上线性，且动态特性不随时间变化的系统。
PID控制器是一个在工业控制应用中常见的反馈回路部件。这个控制器把收集到的数据和一个参考值进行比较，然后把这个差别用于计算新的输入值，这个新的输入值的目的是可以让系统的数据达到或者保持在参考值。PID控制器可以根据历史数据和差别的出现率来调整输入值，使系统更加准确而稳定。
PID控制器的比例单元（P）、积分单元（I）和微分单元（D）分别对应目前误差、过去累计误差及未来误差。若是不知道受控系统的特性，一般认为PID控制器是最适用的控制器。借由调整PID控制器的三个参数，可以调整控制系统，设法满足设计需求。控制器的响应可以用控制器对误差的反应快慢、控制器过冲的程度及系统震荡的程度来表示。不过使用PID控制器不一定保证可达到系统的最佳控制，也不保证系统稳定性。
有些应用只需要PID控制器的部分单元，可以将不需要单元的参数设为零即可。因此PID控制器可以变成PI控制器、PD控制器、P控制器或I控制器。其中又以PI控制器比较常用，因为D控制器对回授噪声十分敏感，而若没有I控制器的话，系统不会回到参考值，会存在一个误差量。

看起来很复杂，其实不然，看下面这个图

为了充分了解pid，请想象这样一个例子：

一个杯子，但是漏水
不但漏水，还随机蒸发一定数量的水（什么垃圾
你被要求每次往里面加一定数量的水
要求水稳定在某一水位

这便是一个及其经典的pid例子，我们用python来实现这个例子吧

import math, time
from pylab import *
import random
import matplotlib

global water_tank, pp, pi, pd, except_number, js
global sigma_error, last_error, times, time_list, num_list,sum_list
sum_list = []
time_list = []
num_list = []
water_tank = -100
sigma_error = 0
last_error = 0
# .8,.5,.2 懂得都懂
pp = 0.4
pi = 0.05
pd = -0.5
times = 0
js = 0


def water_blow():
    global water_tank

    int_num = random.random()
    water_tank = water_tank - 1 * int_num
    water_tank = water_tank - 1
    return water_tank


try:
    except_number = int(10)
except ValueError:
    except_number = 100


def pdi():#再写一些很新的东西
    global water_tank, pp, pi, pd, except_number, \
        sigma_error, last_error, times, time_list, num_list \
        , sum_list, js
    error = except_number - water_tank
    add_p = pp * error
    sigma_error = error
    if len(sum_list) < 10:
        sum_list.append(sigma_error)
    else:
        if js < 10:
            sum_list[js] = sigma_error
            js = js + 1
        elif js >= 10:
            js = 0
            sum_list[js] = sigma_error
            js = js + 1
    sum_sum_five = 0
    for sigma in sum_list:
        sum_sum_five = sum_sum_five + sigma
    add_d = water_tank - last_error
    last_error = water_tank
    delta = ((pp * add_p) + (pi * sum_sum_five)+(pd * add_d) ) * (1+abs(pd * add_d)  )# 这是一个pid算法
    water_tank = delta + water_tank
    # print(sum_list)
    # print(water_tank)
    #print(add_p, sum_sum_five, add_d, delta, water_tank)
    times = times + 1
    time_list.append(times)
    num_list.append(water_tank)
    print(water_tank)


def pid():
    global water_tank, pp, pi, pd, except_number, \
        sigma_error, last_error, times, time_list, num_list \
        , sum_list, js
    error = except_number - water_tank
    add_p = pp * error
    sigma_error = error
    if len(sum_list) < 10:
        sum_list.append(sigma_error)
    else:
        if js < 10:
            sum_list[js] = sigma_error
            js = js + 1
        elif js >= 10:
            js = 0
            sum_list[js] = sigma_error
            js = js + 1
    sum_sum_five = 0
    for sigma in sum_list:
        sum_sum_five = sum_sum_five + sigma
    add_d = water_tank - last_error
    last_error = water_tank
    delta = (pp * add_p) + (pi * sum_sum_five) + (pd * add_d)  # 这是一个pid算法
    water_tank = delta + water_tank
    # print(sum_list)
    # print(water_tank)
    #print(add_p, sum_sum_five, add_d, delta, water_tank)
    times = times + 1
    time_list.append(times)
    num_list.append(water_tank)
    print(water_tank)


while True:
    #time.sleep(0.5)
    #pdi()
    pid()
    # time.sleep(1)
    water_blow()
    if times > 100:
        print(times)
        fig = plt.figure(num=1, figsize=(times + 1, except_number * 1.3))
        ax = fig.add_subplot(111)
        ax.plot(time_list, num_list)
        plt.show()
        plt.clf()
        print(times)
        break
    #elif times == 100:
     #   water_tank = 615
    else:
        pass

运行代码，就可以看到一个及其经典的pid运行过程中的图像

尝试改变代码里面的pp,pi,pd来自己探索pid的妙处吧！（后面有得你调参的

一些解释：pp是比例系数，直接向目标而去，过大会反复震荡。pi是积分系数，过大一样会一直震荡，pi的作用旨在使“漏水”的部分得以被补全，pd是微分控制系数，我们需要让pp/pi发癫乱摆动的时候及时压平其过高速变化的趋势。

Part2:上手吧，少年！

为了在arduino实现pid的控制，我们调用一个非常美丽的pid库

关于这个库的使用，网上有超级多现成的教程，我就不重复造轮子了

需要注意的是，积分系数在本例中是不必要的，如果你理解了pid你就会知道为什么的

第四部分：阿克曼结构和差速转向

简略版本：他妈的科协的车转弯半径过大了转不过弯一坨屎，必须要两个动力轮转速不同甚至其中一个反着转才可以转过某些弯道。哦我也是科协的，那没事了。

为了实现差速转向，可以参考第一部分的代码，里面已经完成了正反控制wwwwwwwww

那么我们把所有部分粘在一起（一坨屎

//传感器
#include "Arduino.h"
#include "PID_v1.h"
#define sensor_0 A3
#define sensor_r1 A4
#define sensor_r2 A5
#define sensor_r3 7
#define sensor_l1 A2
#define sensor_l2 A1
#define sensor_l3 A0


//电机与电机反转
#define motor_r 11
#define motor_l 6
#define motor_l_f 9
#define motor_r_f 10
#define motor_turn 40
#define motor_turn_go 50

//舵机
#define direc 3


//积分周期
#define Cycle 10

//以下为直行的参数们
#define front_motor 90

//以下为关于直角转弯神秘参数们
#define turn_front 160
#define turn_back 160//直角转弯时的电机差速
#define direc_r 0//左转时的舵机方向（我认为是0
#define direc_l 255 //直角右转的舵机方向
#define turn_time 1000//直角转向的具体时间
//以下为关于锐角转弯的神秘参数
#define sharp_turn_front 255
#define sharp_turn_back 0//锐角转弯时的电机差速
#define sharp_direc_r 0//左转时的舵机方向（我认为是0
#define sharp_direc_l 255 //锐角角右转的舵机方向
#define sharp_turn_time 500//锐角转向的具体时间
#define test 30

//不思进取，只等开源（调用现成的库香到爆
double door_ji = 0;//舵机的预期值
double fina;//误差值
double direct_control = 128;
double p_pid = 15;//28;//就是这个值可以让舵机在最左边传感器一个为黑的时候输出最左边值
double i_pid = 0;//关掉！关掉！一定要关掉！
double d_pid = 5;
PID myPID(&fina, &direct_control, &door_ji, p_pid, i_pid, d_pid, REVERSE);

//锐角转弯函数
void sharp_go_left() {
    analogWrite(motor_l_f,sharp_turn_back);
    analogWrite(motor_r,sharp_turn_front);//左电机反转，右边电机正转
    analogWrite(direc,sharp_direc_r);//舵机控制
    delay(sharp_turn_time);//执行时间
}
//锐角转弯函数
void sharp_go_right() {
    analogWrite(motor_l,sharp_turn_front);
    analogWrite(motor_r_f,sharp_turn_back);//右电机反转，左电机正转
    analogWrite(direc,sharp_direc_l);//舵机控制
    delay(sharp_turn_time);//执行时间
}



//直角转弯函数
void go_left() {
    analogWrite(motor_l_f,turn_back);
    analogWrite(motor_r,turn_front);//左电机反转，右边电机正转
    analogWrite(direc,direc_r);//舵机控制
    delay(turn_time);//执行时间
}
//直角转弯函数
void go_right() {
    analogWrite(motor_l,turn_front);
    analogWrite(motor_r_f,turn_back);//右电机反转，左电机正转
    analogWrite(direc,direc_l);//舵机控制
    delay(turn_time);//执行时间
}





//把整个数组里的数加起来   积分数组
int i_count(float values[Cycle]) {
    int res = 0;
    for (int i = 0; i < Cycle; i++) {
        res = res + values[i];
        //Serial.println(values[i]);
    }
    Serial.print("icount:");
    Serial.println(res);
    return res;
}
//计算pid算法所需偏差值
double last_value = 128;

double Value_count(int *value) {
    double res = 0;
    int count = 0;
    double return_value;
    for (int i = -3; i < 4; i++) {
        //Serial.print(i);
        //Serial.print(":");
        //Serial.println(value[i + 3]);
        res = res + i * value[i + 3];  //检测到是1，没检测到是0
        count = count + value[i + 3];
    }
    if (count == 0){
        res = last_value;
        count = 1;
    }
    else{
        last_value = res;
    }
    Serial.print("now value count(with out /):");
    Serial.println(res);
    Serial.print("count:");
    Serial.println(count);
    return_value = (res / count);
    return return_value;
}

//pid控制   参数：   传感器数组
void Pid_control(int *value) {
    static int cycle;  //周期
    static int last_cycle = Cycle - 1;
    static float values[Cycle];  //存积分值
    float directions = 0;        //舵机方向

    //记录积分值
    values[cycle] = Value_count(value);
    //pid计算
    directions = values[cycle] * p_pid + (values[cycle] - values[last_cycle]) * d_pid + i_count(values) * i_pid;
    //操作
    if (directions >= 3) {
        directions = 3;
    } else if (directions <= -3) {
        directions = -3;
    }
    analogWrite(direc,map(directions, -3, 3, 0, 255));
    Serial.print("now value count:");
    Serial.println(values[cycle]);
    Serial.print("direction:");
    Serial.println(directions);
    Serial.print("cycle:");
    Serial.println(cycle);


    //编辑循环
    last_cycle = cycle;
    cycle++;
    //重置循环
    if (cycle > (Cycle - 1)) {
        cycle = 0;
    }
}




//设置电机转速
void Run_motor(int speed_l, int speed_r) {
    if(speed_l >= 0 && speed_r >= 0){
        if(speed_l > 210){
            speed_l =210;
        }
        else if(speed_l < 45){
            speed_l = 45;
        }
        if(speed_r > 240){
            speed_r =240;
        }
        else if(speed_r < 45){
            speed_r = 45;
        }
        analogWrite(motor_l, speed_l);
        analogWrite(motor_r, speed_r);
        analogWrite(motor_l_f, 0);
        analogWrite(motor_r_f, 0);
    }
    else if(speed_l < 0 && speed_r >= 0){
        speed_l = -speed_l;
        if(speed_l > 210){
            speed_l =210;
        }
        else if(speed_l < 45){
            speed_l = 45;
        }
        if(speed_r > 240){
            speed_r =240;
        }
        else if(speed_r < 45){
            speed_r = 45;
        }
        analogWrite(motor_l_f, speed_l);
        analogWrite(motor_r, speed_r);
        analogWrite(motor_l, 0);
        analogWrite(motor_r_f, 0);
    }
    else if(speed_l >= 0 && speed_r < 0){
        speed_r = - speed_r;
        if(speed_l > 210){
            speed_l =210;
        }
        else if(speed_l < 45){
            speed_l = 45;
        }
        if(speed_r > 240){
            speed_r =240;
        }
        else if(speed_r < 45){
            speed_r = 45;
        }
        analogWrite(motor_l, speed_l);
        analogWrite(motor_r_f, speed_r);
        analogWrite(motor_l_f, 0);
        analogWrite(motor_r, 0);
    }
    else if(speed_l < 0 && speed_r < 0){
        speed_l = -speed_l;
        speed_r = - speed_r;
        if(speed_l > 210){
            speed_l =210;
        }
        else if(speed_l < 45){
            speed_l = 45;
        }
        if(speed_r > 240){
            speed_r =240;
        }
        else if(speed_r < 45){
            speed_r = 45;
        }
        analogWrite(motor_l_f, speed_l);
        analogWrite(motor_r_f, speed_r);
        analogWrite(motor_l, 0);
        analogWrite(motor_r, 0);
    }
}



void Run_direct(double pwm){
    analogWrite(direc, pwm);
}

//获取传感器的值
int *Get_value() {
    int *res;
    res = (int *)malloc(sizeof(int) * 7);
    res[0] = digitalRead(sensor_l3);
    res[1] = digitalRead(sensor_l2);
    res[2] = digitalRead(sensor_l1);
    res[3] = digitalRead(sensor_0);
    res[4] = digitalRead(sensor_r1);
    res[5] = digitalRead(sensor_r2);
    res[6] = digitalRead(sensor_r3);
    //for(int i = 0;i<6;i++)
    //{
    //  Serial.println(res[i]);
    //}
    return res;
}

void go_front(){
    //int err;
    //Serial.println("----------------------");
    //emmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm这下有得忙了
    int left_less = 0;
    int right_less = 0;
    if(direct_control >= 170){
        right_less = ((direct_control - 170) * motor_turn);
        direct_control = 210;
        Run_motor(-(motor_turn_go + right_less-test),motor_turn_go + right_less);
    }
    else if(direct_control <= 86){
        left_less =((86 - direct_control) * motor_turn);
        direct_control = 45;
        Run_motor(motor_turn_go + left_less,-(motor_turn_go + left_less-test));
    }
    else{
        // Run_motor(front_motor - right_less + left_less,front_motor -left_less + right_less);
        Run_motor(front_motor,front_motor);
    }
    Run_direct(direct_control);

}


int left_out_of_control = 0;
int right_out_of_control = 0;
void where_are_you_pid(double error){
    if(error >= 254&&right_out_of_control<=10){
        right_out_of_control++;
        left_out_of_control = 0;

    }
    else if(error <= 1 &&left_out_of_control<=10){
        left_out_of_control++;
        right_out_of_control = 0;

    }
    else if(error >= 254 && right_out_of_control >10){
        right_out_of_control = 0;
        go_right();
    }
    else if(error <= 1 && left_out_of_control >10){
        left_out_of_control = 0;
        go_left();
    }
}
void setup() {
    Serial.begin(57600);
    //传感器
    pinMode(sensor_0, INPUT);
    pinMode(sensor_r1, INPUT);
    pinMode(sensor_r2, INPUT);
    pinMode(sensor_r3, INPUT);
    pinMode(sensor_l1, INPUT);
    pinMode(sensor_l2, INPUT);
    pinMode(sensor_l3, INPUT);

    //舵机，左右电机
    pinMode(direc, OUTPUT);
    pinMode(motor_l, OUTPUT);
    pinMode(motor_r, OUTPUT);
    myPID.SetMode(AUTOMATIC);
    myPID.SetOutputLimits(45,210);
    myPID.SetSampleTime(50);
    // put your setup code here, to run once:
}
void loop() {
    int *value = NULL;
    value = Get_value();
    fina = Value_count(value);
    myPID.Compute();
    //Pid_control(value);
    Serial.print("fina:");
    Serial.println(fina);
    Serial.print("direct_control:");
    Serial.println(direct_control);
    free(value);
    go_front();
    delay(49);
    // put your main code here, to run repeatedly:
}

请不要直接使用，里面的参数对于每个车都是不同的，务必自己调参。

不想写了，玩p5r去

查看评论 - 3 条评论

Comments 3 条评论

博客作者 triority

回复

发布于 2022-12-29 06:35 ( Edge 108 Windows 10/11 )( )

佬！带我飞
- 博客作者 qwqpap
  
  回复
  
  发布于 2022-12-29 06:40 ( Edge 108 Windows 10/11 )( )
  
  @triority vivo50看看实力
博客作者上海龙教练

回复

发布于 2023-02-22 17:11 ( Chrome 96 Android 12 )( )

Test,测试です。