Arduino 智能小车项目 - 寻迹（20180512实训课--华清远见）

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