跟随原理

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

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

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

 效果展示

 百度网盘链接：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];
  }

}

//

 采购清单