Arduino循迹避障小车：结合循迹模块和红外传感器实现智能行走

int PWM_left = 6; // 左电机PWM引脚
int left_dir = 7; // 左电机方向引脚
int PWM_right = 5; // 右电机PWM引脚
int right_dir = 2; // 右电机方向引脚
int base_motor_speed = 10; // 电机基础速度
//沿着黑线路径走
int count = 1; // 计数器，用于记录转弯次数
int threshold=940;  //地面传感器感知黑色的阈值，大于则是黑色，小于则不是黑色
//沿墙
int red_led = 11; // 红色LED灯引脚
volatile int proximity[8] ; // 存储红外传感器读数的数组
//全部引脚
unsigned char analogPin[20] = {A8,A9,A10,A11,A0,A1,A2,A3,A4,A5,A6,A7}; // 模拟引脚数组
int distance=0; //表示距离，单位毫米
int iCurLeftMotorForwardSpeed=0,iCurRightMotorForwardSpeed=0,iCurLeftMotorBackwardSpeed,iCurRightMotorBackwardSpeed=0;  //当前左轮和右轮的速度

void setup() {
  //初始化电动机相关引脚为输出模式
  DDRJ |= 0x0F;
  DDRK &= ~0x0F; //将PK0-PK3设置为输入模式
  initMotor();
  //初始化沿墙相关引脚为输出模式
  pinMode(red_led,OUTPUT);
  //初始化红外LED
  initIRLED();
  Serial.begin(57600);
}

void loop() {
  /*设定Ground1和2作为左右点用于一开始就感应到触线(循迹)
    设定Ground0和3也感应到黑线才算完全到达finish（stop）*/
  bool leftIsBlack = readGroundSensorsIsBlack(1); // 读取左侧地面传感器是否检测到黑线
  bool rightIsBlack = readGroundSensorsIsBlack(2); // 读取右侧地面传感器是否检测到黑线
  bool left_2IsBlack = readGroundSensorsIsBlack(0); // 读取左侧第二个地面传感器是否检测到黑线
  bool right_2IsBlack = readGroundSensorsIsBlack(3); // 读取右侧第二个地面传感器是否检测到黑线
  //
  //int distance0 = readProximityDistance(0);
  int distance1 = readProximityDistance(7); // 读取左侧前方红外传感器的距离
  int distance2 = readProximityDistance(5); // 读取左侧前方红外传感器的距离
  

  //循迹运动
  
  if(count<=4){
    //若两边同时感应到则转弯，其余直行
    if(leftIsBlack && rightIsBlack) 
    {
      //感应到后先暂停，给予机器人反应机会
      MotorStop();
      Serial.println('MotorStop'); 
      delay(10);
      //若为奇数次则左转，若为偶数次则右转，即第1、3次左转，2、4次右转
      if(count % 2 ==0) 
      {
        rightTurn(12);
        Serial.println('rightTurn');
        delay(1000);
      }else{
        leftTurn(12);
        Serial.println('leftTurn'); 
        delay(1000);
      } 
      count++;//计数器+1
    }
    else{
      forWard(12);
      Serial.println('forWard'); 
    }
    delay(1000);
  }


  //沿墙运动以及STOP

  else{
    //当四个地面传感器都感应到，则STOP
    if(leftIsBlack && rightIsBlack && left_2IsBlack && right_2IsBlack ){
      MotorStop();
      Serial.println('MotorStop');
    }
    
    //沿墙
    //如果proximity sensor 0检测到前方障碍物且proximity sensor 2未检测到右边墙，则右转
    else if(readProximityDistance(0) && !readProximityDistance(2)){
      rightTurn(12);
        Serial.println('rightTurn');
        delay(1000);
    }
    //如果proximity sensor 6检测到左侧墙，则进行沿墙运动
    else if(readProximityDistance(6)){ 
      //沿左侧墙运动
      forWard(12);
      Serial.println('forWard');
      //如果proximity sensor 5检测到左侧墙，则左轮速度减小
      if(readProximityDistance(5) ){
        leftMotorForward(8);
      }
      //如果proximity sensor 7检测到左侧墙，则右轮速度减小
      else if(readProximityDistance(7) ){
        rightMotorForward(8);
      }
    }
    //继续沿左侧墙运动
    else{
      forWard(12);
      Serial.println('forWard');
    }
  }
}

//循迹

//打开ground LED
void groundLEDon(unsigned char lineIndex)
{
  PORTJ &= ~(1<<lineIndex);
}

//关闭ground LED
void groundLEDoff(unsigned char lineIndex)
{
  PORTJ |= 1<<lineIndex;
}
//读取第lineIndex个传感器的值
int readGroundSensor(unsigned char lineIndex)
{
  return analogRead(analogPin[lineIndex]);
}
//读出第地面传感器的值
bool readGroundSensorsIsBlack(unsigned char lineIndex)
{
  groundLEDon(lineIndex);
  delay(1);
  int val=readGroundSensor(lineIndex);
  groundLEDoff(lineIndex);
  Serial.print('Ground Sensor ');
  Serial.print(lineIndex);
  Serial.print(' : ');
  Serial.println(val); 
  if(val>threshold) //检测到黑色
  {
    return true;
  }else{
    return false;
  }
}

//初始化电动机相关引脚为输出模式
void initMotor()
{
  pinMode(PWM_right, OUTPUT);
  pinMode(PWM_left, OUTPUT);
  pinMode(right_dir, OUTPUT);
  pinMode(left_dir, OUTPUT);
}

//沿墙

void proxLEDon(unsigned char proxIndex){
    PORTA |= 1<<proxIndex;
}

void proxLEDoff(unsigned char proxIndex){
    PORTA &= ~(1<<proxIndex);
}

//初始化红外LED
void initIRLED()
{
  DDRA |= B11111111;
  PORTA &= B00000000; 
}

int readProximityDistance(unsigned char proxIndex){

      int distance;
      proxLEDon(proxIndex);
      delay(1);          
      proximity[proxIndex] = analogRead(analogPin[proxIndex]); 
      proxLEDoff(proxIndex); 
      return proximity[proxIndex] > 400; // 判断是否检测到障碍物
}

//前后左右转

//左转
void leftTurn(float duty)
{
  leftMotorForward(0);
  rightMotorForward(duty);
}
//右转
void rightTurn(float duty)
{
  leftMotorForward(duty);
  rightMotorForward(0);
}
//前进
void forWard(float duty)
{
  leftMotorForward(duty);
  rightMotorForward(duty);
}
void MotorStop(){
  iCurLeftMotorForwardSpeed = 0;
  iCurRightMotorForwardSpeed = 0;
  iCurLeftMotorBackwardSpeed = 0;
  iCurRightMotorBackwardSpeed = 0;
  digitalWrite(right_dir, HIGH);
  digitalWrite(PWM_right, HIGH);
   
  digitalWrite(left_dir, HIGH);
  digitalWrite(PWM_left, HIGH);
}

//左轮前进，速度duty
void leftMotorForward(float duty){
  int dutyInt = duty;
  if(iCurLeftMotorForwardSpeed != dutyInt)
  {
    int dutyVal = map(dutyInt,0,100,0,255);

    analogWrite(PWM_left, dutyVal);
    analogWrite(left_dir, 0);
    iCurLeftMotorForwardSpeed = dutyInt;
  }
}
//右轮前进，速度duty
void rightMotorForward(float duty){
  int dutyInt = duty;
  if(iCurRightMotorForwardSpeed != dutyInt)
  {
    int dutyVal = map(dutyInt,0,100,0,255);
    analogWrite(PWM_right, dutyVal);
    analogWrite(right_dir, 0);
    iCurRightMotorForwardSpeed = dutyInt;
  }
}
//左轮后退，速度duty
void leftMotorBackward(float duty){
  int dutyInt = duty;
  if(iCurLeftMotorBackwardSpeed != dutyInt)
  {
    int dutyVal = map(dutyInt,0,100,0,200);  
    analogWrite(PWM_left, 0);
    analogWrite(left_dir, dutyVal);
    iCurLeftMotorBackwardSpeed = dutyInt;
  }
}
//右轮后退，速度duty
void rightMotorBackward(float duty){
  int dutyInt = duty;
  if(iCurRightMotorBackwardSpeed != dutyInt)
  {
    int dutyVal = map(dutyInt,0,100,0,200);
    analogWrite(PWM_right, 0);
    analogWrite(right_dir, dutyVal);
    iCurRightMotorBackwardSpeed = dutyInt;
  }
}