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; }