fix

include/consts.example.h

@@ -12,16 +12,25 @@
 #define EEPROM_SIZE 1024
 
 // 超声波传感器引脚
-#define HC_SR04_TRIG 23
+#define US_TRIG 23
-#define HC_SR04_ECHO 22
+#define US_ECHO 22
+
+// 超声波的舵机
+#define US_SERVO_PIN 4
 
 // 红外循迹模块引脚
 // 模式选择
-#define IR_MODE IRMode::IR_MODE_GPIO
+// #define IR_MODE IRMode::IR_MODE_GPIO
+#define IR_MODE IRMode::IR_MODE_I2C
+#define IR_COUNT 8
+// 是否反转电平输出
+#define IR_OUTPUT_REVERSE 0
+// 是否反转传感器排列方向
+#define IR_DIRECTION_REVERSE 1
 // GPIO 模式
-#define IR_PINS {34, 35, 12, 13, 15}
+// #define IR_PINS {34, 35, 12, 13, 15}
-// 串口模式
+// I2C 模式
-// #define IR_PINS {34, 35}
+#define IR_PINS {22, 23}
 
 // TB6612 #1 控制 A B 电机
 // A 电机 左前轮
@@ -53,9 +62,9 @@
 #define SPEED_INCREMENT 10 // 速度增量
 
 // 默认 PID 参数
-#define PID_KP 0.1
+#define PID_KP 1.0f    // 比例系数
-#define PID_KI 0.01
+#define PID_KI 0.0f    // 积分系数
-#define PID_KD 0.001
+#define PID_KD 0.1f    // 微分系数
 
 // LED闪烁间隔(ms)
 #define LED_FLASH_INTERVAL 1000 

include/ultrasound.h

@@ -2,6 +2,7 @@
 #define ULTRASOUND_H
 
 #include <Arduino.h>
+#include <ESP32Servo.h>
 
 struct UltrasonicPin
 {
@@ -12,8 +13,12 @@ struct UltrasonicPin
 struct Ultrasonic
 {
   static UltrasonicPin pin;
-  static void init(UltrasonicPin pin);
+  static void init(UltrasonicPin pin, int servoPin);
   static float getDistance();
+
+  static int servoPin;
+  static Servo servo;
+  static void servoControl(int angle);
 };
 
 #endif

platformio.ini

@@ -13,11 +13,13 @@ platform = espressif32
 board = esp32dev
 framework = arduino
 monitor_speed = 115200
-lib_deps = plerup/EspSoftwareSerial@^8.2.0
+lib_deps = 
+	madhephaestus/ESP32Servo@^3.0.6
 
 [env:esp32s3]
 platform = espressif32
 board = adafruit_feather_esp32s3_tft
 framework = arduino
 monitor_speed = 115200
-lib_deps = plerup/EspSoftwareSerial@^8.2.0
+lib_deps = 
+	madhephaestus/ESP32Servo@^3.0.6

src/main.cpp

@@ -30,6 +30,9 @@ void setup()
   // 初始化红外循迹模块
   IR::init();
 
+  // 初始化超声波
+  Ultrasonic::init({US_TRIG, US_ECHO}, US_SERVO_PIN);
+
   // 初始化状态灯
   if (STATUS_LED_ENABLE)
   {

src/ultrasound.cpp

@@ -1,12 +1,23 @@
 #include "ultrasound.h"
 
-UltrasonicPin Ultrasonic::pin = {0, 0};
+Servo Ultrasonic::servo;
 
-void Ultrasonic::init(UltrasonicPin pin)
+UltrasonicPin Ultrasonic::pin = {0, 0};
+int Ultrasonic::servoPin = 0;
+
+void Ultrasonic::init(UltrasonicPin pin, int servoPin)
 {
   Ultrasonic::pin = pin;
+  Ultrasonic::servoPin = servoPin;
   pinMode(pin.trig, OUTPUT);
   pinMode(pin.echo, INPUT);
+  
+  servo.attach(servoPin);
+  servo.write(0);
+  delay(500);
+  servo.write(180);
+  delay(500);
+  servo.write(90);
 }
 
 float Ultrasonic::getDistance()
@@ -18,3 +29,11 @@ float Ultrasonic::getDistance()
   distance = distance * 340 / 2 / 10000;    // 计算距离 1：声速：340M/S  2：实际距离为1/2声速距离 3：计数时钟为1US//温补公式：c=(331.45+0.61t/℃)m•s-1 (其中331.45是在0度）
   return distance;
 }
+
+void Ultrasonic::servoControl(int angle)
+{
+  if (angle < 0) angle = 0;
+  if (angle > 180) angle = 180;
+  
+  servo.write(angle);
+}