单片机互通调试初步完成

esp32_robot_firmware/sensor_handler.cpp

@@ -0,0 +1,74 @@
+#include "sensor_handler.h"
+
+void sensor_setup() {
+    // 超声波传感器引脚设置
+    pinMode(ULTRASONIC_FRONT_TRIG_PIN, OUTPUT);
+    pinMode(ULTRASONIC_FRONT_ECHO_PIN, INPUT);
+    pinMode(ULTRASONIC_SIDE_TRIG_PIN, OUTPUT);
+    pinMode(ULTRASONIC_SIDE_ECHO_PIN, INPUT);
+    Serial.println("超声波传感器引脚已初始化。");
+
+    // TCRT5000 循迹传感器引脚设置
+    // 如果TCRT5000模块的DOUT引脚直接给出高低电平，则设为INPUT
+    // 如果是AOUT引脚，需要用analogRead()，引脚模式通常不需要显式设置，但INPUT也可以
+    pinMode(TCRT5000_LEFT_PIN, INPUT);
+    pinMode(TCRT5000_RIGHT_PIN, INPUT);
+    Serial.println("TCRT5000循迹传感器引脚已初始化。");
+}
+
+// --- 超声波传感器函数 ---
+long get_ultrasonic_distance_cm(int trig_pin, int echo_pin) {
+    digitalWrite(trig_pin, LOW);
+    delayMicroseconds(2);
+    digitalWrite(trig_pin, HIGH);
+    delayMicroseconds(10);
+    digitalWrite(trig_pin, LOW);
+    long duration = pulseIn(echo_pin, HIGH, 30000); // 等待脉冲返回，超时时间30ms (约5米)
+    // 声速为 340m/s 或 29.412 µs/cm (1 / (34000 / 1000000 / 2))
+    // (duration / 2) * 0.034 = duration * 0.017
+    // More precise: duration / 58.8 (for cm)
+    if (duration == 0) { // 超时或检测失败
+        return -1; // 返回-1表示无效读数或超出范围
+    }
+    return duration / 58; // Convert duration to cm
+}
+
+long get_front_obstacle_distance() {
+    return get_ultrasonic_distance_cm(ULTRASONIC_FRONT_TRIG_PIN, ULTRASONIC_FRONT_ECHO_PIN);
+}
+
+long get_side_spot_distance() {
+    return get_ultrasonic_distance_cm(ULTRASONIC_SIDE_TRIG_PIN, ULTRASONIC_SIDE_ECHO_PIN);
+}
+
+// --- TCRT5000 循迹传感器函数 ---
+
+// 辅助函数：根据模拟值判断是否在线上
+// 注意: TCRT5000输出特性可能不同，白色反射强->低电压/低模拟值，黑色吸收->高电压/高模拟值
+// 或者相反，取决于模块设计。这里的THRESHOLD假设的是：检测到黑线时读数高于阈值。
+// 您需要根据实际情况校准和调整逻辑。
+bool is_on_line(int sensor_pin, int threshold) {
+    int sensor_value = analogRead(sensor_pin);
+    // Serial.print("Sensor pin "); Serial.print(sensor_pin); 
+    // Serial.print(" value: "); Serial.println(sensor_value);
+    return sensor_value > threshold; // 示例：高于阈值是在黑线上
+    // 如果您的传感器是检测到黑线时值变小，则应为: return sensor_value < threshold;
+}
+
+LineFollowValues get_line_follow_values() {
+    LineFollowValues values;
+    
+    // 如果TCRT5000的DOUT引脚直接连接到数字引脚，则使用digitalRead()
+    // values.left_on_line = digitalRead(TCRT5000_LEFT_PIN) == HIGH; // 假设HIGH表示在黑线上
+    // values.right_on_line = digitalRead(TCRT5000_RIGHT_PIN) == HIGH;
+
+    // 如果使用AOUT引脚并进行模拟比较：
+    values.left_on_line = is_on_line(TCRT5000_LEFT_PIN, TCRT5000_THRESHOLD);
+    values.right_on_line = is_on_line(TCRT5000_RIGHT_PIN, TCRT5000_THRESHOLD);
+    
+    // 调试输出
+    // Serial.print("Line Follow: Left OnLine="); Serial.print(values.left_on_line);
+    // Serial.print(", Right OnLine="); Serial.println(values.right_on_line);
+
+    return values;
+}