小车快速循迹 串级PID算法 稳得很 纯纯干货

   网上的文章翻了一遍，都没有讲智能小车如何做到快速循迹，而且又稳的，是不是代码高保密，只传亲学弟。

  有讲PID算法循迹的，要么运用到自己小车身上就不行，要么就是文章中没有效果展示，代码收费，导致不敢买。你的循迹小车是不是做不到速度快了小车还能稳稳循迹的效果，出线、左右摆的问题一直困扰你？网上人家智能车竞速的视频深深吸引着你，你在思考这小车怎么做到速度这么快，而且又稳的，此篇就帮你解决这个问题。话不多说，先看视频效果。

http://【32小车快速循迹 串级PID 稳得很】 https://www.bilibili.com/video/BV1XJ3jeWE6S/?share_source=copy_web&vd_source=95344b969e8c93113a217b133e31aef9

怎么样？稳吧，（如果还没你的好此时你应该退出这篇文章），下面正文开始。

一、基础篇

  如果你知道小车是如何循迹的可直接跳过。小车一般采用灰度、或者红外模块来判断小车在循迹线的位置，

  我用到的是第二种，可以看到总共8个灯（8路），从左到右我们依次记为灯1 - 灯8，小车在线的中间位置时灯4和灯5就会亮起，所对应的信号输出口就会返回0，如果小车往右偏一点，那么灯3和灯4所对应的信号输出口就为0，小车就可以据此来做出判断。快速循迹一般采用12路循迹模块，最少8路，我使用的是12路，因为路数越多，小车的微动变化主控也能判断。

二、进阶篇

  此篇主讲主控得到信号后怎样处理。你不会还在用if  elseif 吧，太LOW了！以STM32标准库为例，有基础的想必都知道如果要读取C1口的高低电平的函数是

GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_1);

一般做法是将它读取出来赋值给一个变量，像这样

int H1 = GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_1);

依次读取8个引脚，分别赋值给H1-H8，然后再对变量H1-H8进行if elseif 判断。每次写 if elseif 写一大堆，还不好看，今天我用另外一种处理方法来代替它，代码简洁明了。

我们知道假如第一个灯亮起，那么8个灯所返回的值依次是01111111，可以看做它一个二进制数，所对相应的就是127，此时我们用 switch语句来判断不就好了，关键在于怎样将8个数合并成一个数呢，直接上代码

int read(void)
{
 int val;
  val |= (GPIO_ReadInputDataBit(GPIOC, OUT1) << 7);
  val |= (GPIO_ReadInputDataBit(GPIOC, OUT2) << 6);
  val |= (GPIO_ReadInputDataBit(GPIOC, OUT3) << 5);
  val |= (GPIO_ReadInputDataBit(GPIOC, OUT4) << 4);
  val |= (GPIO_ReadInputDataBit(GPIOC, OUT5) << 3);
  val |= (GPIO_ReadInputDataBit(GPIOC, OUT6) << 2);
  val |= (GPIO_ReadInputDataBit(GPIOC, OUT7) << 1);
  val |= (GPIO_ReadInputDataBit(GPIOC, OUT8) << 0);
  return val;
}

这样不就全部保存在val这个变量里了嘛，下面就是用switch语句来对这个变量判断，上代码

    read();
	switch (read())
	{
		case 127:     break; //0111 1111
		case 191:     break; //1011 1111
		case 223:     break; //1101 1111
		case 239:     break; //1110 1111
		case 247:     break; //1111 0111
		case 251:     break; //1111 1011
		case 253:     break; //1111 1101
		case 254:     break; //1111 1110

		case 0:       break; //0000 0000
		case 255:     break; //1111 1111
		default:      break;
   }

  两个灯亮和三个灯亮的情况一样，这里就不给你了，自己动动小手算算一算吧。这不就完美解决了if elseif 写一堆的问题了嘛。

三、单级PID

 加12758 来了来了，它终于来了! 不过此篇还是单级PID，耐心看完，你会有收获的，还不知道什么叫做PID的小伙伴请去翻阅资料，我在此不做解释了，懂PID的小伙伴此时是不是不知道PID怎样运用在循迹小车上呢，我来给你解惑。PID最重要的是得有误差反馈，误差从何而来，就是你给它，什么意思呢，直接看代码

 float error;
 read();
	switch (read())
	{
		case 127:  error = -3;     break; //0111 1111
		case 191:  error = -1;     break; //1011 1111
		case 223:  error = -0.5;   break; //1101 1111
		case 239:  error = 0;      break; //1110 1111
		case 247:  error = 0;      break; //1111 0111
		case 251:  error = 0.5;    break; //1111 1011
		case 253:  error = 1;      break; //1111 1101
		case 254:  error = 3;      break; //1111 1110

		case 0:    error = 0;      break; //0000 0000
		case 255:  error = 0;      break; //1111 1111
		default:   error = 0;      break;
   }

这样误差不就有了嘛，小车往右偏，反馈误差为负，偏的越很，误差越大，小车在中间误差为0，

误差有了，那么下面就是对误差的PID算法处理，我们知道PID算法常见的有位置PID和增量式PID，很明显咱们要用位置PID，此处我只用P和D，上代码

    P = error;                      //P项
	D = error - previous_error;     //D项（当前误差减上次误差）
	PID_value = (Kp * P)+ (Kd * D); //核心公式
	previous_error = error;         //将当前误差保存
    
    X = Pwm +PID_value;              //最终计算出左右轮要输出的PWM     
	Y = Pwm -PID_value;   
		
    if(X>=100) { X=100;}else if(X<-100){X=-100;}  //限幅
    if(Y>=100) { Y=100;}else if(Y<-100){Y=-100;}
	
    motor(X,Y);    //电机执行

previous_error，PID_value,X,Y,Pwm均已定义，motor函数为电机执行函数，如果motor函数传入的是两个负数，电机后转，一边正一边负就会左转或右转。限幅100是因为我给你PWM满量程为100；Pwm这个变量是你想让小车达到速度，想让小车以50的速度循迹就给50，想让小车以100的速度循迹就给100。在调参时先调KP，再调KD，具体调参过程不再细讲，请搜索相关文章，以我的经验来说KD要比KP大的多。

至此，单级PID循迹已经完成，但是!效果并不是太好，面对低速还可以，但是面对高速状态下仍然是左右震荡、出线！下面的串级PID完美解决了该问题。

四、串级PID

 52972所谓串级PID，用大白话来说就是两个PID串一块。具体原理请搜索相关文章，在此只说另外一个PID是什么，另外一个PID是加上了陀螺仪，我用的是最便宜的mpu6050陀螺仪，十几块钱，对它哪个值进行PID运算呢？Z轴角速度值！关于陀螺仪角速度、姿态角之类的不懂的请搜索相关文章，怎样串，先谁后谁，具体代码是什么，请有偿联系，创作不易，谢谢。（代码基于STM32F103ZET6，标准库）