Arduino智能小车程序+资料 pid算法 -

#include "pid.h"
#ifdef ARDUINO_DEBUG
int debugLeftSpeed;
int debugRightSpeed;
uint8_t debugIrs = 0;
#endif
const float motorSpeed = 140; //
const int IR_PIN[] = {A0, A1, A2, A3, A4}; //
const int IN_A1 = 13; //
const int IN_A2 = 12; //
const int IN_B1 = 11; //
const int IN_B2 = 10; //
const int _pwmLeftPin = 5;
const int _pwmRightPin = 6;
pid_t pid;
float pidValue = 0;
bool turnFlag = false;
void setup(void)
{
int i;
/********************1.初始化电机模块引脚13,12,11,10为输出模式******************/
/*
* 编写代码
*/
/********************2.设置小车启动速度为140---set_speed()******************/
/*
* 编写代码
*/
/********************3.初始化循迹模块引脚A0, A1, A2, A3, A4为输入模式******************/
/*
* 编写代码
*/
//3.设置PID参数
pid.sampleTime = SAMPLE_TIME;
pid.Kp = KP_VALUE;
pid.Ki = KI_VALUE;
pid.Kd = KD_VALUE;
pid.error = 0;
pid.previous_error = 0;
Serial.begin(115200);
delay(5000);
goForward();
return;
}
//get ir data and middle filter
uint8_t getIrData(void) //
{
int i, j;
uint8_t level;
uint8_t temp;
uint8_t irs[9] = {0};//采集9次传感器的传感器的数据
//减少取平均值，滤波。
for (j = 0; j < 9; j ++) {
for (i = 0; i < 5; i++) {
level = digitalRead(IR_PIN[i]);
if (level) {
bitSet(irs[j], i); // 设置irs[j]的第jbit 为1
} else {
bitClear(irs[j], i); //清除irs[j]的第jbit为0
}
}
}
//对irs中的数据，进行9次排序，取最中间的值。
for (i = 0; i < 9 - 1; i ++) {
for (j = 0; j < 9 - i - 1; j ++) {
if (irs[j] > irs[j + 1]) {
temp = irs[j];
irs[j] = irs[j + 1];
irs[j + 1] = temp;
}
}
}
#ifdef ARDUINO_DEBUG
debugIrs = irs[4];
#endif
return irs[4];
}
int calcErrorByIrsValue(uint8_t irs)
{
//右边压线为负，左边压线为正。
//要左转为负，要右转为正
int curError = pid.error; //获得上一次的pid误差值
//压线输出低电平0，没有压线输出高电平1
switch (irs) { //h'h
case B11110: curError = -8; break; //最右边压线
case B10000:
case B11000: curError = -7; break; //右四个或者右3个沿线
case B11100: curError = -6; break; //右两个沿线
case B11101: curError = -4; break; //右倒数第二个沿线
case B11001: curError = -2; break; //
case B00000: //全部压线和中间压线，对应"十字"弯道，直接郭即可。
case B11011: curError = 0; break;
case B10011: curError = 2; break;
case B10111: curError = 4; break;
case B00111: curError = 6; break;
case B00011:
case B00001: curError = 7; break;
case B01111: curError = 8; break;
//都没有沿线，按照上次的PID值来计算
case B11111: curError = pid.error > 0 ? 9 : - 9; break;
}
return curError;
}
void _sortData(int *p, int n)
{
int temp;
int i, j;
/**编写冒泡排序，对采集到的错误码，进行十次从小到大排序 **/
/*
* 编写代码
*/
return;
}
//计算当前的误差值，得到当前的偏移值
void calcCurrentError(void)
{
int i;
uint8_t irs;
float sum = 0;
int errorData[10];
//1.采集10次循迹模块获得的错误码
for (i = 0; i < 10; i ++) {
irs = getIrData(); //得到这次传感器采集的数据（5路数据）
errorData[i] = calcErrorByIrsValue(irs);
}
//2.要求对errorData中的数据进行排序（从小到大排序）
/*
* 编写代码
*/
//3.十次中去处一个最高数，去除一个最低数，求中间8次错误码数据的和。
/*
* 编写代码
*/
//4.求8次错误码数据中的偏差。
pid.error = sum / 8;
return;
}
void turnRight(void)
{
digitalWrite(IN_B1, HIGH); //左正
digitalWrite(IN_B2, LOW);
digitalWrite(IN_A1, LOW); //右反
digitalWrite(IN_A2, HIGH);
}
void turnLeft(void)
{
digitalWrite(IN_B1, LOW);// 左反
digitalWrite(IN_B2, HIGH);
digitalWrite(IN_A1, HIGH); //右正
digitalWrite(IN_A2, LOW);
}
void goForward(void)
{
digitalWrite(IN_B1, HIGH); //左正传
digitalWrite(IN_B2, LOW);
digitalWrite(IN_A1, HIGH); //右正转
digitalWrite(IN_A2, LOW);
}
void motorControl(float pidValue, bool turnFlag)
{
int leftMotorSpeed = 0;
int rightMotorSpeed = 0;
leftMotorSpeed = constrain((motorSpeed + pidValue), -255, 255);
rightMotorSpeed = constrain((motorSpeed - pidValue), -255, 255);
if (turnFlag) {
if (abs(leftMotorSpeed) > abs(rightMotorSpeed)) {
leftMotorSpeed = abs(leftMotorSpeed);
rightMotorSpeed = leftMotorSpeed;
} else {
rightMotorSpeed = abs(rightMotorSpeed);
leftMotorSpeed = rightMotorSpeed;
}
} else {
leftMotorSpeed = leftMotorSpeed > 0 ? leftMotorSpeed : -leftMotorSpeed;
rightMotorSpeed = rightMotorSpeed > 0 ? rightMotorSpeed : -rightMotorSpeed;
}
analogWrite(_pwmLeftPin, leftMotorSpeed );
analogWrite(_pwmRightPin, rightMotorSpeed);
#ifdef ARDUINO_DEBUG
debugLeftSpeed = leftMotorSpeed ;
debugRightSpeed = rightMotorSpeed;
#endif
return;
}
bool calculatePid(float *pValue)
{
float P = 0;
static float I = 0 ;
float D = 0 ;
static unsigned long lastTime = 0;
//获得机器从启动到现在所运行的时间
unsigned long now = millis();
int timeChange = now - lastTime;//得到这次的时间
//若是我们的这次变化的时间，小于pid的
//采样时间，就需要等待一下。
if (timeChange < pid.sampleTime) {
return false;
}
P = pid.error; //本次小车传感器的偏移大小
I = I + pid.error;//历史的偏移
D = pid.error - pid.previous_error; //最近两次的差值
*pValue = (pid.Kp * P) + (pid.Ki * I) + (pid.Kd * D) + 1;
//constrain()函数的功能：若是*pvalue > motorSpeed,取 motorSpeed，
// 若是*pvalue < -motorSpeed,取 -motorSpeed,
// 若是-pValue<=*pvalue <= motorSpeed,取*pvalue
*pValue = constrain(*pValue, -motorSpeed,motorSpeed);
pid.previous_error = pid.error; //记录本次的pid.error，作为下一次的历史差值。
lastTime = now; // 记录本次的运行时间，作为下一次时间的历史时间参考。
return true;
}
void calcDirection(void) //判断小车的转向
{
//pid.error > 0 说明小车左边压线多，要
if (pid.error >= 7 && pid.error <= 8) {
turnRight();
turnFlag = true;
} else if (pid.error >= -8 && pid.error <= -7) {
turnLeft();
turnFlag = true;
} else if(pid.error == 9 || pid.error == -9){
turnRight();
turnFlag = true;
}else {
goForward();
turnFlag = false;
}
return;
}
void loop(void)
{
bool ok;
float pidValue;
calcCurrentError();
ok = calculatePid(&pidValue); //计算当前pid的差值。，若是就绪了
if (ok) {
calcDirection();
motorControl(pidValue, turnFlag);
}
return;
}

复制代码