Arduino 小车沿墙运动代码 - 避障转向

// 定义引脚
int red_led = 11; // 红色 LED 引脚
volatile int proximity[8]; // 8 个 proximity sensor 的值
unsigned char analogPin[8] = {A0, A1, A2, A3, A4, A5, A6, A7}; // 8 个 proximity sensor 连接的模拟引脚
int PWM_left = 6; // 左电机 PWM 引脚
int left_dir = 7; // 左电机方向引脚
int PWM_right = 5; // 右电机 PWM 引脚
int right_dir = 2; // 右电机方向引脚
volatile int base_speed = 12; // 基本速度

void setup() {
  // 初始化设置
  pinMode(red_led, OUTPUT); // 设置红色 LED 为输出
  Serial.begin(57600); // 初始化串口通信
}

void loop() {
  // 主循环
  // 检测左侧是否有墙
  if (digitalRead(5) == HIGH || digitalRead(6) == HIGH || digitalRead(7) == HIGH) {
    // 左侧有墙，按照沿墙代码进行运动
    int distance1 = readProximityDistance(7); // 读取左侧 proximity sensor 7 的距离
    int distance2 = readProximityDistance(5); // 读取左侧 proximity sensor 5 的距离
    Serial.print("distance1:");
    Serial.println(distance1);
    Serial.print("distance2:");
    Serial.println(distance2);
    if ((distance1 >= 15 && distance1 < 20) && (distance2 >= 15 && distance2 < 20)) {
      // 左右距离都适中，保持直线行驶
      leftMotorForward(base_speed + 2);
      rightMotorForward(base_speed);
    } else if (distance1 >= 20 && distance2 < 20) {
      // 左侧距离较远，右转
      leftMotorForward(base_speed);
      rightMotorForward(base_speed + 1);
    } else if (distance1 >= 20 && distance2 >= 20) {
      // 两侧距离都较远，右转
      leftMotorForward(base_speed);
      rightMotorForward(base_speed + 3);
    } else if (distance1 < 15 && distance2 > 15) {
      // 左侧距离较近，左转
      leftMotorForward(base_speed + 3);
      rightMotorForward(base_speed);
    } else if (distance1 < 15 && distance2 < 15) {
      // 两侧距离都较近，左转
      leftMotorForward(base_speed + 5);
      rightMotorForward(base_speed);
    }
  } else {
    // 左侧无墙，检测前方是否有障碍物
    int distance0 = readProximityDistance(0); // 读取前方 proximity sensor 0 的距离
    int distance2 = readProximityDistance(2); // 读取右侧 proximity sensor 2 的距离
    if (distance0 < 15 && distance2 > 15) {
      // 前方有障碍物且右侧无障碍物，右转
      rightTurn();
    } else {
      // 没有障碍物，继续沿左侧墙运动
      int distance1 = readProximityDistance(7); // 读取左侧 proximity sensor 7 的距离
      int distance2 = readProximityDistance(5); // 读取左侧 proximity sensor 5 的距离
      Serial.print("distance1:");
      Serial.println(distance1);
      Serial.print("distance2:");
      Serial.println(distance2);
      if ((distance1 >= 15 && distance1 < 20) && (distance2 >= 15 && distance2 < 20)) {
        // 左右距离都适中，保持直线行驶
        leftMotorForward(base_speed + 2);
        rightMotorForward(base_speed);
      } else if (distance1 >= 20 && distance2 < 20) {
        // 左侧距离较远，右转
        leftMotorForward(base_speed);
        rightMotorForward(base_speed + 1);
      } else if (distance1 >= 20 && distance2 >= 20) {
        // 两侧距离都较远，右转
        leftMotorForward(base_speed);
        rightMotorForward(base_speed + 3);
      } else if (distance1 < 15 && distance2 > 15) {
        // 左侧距离较近，左转
        leftMotorForward(base_speed + 3);
        rightMotorForward(base_speed);
      } else if (distance1 < 15 && distance2 < 15) {
        // 两侧距离都较近，左转
        leftMotorForward(base_speed + 5);
        rightMotorForward(base_speed);
      }
    }
  }
}

// 初始化红外 LED
void initIRLED() {
  DDRA |= B11111111; // 设置端口 A 为输出
  PORTA &= B00000000; // 关闭端口 A 的所有引脚
}

// 打开 proximity sensor 的红外 LED
void proxLEDon(unsigned char proxIndex) {
  PORTA |= 1 << proxIndex; // 打开对应 proximity sensor 的红外 LED
}

// 关闭 proximity sensor 的红外 LED
void proxLEDoff(unsigned char proxIndex) {
  PORTA &= ~(1 << proxIndex); // 关闭对应 proximity sensor 的红外 LED
}

// 读取 proximity sensor 的距离
int readProximityDistance(unsigned char proxIndex) {
  int distance;
  proxLEDon(proxIndex); // 打开对应 proximity sensor 的红外 LED
  delay(1); // 延迟 1 毫秒
  proximity[proxIndex] = analogRead(analogPin[proxIndex]); // 读取 proximity sensor 的模拟值
  // 计算距离
  distance = 3.425 * pow(2.7182, 0.0026 * proximity[proxIndex]);
  proxLEDoff(proxIndex); // 关闭对应 proximity sensor 的红外 LED
  return distance;
}

// 停止电机
void MotorStop() {
  digitalWrite(right_dir, HIGH); // 设置右电机方向为高电平
  digitalWrite(PWM_right, HIGH); // 设置右电机 PWM 为高电平
  digitalWrite(left_dir, HIGH); // 设置左电机方向为高电平
  digitalWrite(PWM_left, HIGH); // 设置左电机 PWM 为高电平
}

// 左电机前进
void leftMotorForward(float duty) {
  int dutyInt = duty;
  int dutyVal = map(dutyInt, 0, 100, 0, 100); // 将 duty 映射到 0-100 的范围
  analogWrite(PWM_left, dutyVal); // 设置左电机 PWM
  analogWrite(left_dir, 0); // 设置左电机方向为低电平
}

// 右电机前进
void rightMotorForward(float duty) {
  int dutyInt = duty;
  int dutyVal = map(dutyInt, 0, 100, 0, 100); // 将 duty 映射到 0-100 的范围
  analogWrite(PWM_right, dutyVal); // 设置右电机 PWM
  analogWrite(right_dir, 0); // 设置右电机方向为低电平
}

// 左电机后退
void leftMotorBackward(float duty) {
  int dutyInt = duty;
  int dutyVal = map(dutyInt, 0, 100, 0, 80); // 将 duty 映射到 0-80 的范围
  analogWrite(PWM_left, 0); // 设置左电机 PWM 为低电平
  analogWrite(left_dir, dutyVal); // 设置左电机方向为高电平
}

// 右电机后退
void rightMotorBackward(float duty) {
  int dutyInt = duty;
  int dutyVal = map(dutyInt, 0, 100, 0, 80); // 将 duty 映射到 0-80 的范围
  analogWrite(PWM_right, 0); // 设置右电机 PWM 为低电平
  analogWrite(right_dir, dutyVal); // 设置右电机方向为高电平
}

// 右转
void rightTurn() {
  // 右转代码
  leftMotorForward(base_speed + 3); // 左电机前进
  rightMotorBackward(base_speed + 3); // 右电机后退
  delay(1000); // 延迟 1 秒
  MotorStop(); // 停止电机
}