Arduino循迹和沿墙机器人代码详解

Arduino循迹和沿墙机器人代码详解

这篇博客提供了一个完整的Arduino代码，用于控制机器人实现循迹和沿墙运动，并对每一行代码都进行了详细的注释。

功能描述:

• 机器人在地面传感器未检测到黑线时直行。
• 当左右两个地面传感器同时感应到黑线时，机器人停止5秒，然后根据计数器进行左转或右转：
  • 第一次、第三次等奇数次检测到黑线时左转。
  • 第二次、第四次等偶数次检测到黑线时右转。
  • 右转结束后直行5秒。
• 当左侧的四个红外传感器都检测到墙体时，机器人开始沿墙运动，并根据传感器读数调整左右电机速度，以保持与墙体平行。
• 当四个地面传感器同时感应到黑色时，机器人停止。

代码:

// 定义电机控制引脚
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; 
  // 将PK0-PK3设置为输入模式，用于读取地面传感器
  DDRK &= ~0x0F; 
  // 初始化电机控制引脚
  initMotor();
  // 初始化红外LED控制引脚
  pinMode(red_led, OUTPUT);
  // 初始化串口通信
  Serial.begin(57600);
}

void loop() {
  // 读取地面传感器状态
  bool leftIsBlack = readGroundSensorsIsBlack(1); // 读取左侧地面传感器
  bool rightIsBlack = readGroundSensorsIsBlack(2); // 读取右侧地面传感器

  // 根据传感器状态控制机器人运动
  if(leftIsBlack && rightIsBlack) {
    // 两侧传感器都检测到黑线，停止并转向
    MotorStop();
    Serial.println('MotorStop'); 
    delay(5000); // 停止5秒

    // 根据计数器确定转向方向
    if(count % 2 == 0) {
      rightTurn(12); // 右转
      Serial.println('rightTurn');
    } else {
      leftTurn(12); // 左转
      Serial.println('leftTurn'); 
    } 
    count++; // 计数器加1
  } else {
    // 未检测到黑线，直行
    forWard(12);
    Serial.println('forWard'); 
  }

  // 沿墙运动
  if (readProximityDistance(0) < 15 && readProximityDistance(1) < 15 && readProximityDistance(2) < 15 && readProximityDistance(3) < 15) {
    // 四个红外传感器都检测到墙体，停止
    MotorStop();
    Serial.println('MotorStop'); 
  } else {
    // 根据红外传感器读数调整左右电机速度，实现沿墙运动
    int distance1 = readProximityDistance(7); // 读取右侧前方红外传感器
    int distance2 = readProximityDistance(5); // 读取右侧后方红外传感器

    Serial.print('distance1:'); 
    Serial.println(distance1);
    Serial.print('distance2:');  
    Serial.println(distance2); 

    if((distance1 >= 15 && distance1 < 20) && (distance2 >= 15 && distance2 < 20)) {
      // 与墙体平行，保持直行
      leftMotorForward(base_motor_speed + 2);
      rightMotorForward(base_motor_speed);
    } else if(distance1 >= 20 && distance2 < 20) {
      // 远离墙体，向右调整
      leftMotorForward(base_motor_speed);
      rightMotorForward(base_motor_speed + 1);
    } else if(distance1 >= 20 && distance2 >= 20) {
      // 远离墙体，向右调整
      leftMotorForward(base_motor_speed);
      rightMotorForward(base_motor_speed + 3);
    } else if(distance1 < 15 && distance2 > 15) {
      // 靠近墙体，向左调整
      leftMotorForward(base_motor_speed + 3);
      rightMotorForward(base_motor_speed);
    } else if (distance1 < 15 && distance2 < 15) {
      // 靠近墙体，向左调整
      leftMotorForward(base_motor_speed + 5);
      rightMotorForward(base_motor_speed);
    }
  }
  delay(10);
}

// 打开地面传感器LED
void groundLEDon(unsigned char lineIndex) {
  PORTJ &= ~(1 << lineIndex);
}

// 关闭地面传感器LED
void groundLEDoff(unsigned char lineIndex) {
  PORTJ |= 1 << 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 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);
}

// 左轮前进
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;
  }
}

// 右轮前进
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;
  }
}

// 左轮后退
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;
  }
}

// 右轮后退
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;
  }
}

// 打开红外LED
void initIRLED(){
  DDRA |= B11111111;
  PORTA &= B00000000; 
}

// 打开指定红外LED
void proxLEDon(unsigned char proxIndex){
    PORTA |= 1<<proxIndex;
}

// 关闭指定红外LED
void proxLEDoff(unsigned char proxIndex){
    PORTA &= ~(1<<proxIndex);
}

// 读取指定红外传感器的距离值
int readProximityDistance(unsigned char proxIndex){
  int distance;
  proxLEDon(proxIndex);
  delay(1);          
  proximity[proxIndex] = analogRead(analogPin[proxIndex]); 
  // 使用公式将传感器读数转换为距离值
  distance = 3.425 * pow(2.7182, 0.0026 * proximity[proxIndex]); 
  proxLEDoff(proxIndex); 
  return distance;
}

代码说明:

• 代码中使用了多个函数来实现机器人的不同功能，例如forWard()用于控制机器人前进，leftTurn()用于控制机器人左转，readProximityDistance()用于读取红外传感器距离值等。
• 代码中使用了大量的注释来解释每一行代码的功能，方便读者理解。
• 代码中使用了Serial.print()函数将传感器读数和其他信息打印到串口监视器，方便调试。

希望这篇博客能够帮助你学习如何使用Arduino制作循迹和沿墙机器人。