znzcqy的博客

一、实现目标
本项目使用五路循迹模块，能实现闭环赛道的简单循迹。（如下图）

五路循迹模块含有5个传感器，用于判断路况。赛道使用黑色胶带搭建，背景使用白色或者反射性强的，以便传感器能准确检测。

二、使用配件
Arduino uno R3开发板

L298N电机驱动模块

TCRT5000五路循迹模块

小车模块

杜邦线若干、六角铜柱、螺丝螺母若干
以上配件均可在某宝购买。
三、小车的安装与调试

（本图仅供参考）
L298N电机驱动模块的连接：out1,out2,out3,out4分别连接两个马达的两端，注意正反，IN1,IN2,IN3,IN4分别连接开发板上的引脚2，4，7，8。左右两端的使能端需要拔掉键帽，再用杜邦线连接，ENA，ENB分别连接开发板上的引脚3和5。12v电源端接电源正极，5v电源端接开发板上的5v，用于给开发板供电，GND接地。
TCRT5000五路循迹模块的连接：5v电源端接开发板上的5v，GND接地。out1，2，3，4，5分别接引脚9，10，11，12，13。
连接完成

以下是测试代码（使用Arduino IDE进行编译，烧录），可以控制小车前进，左转，右转，后退，停车。

const int IN_A1 = 2;
const int PWM_A = 3;
const int IN_A2 = 4;
const int PWM_B = 5;
const int IN_B1 = 7;
const int IN_B2 = 8;
void setup() {
  pinMode(IN_A1, OUTPUT);
  pinMode(IN_A2, OUTPUT);
  pinMode(IN_B1, OUTPUT);
  pinMode(IN_B2, OUTPUT);
}
void moveLeft(void)
{
  digitalWrite(IN_A1, LOW);
  digitalWrite(IN_B1, HIGH);
  digitalWrite(IN_A2, LOW);
  digitalWrite(IN_B2, LOW);
  analogWrite(PWM_A, 255);
  analogWrite(PWM_B, 255);
}
void moveRight(void)
{
  digitalWrite(IN_A1, HIGH);
  digitalWrite(IN_B1, LOW);
  digitalWrite(IN_A2, LOW);
  digitalWrite(IN_B2, LOW);
  analogWrite(PWM_A, 255);
  analogWrite(PWM_B, 255);
}
void moveForward(void)
{
  digitalWrite(IN_A1, HIGH);
  digitalWrite(IN_B1, HIGH);
  digitalWrite(IN_A2, LOW);
  digitalWrite(IN_B2, LOW);
  analogWrite(PWM_A, 255);
  analogWrite(PWM_B, 255);
}
void moveBackward(void)
{
  digitalWrite(IN_A1, LOW);
  digitalWrite(IN_B1, LOW);
  digitalWrite(IN_A2, HIGH);
  digitalWrite(IN_B2, HIGH);
  analogWrite(PWM_A, 255);
  analogWrite(PWM_B, 255);
}
void stopCar(void)
{
  digitalWrite(IN_A1, LOW);
  digitalWrite(IN_B1, LOW);
  digitalWrite(IN_A2, LOW);
  digitalWrite(IN_B2, LOW);
  analogWrite(PWM_A, 255);
  analogWrite(PWM_B, 255);
}
void loop() {
  moveForward();
  delay(3000);
  moveLeft();
  delay(3000);
  moveRight();
  delay(3000);
  moveBackward();
  delay(3000);
  stopCar();
  delay(3000);
}

测试电机没问题后，还要对循迹模块进行测试，用六角铜柱连接以保证离地间距，在比较光洁的地面上，5个灯应该是全亮的状态。然后用手或者黑色胶带放到每个模块下，观察灯是否熄灭，逐一测试，没问题后可以进行下一步。（注意：灯亮代表1，灯灭代表0）
四、循迹
搭建好赛道，就可以放上小车进行循迹了。下面是代码：

const int IN_A1 = 2;
const int PWM_A = 3;
const int IN_A2 = 4;
const int PWM_B = 5;
const int IN_B1 = 7;
const int IN_B2 = 8;
const int sensor1 = 9;
const int sensor2 = 10;
const int sensor3 = 11;
const int sensor4 = 12;
const int sensor5 = 13;
int sensor[5] = {0, 0, 0, 0, 0};
float Kp = 30, Ki = 10, Kd = 1;
float error = 0, P = 0, I = 0, D = 0, PID_value = 0;
float previous_error = 0, previous_I = 0;
int DIN_A1 = 100;
int DIN_B1 = 100;
void setup() {
  Serial.begin(9600);
  pinMode(IN_A1, OUTPUT);
  pinMode(IN_A2, OUTPUT);
  pinMode(IN_B1, OUTPUT);
  pinMode(IN_B2, OUTPUT);
  pinMode(sensor1, INPUT);
  pinMode(sensor2, INPUT);
  pinMode(sensor3, INPUT);
  pinMode(sensor4, INPUT);
  pinMode(sensor5, INPUT);
}
void loop() {
  sensor[0] = digitalRead(sensor1);
  sensor[1] = digitalRead(sensor2);
  sensor[2] = digitalRead(sensor3);
  sensor[3] = digitalRead(sensor4);
  sensor[4] = digitalRead(sensor5);
  if (sensor[0] == 0 && sensor[1] != 0 && sensor[2] != 0 && sensor[3] != 0 && sensor[4] != 0) {
    error = -2;
  } else if (sensor[0] == 0 && sensor[1] == 0 && sensor[2] != 0 && sensor[3] != 0 && sensor[4] != 0) {
    error = -2;
  } else if (sensor[1] == 0 && sensor[0] != 0 && sensor[2] != 0 && sensor[3] != 0 && sensor[4] != 0) {
    error = -1;
  } else if (sensor[1] == 0 && sensor[0] != 0 && sensor[2] == 0 && sensor[3] != 0 && sensor[4] != 0) {
    error = -1;
  } else if (sensor[2] == 0 && sensor[1] != 0 && sensor[0] != 0 && sensor[3] != 0 && sensor[4] != 0) {
    error = 0;
  } else if (sensor[2] == 0 && sensor[1] == 0 && sensor[0] != 0 && sensor[3] == 0 && sensor[4] != 0) {
    error = 0;
  } else if (sensor[3] == 0 && sensor[1] != 0 && sensor[2] != 0 && sensor[0] != 0 && sensor[4] != 0) {
    error = 1;
  } else if (sensor[3] == 0 && sensor[1] != 0 && sensor[2] == 0 && sensor[0] != 0 && sensor[4] != 0) {
    error = 1;
  } else if (sensor[4] == 0 && sensor[1] != 0 && sensor[2] != 0 && sensor[3] == 0 && sensor[0] != 0) {
    error = 2;
  } else if (sensor[4] == 0 && sensor[1] != 0 && sensor[2] != 0 && sensor[3] != 0 && sensor[0] != 0) {
    error = 2;
  } else {
    error = 0;
  }
  P = error;
  I = I + previous_I;
  D = error - previous_error;
  PID_value = (Kp * P) + (Ki * I) + (Kd * D);
  previous_I = I;
  previous_error = error;
  int left_motor_speed = DIN_A1 - PID_value;
  int right_motor_speed = DIN_B1 + PID_value;
  constrain(left_motor_speed, 0, 255);
  constrain(right_motor_speed, 0, 255);
  digitalWrite(IN_A2, HIGH);
  digitalWrite(IN_B2, HIGH);
  digitalWrite(IN_A1, LOW);
  digitalWrite(IN_B1, LOW);
  analogWrite(PWM_A, DIN_A1 - PID_value);
  analogWrite(PWM_B, DIN_B1 + PID_value);
}

烧入Arduino开发板，大功告成。