基于红外感应的远距离智能跟随小车，自动跟随小车

跟随原理

下面介绍红外感应的跟随小车的自动跟随部分。

基本原理：在小车上安装一个红外接收器（能测量红外入射角的感应器），人手持一个红外发射模块。

根据不同的入射角，调整小车前进方向。若入射角在右边，就控制小车右转，若入射角在左边，就控制小车左转。

 效果展示

 百度网盘链接：https://pan.baidu.com/s/1PGTwDxvUbdksDmyfcXAMOw?pwd=vddg

提取码：vddg

代码

以ESP32芯片(Arduino开发环境)为例。

void loop() {
 
if (millis() - Run_Time > 200) {
    Run_Time = millis();
    LeftSpeed = 0; RightSpeed = 0;
    PineMotorRun(0);
  }
 
GetIRData(&Angle, &Distance);
 
if (Distance > 30 && Angle > 0 && Angle < 180) {
      LeftSpeed = 50 - (Angle - 90) * 0.5;
      RightSpeed = 50 + (Angle - 90) * 0.5;
      PineMotor_Run(LeftSpeed, RightSpeed);
      Run_Time = millis();
      OutStr = "L/R speed:" + String(LeftSpeed) + "," + String(RightSpeed);
     
 
 }
 
}

 红外感应代码 "IRSensor.h"

#ifndef __IRSENSOR_H__
#define __IRSENSOR_H__
 
#define IRPINCOUNT 7
#define MaxIRShotCount  300
#define MaxHistoryCount 10
#define InValidAngle -360.0
 
struct IRSENSOR_DATA{
  uint16_t IRCount[IRPINCOUNT];//红外感应器接受的脉冲数
  uint16_t IRValidCount[IRPINCOUNT];//红外感应器接受的有效脉冲数（脉冲间隔时间满足一定的范围）
  int IRTimeHistory[IRPINCOUNT][MaxIRShotCount];//红外感应器接受到脉冲时的时刻，单位为微秒
  long FirstTime[IRPINCOUNT];//单个感应器的首次脉冲时刻，单位为微秒
  long LastTime[IRPINCOUNT];  //单个感应器的最后脉冲时刻，单位为微秒
  long IRFirstTime, IREndTime;//整组感应器的首次，最后脉冲时刻，单位为微秒
 
  uint8_t ValidPinCount;//接受到有效信息的红外感应器个数
  double IRZone;
  double IRDistance;  
};
 
//红外感应器初始化
void iniIR();
static void IRPinCallBack(uint8_t i);
void DisableIRInterrupt();
void ResetIRSensorData();
void ResetIRData();
 
void ResetIRSensorData();
long GetFirstIRTime_ms();
long GetFinalIRTime_ms();
double CalculateAngleMovingMean(uint8_t LastCount);
 
void SumIRDataLoop();
void GetIRDataFromPin();//NotConfirmTime
double GetIRAngle();
void GetIRData(double *angle, int *distance);
String GetIRSignalStatus();
 
 
uint8_t GetHighLevelPinCount(uint16_t IRCount[], uint16_t LL);
void AnalysisIRValues_OneSide(IRSENSOR_DATA* mData, uint8_t Angle[]);
 
 
 
void RecordIRData(double Currentdata);
 
void GetValidCount(IRSENSOR_DATA* mData);
 
void Record_DoubleArray(double DArray[], double mydata, uint8_t * index, uint8_t Length);
void Push_DoubleArray(double History[], int Length);
void Reset_Array16(uint16_t IRCount[], int Length);
 
 
#endif

 红外感应代码 “IRSensor.cpp”

#include <Arduino.h>
 
#include "IRSensor.h"
 
struct IRSENSOR_DATA mIRData;
 
int  LineCount, Count2 = 0;
bool IsIniIRPin = false, IsReceivedIRData = false;
 
char strIR[140], Head[] = {"Angle:"};
 
//IR, Pin从左到右，角度从大到小
uint8_t IRPin[] = {33, 32, 34, 35, 23, 21, 19}; //
uint8_t Angle_Pin[] = {165, 140, 115, 90, 65, 40, 15};
 
bool IsHaveIRData = false, IsInNearArea = false;
 
double IRAngleHistory[MaxHistoryCount];
uint8_t IRRecordIndex = MaxHistoryCount - 1;
 
char IRs[256];
String IRStatus, AngleStatus, IRResponseTime_String, IRTimeStatus;
 
long LastTime_AnalysisData;
bool IsFinishAnalysis;
long SimulatedTime;
 
 
 
//
 
static void IRPinCallBack(uint8_t i) {
  long CurrentTime = micros();
 
  if (!IsHaveIRData) {
    mIRData.IRFirstTime = CurrentTime;
 
  }
 
  if (IsFinishAnalysis) {
    IsFinishAnalysis = false;
  }
 
  mIRData.IREndTime = CurrentTime;
 
  if (mIRData.IRCount[i] < MaxIRShotCount) {
 
    if (mIRData.IRCount[i] == 0) {
      mIRData.FirstTime[i] = CurrentTime;
      mIRData.IRTimeHistory[i][mIRData.IRCount[i]] = 0;
      mIRData.LastTime[i] = CurrentTime;
    } else {
      mIRData.IRTimeHistory[i][mIRData.IRCount[i]] = CurrentTime - mIRData.LastTime[i];
    }
    mIRData.LastTime[i] = CurrentTime;
 
    mIRData.IRCount[i]++;
  } else {
    mIRData.IRCount[i] = 0;
  }
 
  IsHaveIRData = true;
}
//
 
 
///
void IRAM_ATTR IntCallbackIR0() {
  uint8_t i = 0;
  IRPinCallBack(i);
}
 
void IRAM_ATTR IntCallbackIR1() {
  uint8_t i = 1;
  IRPinCallBack(i);
}
 
void IRAM_ATTR IntCallbackIR2() {
  uint8_t i = 2;
  IRPinCallBack(i);
}
 
void IRAM_ATTR IntCallbackIR3() {
  uint8_t i = 3;
  IRPinCallBack(i);
}
 
void IRAM_ATTR IntCallbackIR4() {
  uint8_t i = 4;
  IRPinCallBack(i);
}
 
void IRAM_ATTR IntCallbackIR5() {
  uint8_t i = 5;
  IRPinCallBack(i);
}
 
void IRAM_ATTR IntCallbackIR6() {
  uint8_t i = 6;
  IRPinCallBack(i);
}
///
 
void SumIRDataLoop() {
 
  if (IsHaveIRData && !IsFinishAnalysis && (millis() - GetFirstIRTime_ms() > 30)) {
    //一个发射周期是200毫秒，一个脉冲105*2微妙，共25个脉冲，共5.26毫秒。 在30毫秒后分析数据
    LastTime_AnalysisData = millis();
    GetIRDataFromPin();
    IsFinishAnalysis = true;
  }
 
  if (millis() - LastTime_AnalysisData > 200) {
    ResetIRData();
  }
 
}
 
 
void GetIRDataFromPin() {
  String IRstr = "";
  IRStatus = "";
 
  //SimulateIRData();
 
  if ( GetHighLevelPinCount(mIRData.IRCount, MaxHistoryCount) >= 6) {
    IsInNearArea = true;
    //Serial.printf("近距离\n");
  }
 
  AnalysisIRValues_OneSide(&mIRData, Angle_Pin);
 
  char SS[256];
  sprintf(SS, "Angle:%.2f\r\n", mIRData.IRZone);
  IRstr = String(SS);
 
  if (IsInNearArea) {
    IRstr += "IRD:30\r\n";
    mIRData.IRDistance = 30;
  } else {
    IRstr += "IRD:500\r\n";
    mIRData.IRDistance = 500;
  }
 
  IRstr += IRStatus;
 
  //Serial.print(IRstr);
 
  ResetIRSensorData();
}
 
 
 
double CalculateAngleMovingMean(uint8_t LastCount) {
  double sum = 0, validcount = 0;
  uint8_t i;
 
  if (LastCount > MaxHistoryCount) LastCount = MaxHistoryCount;
 
 
  for (i = 1; i <= LastCount; i++) {
    if (0 <= IRAngleHistory[MaxHistoryCount - i] && IRAngleHistory[MaxHistoryCount - i] <= 180 ) {
      sum += IRAngleHistory[MaxHistoryCount - i];
      validcount++;
    }
  }
 
  if (validcount > 1) {
    return sum / validcount;
  } else {
    return -1;
  }
 
 
}
 
String GetIRSignalStatus() {
  return IRStatus;
}
 
String GetIRResponseTime() {
  return IRResponseTime_String;
}
 
double GetIRAngle() {
  return mIRData.IRZone;
}
 
void GetIRData(double *angle, int *distance) {
  *angle =  mIRData.IRZone;
  *distance =  mIRData.IRDistance;
}
 
void ResetIRData() {
 
  mIRData.IRZone = InValidAngle;
  mIRData.IRDistance = 0;
  IRStatus = "";
  IRResponseTime_String = "";
  IsReceivedIRData = false;
}
 
void ResetIRSensorData() {
  uint8_t i;
 
 
  for (i = 0; i < IRPINCOUNT; i++) {
    mIRData.IRCount[i] = 0;
  }
 
  long TempTime;
  TempTime = micros();
  for (i = 0; i < IRPINCOUNT; i++) {
    mIRData.IRTimeHistory[i][0] = 0;
 
    uint16_t j;
    for (j = 0; j < MaxIRShotCount; j++) {
      mIRData.IRTimeHistory[i][j] = 0;
    }
 
    mIRData.LastTime[i] = TempTime + 20 * 1000;
    mIRData.FirstTime[i] = TempTime + 20 * 1000;
 
  }
  mIRData.ValidPinCount = 0;
 
  IsHaveIRData = false;
  IsInNearArea = false;
}
 
 
uint8_t GetHighLevelPinCount(uint16_t IRCount[], uint16_t LL) {
 
  uint8_t i, Count = 0;
  for (i = 0; i < IRPINCOUNT; i++) {
    if (IRCount[i] > LL) {
      Count++;
    }
  }
  return Count;
}
 
 
 
 
void AnalysisIRValues_OneSide(IRSENSOR_DATA *mData, uint8_t Angle[]) {
 
  uint16_t i, j;
  boolean IsAbnormal = false;
  double AngleMean = InValidAngle, AngleSum = 0.0;  //arduino无float型数据
  int CountSum = 0, ValidCountSum = 0;
  String IRStr = "", SideStr = "";
 
  SideStr = "IR:";
  IRStr = SideStr;
 
 
  for (i = 0; i < IRPINCOUNT; i++) {
    IRStr += String(mData->IRCount[i]) + ",";
    CountSum += mData->IRCount[i];
  }
 
  IRStr += "Valid:";
  if (CountSum <= 4) {  //偶尔几个脉冲，无需理会
    for (i = 0; i < IRPINCOUNT; i++) {
      mData->IRValidCount[i] = 0;
      IRStr +=  "0,";
 
    }
    IRStr += "\r\n";
    IRStatus += IRStr;
    //Serial.println(IRStatus);
    mData->IRZone = InValidAngle;
    //Serial.println("No IR Signal.");
    return;
  }
 
  //  Serial.printf("Signal Count: %d \n", CountSum);
  boolean isCheckPulseTime = false;
  String strOut;
 
  if (isCheckPulseTime) {
    GetValidCount(mData);
  } else {
    for (i = 0; i < IRPINCOUNT; i++) {
      mData->IRValidCount[i] = mData->IRCount[i];
    }
  }
 
  ValidCountSum = 1;//相除时，不会是0
  for (i = 0; i < IRPINCOUNT; i++) {
    IRStr += String(mData->IRValidCount[i]) + ",";
    ValidCountSum += mData->IRValidCount[i];
  }
  IRStr += "\r\n";
 
  IRStatus += IRStr;
 
  //  Serial.println(IRStr);
 
  for (i = 0; i < IRPINCOUNT; i++) {
    if ( mData->IRCount[i] > 100) {  //脉冲太多，是干扰信号
      IsAbnormal = true;
      mData->IRZone = InValidAngle;
      break;
    }
  }
 
  if (CountSum / ValidCountSum >= 3) {  //少于33%的有效脉冲，极有可能是干扰信号
    IsAbnormal = true;
    mData->IRZone = InValidAngle;
  }
 
  if (IsAbnormal) {
    Reset_Array16(mData->IRCount, IRPINCOUNT);
 
    //Serial.printf("abnormal\n");
    return;
  }
 
 
  //加权平均
 
  //  Serial.println("");
 
  AngleMean = 0;    AngleSum = 0;
  mData->ValidPinCount = 0;
  ValidCountSum = 0;
 
  for (i = 0; i < IRPINCOUNT; i++) {
    if (mData->IRValidCount[i] < 1) {    //脉冲数量少，认为是干扰，就置0
      mData->IRValidCount[i] = 0;
    } else {
      (mData->ValidPinCount)++;
    }
 
    AngleSum += Angle[i]  * mData->IRValidCount[i];
    ValidCountSum += mData->IRValidCount[i];
  }
 
  if (ValidCountSum > 0) {
    AngleMean = AngleSum / ValidCountSum;
  } else {
    AngleMean = InValidAngle;
  }
 
  sprintf(IRs, " Zone: %.2f Valid Signal Count: %d \n", AngleMean, ValidCountSum);
 
  strOut = SideStr + String(IRs);
 
  AngleStatus += strOut;
 
  mData->IRZone = AngleMean;
  Record_DoubleArray(IRAngleHistory, AngleMean, &IRRecordIndex, MaxHistoryCount);
 
}
 
void GetValidCount(IRSENSOR_DATA *mData) {
  int CL = 500; uint8_t Error = 50;
  int IRTime;
  uint16_t i, j;
 
 
  //用于调试程序
  String IRStr = "", strOut;
  for (j = 0; j < IRPINCOUNT; j++) {
    IRStr = String(j) + ":";
    for (i = 0; i < mData->IRCount[j]; i++) {
      IRStr += String(mData->IRTimeHistory[j][i]) + " ";
    }
    //    Serial.println(IRStr);
  }
 
 
 
  for (j = 0; j < IRPINCOUNT; j++) {
    mData->IRValidCount[j] = 0;
    for (i = 0; i < MaxIRShotCount; i++) {
 
      IRTime = (mData->IRTimeHistory[j][i] + CL / 2) % CL + CL / 2;
      if (CL - Error < mData->IRTimeHistory[j][i] && CL - Error < IRTime  &&  IRTime < CL + Error) {
        mData->IRValidCount[i]++;
      }
    }
 
  }
 
}
 
 
 
long GetFirstIRTime_ms() {
  return mIRData.IRFirstTime / 1000;
}
 
long GetFirstIRTime_us() {
  return mIRData.IRFirstTime;
}
 
long GetFinalIRTime_ms() {
  return mIRData.IREndTime / 1000;
}
 
//
 
 
//
void iniIR() {
  uint8_t i;
  uint8_t Model = FALLING; //CHANGE
 
  if (!IsIniIRPin) {
 
    for (i = 0; i < IRPINCOUNT; i++) {
      pinMode(IRPin[i], INPUT_PULLUP);
    }
 
    i = 0;
    attachInterrupt(digitalPinToInterrupt(IRPin[i]), IntCallbackIR0, Model);//FALLING
 
    i++;
    attachInterrupt(digitalPinToInterrupt(IRPin[i]), IntCallbackIR1, Model);
 
    i++;
    attachInterrupt(digitalPinToInterrupt(IRPin[i]), IntCallbackIR2, Model);
 
    i++;
    attachInterrupt(digitalPinToInterrupt(IRPin[i]), IntCallbackIR3, Model);
 
    i++;
    attachInterrupt(digitalPinToInterrupt(IRPin[i]), IntCallbackIR4, Model);
 
    i++;
    attachInterrupt(digitalPinToInterrupt(IRPin[i]), IntCallbackIR5, Model);
 
    i++;
    attachInterrupt(digitalPinToInterrupt(IRPin[i]), IntCallbackIR6, Model);
 
 
    ResetIRSensorData();
    IRStatus = "";
    AngleStatus = "";
    IRResponseTime_String = "";
    IRTimeStatus = "";
    IsIniIRPin = true;
  }
 
}
 
void DisableIRInterrupt() {
  uint8_t i;
  for (i = 0; i < IRPINCOUNT; i++) {
    detachInterrupt(digitalPinToInterrupt(IRPin[i]));
 
  }
  IsIniIRPin = false;
}
 
 
//
void Reset_Array16(uint16_t IRCount[], int Length) {
  int i;
  for (i = 0; i < Length; i++) {
    IRCount[i] = 0;
  }
 
}
 
 
void Record_DoubleArray(double DArray[], double mydata, uint8_t * index, uint8_t Length) {
 
  DArray[*index] = mydata;
  (*index)++;
  if ((*index) >= Length) {
    Push_DoubleArray(DArray, Length);
    *index = Length - 1;
  }
 
}
 
 
void Push_DoubleArray(double History[], int Length) {
  int i;
  for (i = 0; i < Length - 1; i++) {
    History[i] = History[i + 1];
  }
 
}
 
//

 采购清单