本系列教程讲解零知小车套件以零知迷你板为主控，使用零知开源平台进行智能小车开发。
使用的硬件：零知智能小车套件
实现的功能：红外循迹、超声避障、直线运行等基本功能。
扩展：可以在小车上加入其它传感器模块进行扩展，比如加入摄像头模块进行监控、加入火焰传感器做灭火小车、加入更多超声波模块实现自动跟随...
零知两轮小车入门版(零知mini主控)系列教程-小车组装
零知两轮小车入门版(零知mini主控)系列教程-程序控制电机开跑
零知两轮小车入门版(零知mini主控)系列教程-红外循迹走S弯
零知两轮小车入门版(零知mini主控)系列教程-超声波自动避障
零知两轮小车入门版(零知mini主控)系列教程-使用PID走直线
在前面的几节中我们的小车在直行时会发现总是走着走着就歪了，我们虽然设置轮子速度是一样的，但是由于摩擦阻力及制造工艺等差异使得小车两边的轮子的速度无法完全一致的，如果不做处理，那么小车将无法走出笔直的直线的，要想走出笔直的直线，就需要对两边的轮子进行测速，并对其PWM输出进行微调，让两边轮子的速度之差尽量减小，这里就是用经典的做法PID控制来使小车走出直线，为省去我们自整定PID参数的麻烦，我们使用自整定的方法来整定PID参数。主程序如下所示：
/**********************************************************
* 文件: auto_PID.ino by 零知实验室([url]www.lingzhilab.com[/url])
* -^^- 零知开源，让电子制作变得更简单！ -^^-
* 时间: 2019/12/07 14:58
* 说明:
************************************************************/
#include "PID_v1.h"
#define AUTO_PID 1 //PID自整定开关
//电机模块
#define MotorIN1 2
#define MotorIN2 3
#define MotorIN3 4
#define MotorIN4 5
#define MotorIN5 8
#define MotorIN6 9
#define MotorIN7 10
#define MotorIN8 11
//测速模块
#define SM1Pin 29
#define SM2Pin 28
int leftCounter = 0, rightCounter = 0;
unsigned long time1 = 0, old_time = 0; // 时间标记
unsigned long timechange = 0;
int sampletime = 200; // 采样标记
double speedLeft, speedRight;
bool noteFlag = 0;
bool controlType = 0; // 0为PI控制，1为PID控制
double Kp = 0.5,Ki = 0.01,Kd = 0;
double leftIn, leftOut, Setpoint = 80;
double rightIn, rightOut;
PID leftPID(&leftIn, &leftOut, &Setpoint, Kp, Ki, Kd, DIRECT);
PID rightPID(&rightIn, &rightOut, &Setpoint, Kp, Ki, Kd, DIRECT);
#if AUTO_PID
class AutoPid{
double autoKp, autoKi, autoKd;
double outputHigh, outputLow, outputStep = 20;
double inputHighAll, inputHighNum, inputLowAll, inputLowNum;
double inputHistory[20];
uint8_t atemp, btemp;
public:
bool caculateFlag = 1;
void autoPid(PID pid, double input,
double output, double setpoint, bool noteFlag, bool isleft);
};
AutoPid leftauto;
AutoPid rightauto;
#endif
// 复位或上电后运行一次:
void setup() {
//在这里加入初始化相关代码，只运行一次:
Serial.begin(9600);
leftPID.SetMode(AUTOMATIC);
rightPID.SetMode(AUTOMATIC);
leftPID.SetOutputLimits(0,255);
rightPID.SetOutputLimits(0,255);
//设置引脚
pinMode(MotorIN1,OUTPUT);
pinMode(MotorIN2,OUTPUT);
pinMode(MotorIN3,OUTPUT);
pinMode(MotorIN4,OUTPUT);
pinMode(SM1Pin,INPUT);
pinMode(SM2Pin,INPUT);
attachInterrupt(SM1Pin, RightCount_CallBack,FALLING); // 设置外部中断SEP1
attachInterrupt(SM2Pin, LeftCount_CallBack,FALLING); // 设置外部中断SEP2
}
//一直循环执行:
void loop() {
// 在这里加入主要程序代码，重复执行:
noteFlag = speedDetection(&speedLeft, &speedRight, sampletime); //测速
leftIn = speedLeft;
rightIn = speedRight;
#if AUTO_PID
leftauto.autoPid(leftPID, leftIn,
leftOut, Setpoint, noteFlag, true);
rightauto.autoPid(rightPID, rightIn,
rightOut, Setpoint, noteFlag, false);
if(leftauto.caculateFlag == 0 && rightauto.caculateFlag == 0){
#endif
leftPID.Compute();
rightPID.Compute();
motorRun(1, true, speedLeft);
motorRun(2, true, speedRight);
#if AUTO_PID
}
#endif
}
void motorRun(byte motorNum, bool forward, int speed){
if(motorNum == 1){
if(forward){
analogWrite(MotorIN1,140);
analogWrite(MotorIN2,speed);
}else{
analogWrite(MotorIN1,speed);
analogWrite(MotorIN2,140);
}
}
if(motorNum == 2){
if(forward){
analogWrite(MotorIN3,speed);
analogWrite(MotorIN4,140);
}else{
analogWrite(MotorIN3,140);
analogWrite(MotorIN4,speed);
}
}
}
#if AUTO_PID
void AutoPid::autoPid(PID pid, double input,
double output, double setpoint, bool noteFlag, bool isleft)
{
if (millis() < 5000)
{
pid.Compute();
if (isleft){
motorRun(1, true, output);
}
else{
motorRun(2, true, output);
}
}
else if (millis() < (20 * sampletime + 6000))
{
outputLow = output - outputStep;
outputHigh = output + outputStep;
//如果当前速度小于设定速度，输出高的PWM，产生波峰
if (input < setpoint)
{
output = outputHigh;
if (output > 255)
output = 255;
}
else if (input > setpoint)
{
output = outputLow;
if (output < 0)
output = 0;
}
if (isleft){
motorRun(1, true, output);
}
else{
motorRun(2, true, output);
}
if (noteFlag == 1)
{
for (int i = 1; i < 20; i++)
{
inputHistory[20 - i] = inputHistory[20 - i - 1];
}
inputHistory[0] = input;
noteFlag = 0;
}
}
else
{
while (caculateFlag)
{
// 开始分析波形
for (int i = 1; i < 20; i++)
{
// 波峰
if (inputHistory[i] > inputHistory[i - 1]
&& inputHistory[i] > inputHistory[i + 1])
{
inputHighAll += inputHistory[i]; // 波峰数据累加
inputHighNum++; // 波峰个数加1
if (inputHighNum == 1)
atemp = i; // 记录第一个波峰的位置
btemp = i; // 记录最后一个波峰的位置
}
// 波谷
else if (inputHistory[i] < inputHistory[i - 1]
&& inputHistory[i] < inputHistory[i + 1])
{
inputLowAll += inputHistory[i];
inputLowNum++;
}
}
float autoTuneA = (inputHighAll / inputHighNum) -
(inputLowAll / inputLowNum); // 波峰与波谷之差的平均值
float autoTunePu = (btemp - atemp) *
(sampletime / 1000) / (inputHighNum - 1); // 两个波峰之间的时间间隔
// 根据公式计算PID
double Ku = 4 * outputStep / (autoTuneA * 3.14159);
autoKp = controlType == 1 ? 0.6 * Ku : 0.4 * Ku;
autoKi = controlType == 1 ? 1.2 * Ku / autoTunePu : 0.48 * Ku / autoTunePu;
autoKd = controlType == 1 ? 0.075 * Ku * autoTunePu : 0;
caculateFlag = 0; // 关闭电机自整定
// 串口打印出自整定计算得到的PID参数
Serial.println("******************************");
Serial.println("自整定的PID参数为： ");
Serial.print("Kp：");
Serial.println(autoKp);
Serial.print("Ki：");
Serial.println(autoKi);
Serial.print("Kd：");
Serial.println(autoKd);
Serial.println(" ");
Serial.print("震荡过程中的峰峰值 A：");
Serial.print(autoTuneA);
Serial.println("rpm/min");
Serial.print("两个波峰之间的时间间隔Pu：");
Serial.print(autoTunePu);
Serial.println("秒");
Serial.println("******************************");
Serial.println(" ");
pid.SetTunings(autoKp, autoKi, autoKd); // 导入新的PID参数
delay(2000);
}
}
}
#endif
//测速
bool speedDetection(double *left, double *right, int sampleTime)
{
time1 = millis(); // 以毫秒为单位，获取当前时间
timechange = time1 - old_time;
if(timechange >= sampleTime)
{
detachInterrupt(SM1Pin);
detachInterrupt(SM2Pin);
// 计算当前速度
*left = (float)leftCounter * 60000 / (20.0 * timechange);
*right = (float)rightCounter * 60000 / (20.0 * timechange);
// //记录波形数据
// if(noteFlag = 1){
// for (int i = 1; i < 20; i++) {
// inputHistory[20 - i] = inputHistory[20 - i - 1];
// }
// inputHistory[0] = speed;
// }
//把脉冲计数值清零，以便计算下一次的脉冲计数
leftCounter = 0;
rightCounter = 0;
old_time = millis(); // 记录测速时的时间节点
attachInterrupt(SM1Pin, RightCount_CallBack,FALLING);
attachInterrupt(SM2Pin, LeftCount_CallBack,FALLING);
return true;
}
else
return false;
}
void RightCount_CallBack()
{
rightCounter++;
}
void LeftCount_CallBack()
{
leftCounter++;
}