PID算法

1.Arduino 智能小车寻迹原理寻迹采用的主要原理就是红外探测法,即利用红外线在不同颜色的物体表面具有不同的反射性质的特点Arduino 单片机就是以是否收到反射回来的红外光为依据来确定黑线的位置和小车的行走路线。

红外探测器探测距离有限，一般最大不应超过15cm。

对于发射和接收红外线的红外探头，可以自己制作或直接采用集成式红外探头2.Arduino 寻迹模块简介3.Arduino pwm 调速4.PID算法5.PID 算法与寻迹6.代码：.#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 = 7; //..const int IN_A2 = 6; //..const int IN_B1 = 5; //..const int IN_B2 = 4; //..const int _pwmLeftPin = 10;//左边 pwm 引脚..const int _pwmRightPin = 11;//右边 pwm 引脚..pid_t pid;..float pidValue = 0; //pid 值..bool turnFlag = false;....void setup(void)..{..int i;....//设置引脚功能..pinMode(IN_A1, OUTPUT); ..pinMode(IN_A2, OUTPUT); ..pinMode(IN_B1, OUTPUT); ..pinMode(IN_B2, OUTPUT); ....//设置寻迹板引脚为 INPUT..for (i = 0; i < 5; i++) { .pinMode(IR_PIN[i], INPUT);..}......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);//延时 5s....analogWrite(_pwmLeftPin, motorSpeed );.analogWrite(_pwmRightPin, motorSpeed ); ....goForward();//小车向前行驶....return;..}........../**..获取寻迹板红外数据..*/..uint8_t getIrData(void)..{..int i, j;..uint8_t level;..uint8_t temp;..uint8_t irs[9] = {0};....//获取10组数据..for (j = 0; j < 9; j ++) {....for (i = 0; i < 5; i++) {..level = digitalRead(IR_PIN[i]); ..if (level) {..bitSet(irs[j], i);//设置对应位为1 ..} else {..bitClear(irs[j], i);//设置对应为0 .}..}..}....//对所有的数据进行排序..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];..}..../**..计算误差值..@param irs :获取的寻迹传感器的值..*/..int calcErrorByIrsValue(uint8_t irs) ..{..int curError = pid.error;....switch (irs) {..case B11110: curError = -8; break; ....case B10000:..case B11000: curError = -7; break; ....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;..case B11111: curError = pid.error > 0 ? 9 : - 9; break; ..}....return curError;..}...../**..排序函数..*/..void _sortData(int *p, int n)..{..int temp;..int i, j;....for (i = 0; i < n - 1; i ++) {..for (j = 0; j < n - i - 1; j ++) { ..if (p[j] > p[j + 1]) {..temp = p[j];..p[j] = p[j + 1];..p[j + 1] = temp;..}..}..}....return;..}....../**..计算误差值..*/..void calcCurrentError(void) ..{..int i;..uint8_t irs;.float sum = 0;..int errorData[10];....//获取 10组数据..for (i = 0; i < 10; i ++) {..irs = getIrData();..errorData[i] = calcErrorByIrsValue(irs); ..}...._sortData(errorData, 10);....for (i = 1; i < 10 - 1; i ++) {..sum += errorData[i];..}...pid.error = sum / 8;....return;..}....void turnRight(void)..{..digitalWrite(IN_A1, LOW); ..digitalWrite(IN_A2, HIGH); ..digitalWrite(IN_B1, HIGH); ..digitalWrite(IN_B2, LOW); ..}....void turnLeft(void)..{..digitalWrite(IN_A1, HIGH); ..digitalWrite(IN_A2, LOW); ..digitalWrite(IN_B1, LOW); ..digitalWrite(IN_B2, HIGH); ..}....void goForward(void)..{..digitalWrite(IN_A1, HIGH); ..digitalWrite(IN_A2, LOW); ..digitalWrite(IN_B1, HIGH); ..digitalWrite(IN_B2, LOW); ..}.../**..小车控制函数..@param pidValue : 计算出来的 pid 值..@param turnFlag : 方向标志..*/..void motorControl(float pidValue, bool turnFlag)..{....int leftMotorSpeed = 0;..int rightMotorSpeed = 0;....//根据 pid 的值调整小车左右电机的速度..leftMotorSpeed = constrain((motorSpeed + pidValue), -255, 255); ..rightMotorSpeed = constrain((motorSpeed - pidValue), -255, 255); ...//当转弯标志被设置时,则需要使用左边与右边的轮子正转与反转来调整,提高调整速度..if (turnFlag) {..//按照较大的 pwm 值进行调整,速度最快,左边速度与右边速度一致..if (abs(leftMotorSpeed) > abs(rightMotorSpeed)) {..leftMotorSpeed = abs(leftMotorSpeed);..rightMotorSpeed = leftMotorSpeed;..} else {..rightMotorSpeed = abs(rightMotorSpeed);..leftMotorSpeed = rightMotorSpeed;..}..} else {..//当速度为正时,则取原值,当速度为负时,则取相反数,保持 pwm 的值为正值..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;..}..../***..计算 pid 的值.*/..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; ....//没有到达采样时间..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; ..*pValue = constrain(*pValue, -motorSpeed,motorSpeed);....pid.previous_error = pid.error;..lastTime = now;....return true;..}....#if ARDUINO_DEBUG..void print_debug()..{..int i;..String irs2bin = String(debugIrs, 2); ..int len = irs2bin.length();..if (len < 5) {..for (i = 0; i < 5 - len; i++) {..irs2bin = "0" + irs2bin;..}..}....Serial.print("IRS : ");..Serial.print(irs2bin);..Serial.print(" ML:");..Serial.print(debugLeftSpeed);..Serial.print(" MR:");..Serial.print(debugRightSpeed);..Serial.print(" ERROR:");..Serial.print(pid.error, OCT);..Serial.println();..}..#endif..../**..计算运动方向..*/..void calcDirection(void)..{....if (pid.error >= 7 && pid.error <= 9) {..turnLeft();..turnFlag = true;..} else if (pid.error >= -9 && pid.error <= -7) { ..turnRight();..turnFlag = true;..} else {..goForward();..turnFlag = false;..}....return;..}....void loop(void)..{..bool ok;..float pidValue;....//计算错误值..calcCurrentError();..//计算 pid 的值..ok = calculatePid(&pidValue);..if (ok) {..calcDirection();//计算小车运动方向..motorControl(pidValue, turnFlag);//控制电机..}....//delay(500);....#if ARDUINO_DEBUG ..print_debug(); ..delay(1000); ..#endif....return;..}.。