基於Arduino紅外控制循跡小車

硬件：Arduino板子；電機驅動模塊LM298（這個型號無所謂明白端口含義就行）；紅外傳感模塊（三個接口的，一個發射管一個接收管）幾根杜邦線，四個普通的小的直流電機 首先明白這些硬件的端口情況

 

對於arduino上面的不同端口還有什麼串口通信，此處程序利用高低電平控制不需要考慮特殊用途。

我在做的時候起初一下想到的PID控制直接就套用的寫了下但是效果很糟糕，在很低的速度下可能能夠完成，原因我沒有確定應該是在時間上，最後還是選擇的單純的高低電平控制，兩者的差別在於

PID控制程序只需要分爲判斷部分和PID算法部分，結構簡單清晰，適合範圍廣

而使用高低電平邏輯控制則需要對傳感器紅外判斷部分進行充分的劃分對每種情況都需要直接做出判斷而不存在計算，適合知道賽道情況的簡單比賽。

//先這裏就確定其他接口與MCU端口的對應關係！D爲紅外 IN 爲驅動 EN使能
#define D1 A0   
#define D2 A1
#define D3 A2
#define D4 A3
#define IN1 2
#define IN2 4
#define IN3 5
#define IN4 7
#define ENA 3
#define ENB 6

#define LED_ON  digitalWrite(12, HIGH)
#define LED_OFF  digitalWrite(12, LOW)

#define SPEED_LINE 75           //直線速度
#define SPEED_ADJUST_LOW 0      //調整速度,低速一側
#define SPEED_ADJUST_HIGH 80    //調整速度,高速一側
#define SPEED_TURN_LOW -80      //轉彎速度,低速一側
#define SPEED_TURN_HIGH 100     //轉彎速度,高速一側
#define SPEED_CYCLE 85         //直角彎轉圈速度
#define SPEED_TRIG 200          //制動速度

#define TIME_TRIG 35            //制動時間,單位ms
#define TIME_STOP 800           //靜止時間,單位ms
#define TIME_ADJUST 1           //小彎調整時間,單位ms
#define TIME_TURN_MAX 2000      //轉彎最大時間,單位ms
#define TIME_CYCLE_MAX 3500     //旋轉最大時間,單位ms
#define TIME_LEAVE_MAX 1000     //離開黑線的最大時間,單位ms
#define TIME_DIRECT_ZERO 500    //判斷V型彎方向最大時間差,單位ms

//速度誤差值,以左輪爲速度基準,小車跑直線時右輪速度誤差,跑直線時實際設置的速度，offsset=L-R
//12個數組分別爲-240,-200,-160,-120,-80,-40,40,80,120,160,200,240時的誤差
static int motor_offset[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

int t = 0; //延時計時時間
int speed_l = 0, speed_r = 0; //當前小車左右輪的設置速度
int temp_read = 0;//傳感器狀態臨時變量
int direct;//V型彎方向,左爲1，右
long int time_direct = 0;//上一次最左面或最右面傳感器檢測到黑線時間,用於計算V型彎時間差

int Read(void);
void Move(int L, int R);
void leave_line(void);
void turnv_l(void);
void turnv_r(void);

void setup() {
  // put your setup code here, to run once:
  //串口初始化
  Serial.begin(9600);
  Serial.println("start");

  //管腳初始化
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);

  //指示LED燈,可忽略
  LED_OFF;

  //當小車放在地面上時開始循跡
  while (Read() != 0);

}

void loop() {
  // put your main code here, to run repeatedly:
  switch (Read()) {

    //直行
    case 0:
      if (millis() - time_direct > TIME_DIRECT_ZERO)
        direct = 0;

      Move(SPEED_LINE, SPEED_LINE);
      break;

    //左轉直角
    case 1110:
    case 1100:
      leave_line();

      //讓小車靜止下來,保證小車旋轉時不受之前的輪子速度影響
      Move(0, 0);
      delay(TIME_STOP);

      //原地旋轉
      Move(-SPEED_CYCLE, SPEED_CYCLE);
      temp_read = 0;
      t = 0;
      while (temp_read != 1 && temp_read != 100 && t++ < TIME_CYCLE_MAX) { //旋轉直到只有左邊第二個燈或最右邊的燈檢測到黑線或超時
        temp_read = Read();
        delay(1);
      }

      Move(0, 0);
      break;

    //右轉直角
    case 11:
    case 111:
      leave_line();

      //讓小車靜止下來,保證小車旋轉時不受之前的輪子速度影響
      Move(0, 0);
      delay(TIME_STOP);

      //原地旋轉
      Move(SPEED_CYCLE, -SPEED_CYCLE);
      temp_read = 0;
      t = 0;
      while (temp_read != 1000 && temp_read != 10 && t++ < TIME_CYCLE_MAX) { //旋轉直到只有右邊第二個燈或最左邊的燈檢測到黑線或超時
        temp_read = Read();
        delay(1);
      }

      Move(0, 0);
      break;

    //左轉大彎
    case 1000:
      //記錄黑線方向,用於之後判斷是否有V型彎
      time_direct = millis();
      direct = 1;

      leave_line();

      Move(SPEED_TURN_LOW, SPEED_TURN_HIGH);
      temp_read = 0;
      t = 0;
      while (temp_read != 1  && temp_read != 100 && temp_read != 101 && temp_read != 1010 && temp_read != 110 && t++ < TIME_TURN_MAX) { //旋轉直到只有左邊第二個燈或最右邊的燈檢測到黑線、超時或檢測到V型彎時
        delay(1);
        temp_read = Read();
      }

      break;

    //右轉大彎
    case 1:
      //記錄黑線方向,用於之後判斷是否有V型彎
      time_direct = millis();
      direct = 2;

      leave_line();

      Move(SPEED_TURN_HIGH, SPEED_TURN_LOW);
      temp_read = 0;
      t = 0;
      while (temp_read != 1000 && temp_read != 10  && temp_read != 101 && temp_read != 1010  && t++ < TIME_TURN_MAX) { //旋轉直到只有右邊第二個燈或最左邊的燈檢測到黑線、超時或檢測到V型彎時
        delay(1);
        temp_read = Read();
      }
      break;

    //左轉小彎調整
    case 100:
      //V型彎檢測算法
      if ((millis() - time_direct) > TIME_DIRECT_ZERO)//清零黑線記錄的方向
        direct = 0;
      else if (direct == 2) {//即在500s內最最右邊和左邊第二個先後檢測到黑線,是V型彎
        leave_line();
        turnv_r();
      }

      //普通位置調整
      Move(SPEED_ADJUST_LOW, SPEED_ADJUST_HIGH);
      delay(TIME_ADJUST);
      break;

    //右轉小彎調整
    case 10:
      //V型彎檢測算法
      if ((millis() - time_direct) > TIME_DIRECT_ZERO)//清零黑線記錄的方向
        direct = 0;
      else if (direct == 1) {//即在500s內最最左邊和右邊第二個先後檢測到黑線,是V型彎
        leave_line();
        turnv_l();
      }

      //普通位置調整
      Move(SPEED_ADJUST_HIGH, SPEED_ADJUST_LOW);
      delay(TIME_ADJUST);
      break;

    //十字路口
    case 1001:
    case 1111:
      delay(1);
      direct = 0;
      break;

    //左轉銳角彎
    case 1010:
      //記錄黑線方向,備用
      time_direct = millis();
      direct = 1;

      leave_line();
      turnv_l();
      break;

    //右轉銳角彎
    case 101:
      //記錄黑線方向,備用
      time_direct = millis();
      direct = 2;

      leave_line();
      turnv_r();
      break;

    //銳角頂點或十字或不確定狀態
    case 110:
      if (direct == 1) {//檢測到了銳角頂點且之前檢測銳角在左面
        leave_line();
        turnv_l();
      }
      else if (direct == 2) {//檢測到了銳角頂點且之前檢測銳角在右面
        leave_line();
        turnv_r();
      }
      else {//應該不是V型彎
        delay(1);
      }
      break;
    default:
      delay(1);
  }
}

/**
   函數功能：讀取小車傳感器狀態
   入口參數：無
   返回參數：千位到個位分別表示四個傳感器的狀態，1111表示四路檢測到，0表示未檢測到黑線
  */
int Read(void) {
  return (digitalRead(D1) + digitalRead(D2) * 10 + digitalRead(D3) * 100 + digitalRead(D4) * 1000);
}

/**
   函數功能：設置小車的兩個輪子轉速
   入口參數：L左輪輸出速度，範圍0~255
   入口參數：R右輪輸出速度，範圍0~255
   返回參數：無
  */
void Move(int L, int R) {
  int area, remainder; //區間，餘數

  //直行指示燈
  if (L == R)
    digitalWrite(13, HIGH);
  else
    digitalWrite(13, LOW);

  //記錄設置速度,制動時使用
  speed_l = L;
  speed_r = R;

  //左輪方向設置
  if (L > 0) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  }
  else if (L < 0) {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  } else {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
  }

  //右輪方向設置
  if (R > 0) {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  }

  else {
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
  }

  //速度計算
  area = R / 40;
  remainder = R % 40;

  if (R > 0 && R <= 40)
    R = R - motor_offset[6] * remainder / 40;
  else if (R > 40 && R < 240)
    R = R - (motor_offset[area + 6] + (motor_offset[area + 6 + 1] - motor_offset[area + 6]) * remainder / 40);
  else if (R >= 240)
    R = R - motor_offset[11];
  else if (R < 0 && R >= -40)
    R = R - motor_offset[5] * (-remainder) / 40;
  else if (R < -40 && R > -240)
    R = R - (motor_offset[area + 6] + (motor_offset[area + 6 - 1] - motor_offset[area + 6]) * (-remainder) / 40);
  else if (R <= -240)
    R = R - motor_offset[0];
  else
    R = 0;

  L = abs(L);
  R = abs(R);
  if (L > 0xff) L = 0xff;
  if (R > 0xff) R = 0xff;

  //速度設置
  analogWrite(ENA, L);
  analogWrite(ENB, R);
  //  analogWrite(ENA, 0);
  //  analogWrite(ENB, 0);


}

/**
   函數功能：讓小車離開黑線
   入口參數：無
   返回參數：無
  */
void leave_line(void) {
  Move(SPEED_LINE, SPEED_LINE);
  t = 0;
  while (Read() != 0 && t++ < TIME_LEAVE_MAX) {
    delay(1);
  }

  if (speed_l > 0 && speed_r > 0)
    Move(-SPEED_TRIG, -SPEED_TRIG);
  if (speed_l > 0 && speed_r < 0)
    Move(SPEED_TRIG, -SPEED_TRIG);
  if (speed_l > 0 && speed_r < 0)
    Move(-SPEED_TRIG, SPEED_TRIG);
  delay(TIME_TRIG);

  Move(0, 0);
}

/**
   函數功能：左轉V型彎
   入口參數：無
   返回參數：無
  */
void turnv_l(void) {
  LED_ON;
  Move(-SPEED_CYCLE, SPEED_CYCLE);
  t = 0;
  while (Read() != 1000 && t++ < TIME_CYCLE_MAX)
    delay(1);

  Move(SPEED_TURN_LOW, SPEED_TURN_HIGH);
  t = 0;
  while (Read() != 100 && t++ < TIME_TURN_MAX) //旋轉直到只有左邊第二個燈檢測到黑線或超時
    delay(1);
  LED_OFF;
}

/**
   函數功能：右轉V型彎
   入口參數：無
   返回參數：無
*/
void turnv_r(void) {
  LED_ON;
  Move(SPEED_CYCLE, -SPEED_CYCLE);
  t = 0;
  while (Read() != 1 && t++ < TIME_CYCLE_MAX)
    delay(1);

  Move(SPEED_TURN_HIGH, SPEED_TURN_LOW);
  t = 0;
  while (Read() != 10 && t++ < TIME_TURN_MAX) //旋轉直到只有右邊第二個燈檢測到黑線或超時
    delay(1);
  LED_OFF;
}