74 lines
3.2 KiB
C++
74 lines
3.2 KiB
C++
#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;
|
||
}
|