设置采集频率20hz 5A 0B 02 C3 4F E0 设置串口波特率115200 5A 06 02 80 04 73
样例1 单独测距
#include <SoftwareSerial.h>
#define START_BYTE 0x5C // 起始帧标志
#define FRAME_SIZE 4 // 数据帧长度
SoftwareSerial softSerial(4, 5); // (RX, TX) - D4 = RX, D5 = TX
uint8_t frameBuffer[FRAME_SIZE]; // 存储完整数据帧
bool capturing = false; // 是否正在接收数据
uint8_t index_ = 0; // 数据存储索引
void Read_distance(uint8_t byteReceived) {
if (!capturing) {
// 进入帧头检测
if (byteReceived == START_BYTE) { // 发现帧头
capturing = true;
index_ = 0;
frameBuffer[index_++] = byteReceived;
}
} else {
// 存储数据
frameBuffer[index_++] = byteReceived;
// 收到完整一帧数据
if (index_ >= FRAME_SIZE) {
capturing = false; // 解析完成，停止接收
processFrame(); // 解析数据
}
}
}
// 处理数据帧
void processFrame() {
uint8_t lowByte = frameBuffer[1]; // 低字节 小端模式 低在前
uint8_t highByte = frameBuffer[2]; // 高字节 小端模式
uint8_t checksum = frameBuffer[3]; // 校验字节
// 计算校验和（取反校验）
uint8_t calculatedChecksum = ~(highByte + lowByte);
if (checksum == calculatedChecksum) { // 校验成功
uint16_t distance = (highByte << 8) | lowByte;
if (distance > 65535) { //200米
// Serial.println("Invalid Distance!"); // 数据无效
} else {
Serial.print("Distance: ");
Serial.print(distance);
Serial.println(" cm");
}
} else {
// Serial.println("Checksum Error!");
}
}
void setup() {
Serial.begin(9600); // 电脑串口
softSerial.begin(115200); // 使用软串口代替 Serial2
}
void loop() {
while (softSerial.available()) {
uint8_t byteReceived = softSerial.read();
Read_distance(byteReceived);
}
}
例子2 计算车速
#include <SoftwareSerial.h>
#define START_BYTE 0x5C // 起始帧标志
#define FRAME_SIZE 4 // 数据帧长度
#define SPEED_HISTORY_SIZE 5 // 记录最近的5次距离用于测速
SoftwareSerial softSerial(4, 5); // (RX, TX) - D4 = RX, D5 = TX
uint8_t frameBuffer[FRAME_SIZE]; // 存储完整数据帧
bool capturing = false; // 是否正在接收数据
uint8_t index_ = 0; // 数据存储索引
unsigned long lastTime = 0; // 上次测量时间
unsigned long currentTime = 0; // 当前时间
float lastDistance = 0; // 上次距离
float speed = 0; // 车辆速度
float distanceHistory[SPEED_HISTORY_SIZE]; // 存储历史距离数据
uint8_t historyIndex = 0; // 历史数据索引
void Read_distance(uint8_t byteReceived) {
if (!capturing) {
// 进入帧头检测
if (byteReceived == START_BYTE) { // 发现帧头
capturing = true;
index_ = 0;
frameBuffer[index_++] = byteReceived;
}
} else {
// 存储数据
frameBuffer[index_++] = byteReceived;
// 收到完整一帧数据
if (index_ >= FRAME_SIZE) {
capturing = false; // 解析完成，停止接收
processFrame(); // 解析数据
}
}
}
// 处理数据帧
void processFrame() {
uint8_t lowByte = frameBuffer[1]; // 低字节 小端模式 低在前
uint8_t highByte = frameBuffer[2]; // 高字节 小端模式
uint8_t checksum = frameBuffer[3]; // 校验字节
// 计算校验和（取反校验）
uint8_t calculatedChecksum = ~(highByte + lowByte);
if (checksum == calculatedChecksum) { // 校验成功
uint16_t distance = (highByte << 8) | lowByte;
if (distance > 65535) { //200米
// Serial.println("Invalid Distance!"); // 数据无效
} else {
currentTime = millis(); // 获取当前时间
// 计算并输出距离
Serial.print("Distance: ");
Serial.print(distance);
Serial.print(" cm ");
// 记录历史距离
distanceHistory[historyIndex] = distance;
historyIndex = (historyIndex + 1) % SPEED_HISTORY_SIZE;
// 计算历史数据的平均值
float avgDistance = 0;
for (int i = 0; i < SPEED_HISTORY_SIZE; i++) {
avgDistance += distanceHistory[i];
}
avgDistance /= SPEED_HISTORY_SIZE;
// 计算速度（单位 cm/s）
if (lastTime != 0 && currentTime > lastTime) {
// 异常值剔除：如果本次距离与历史平均值差异较大，则认为其为异常值
if (abs(distance - avgDistance) < 50) { // 阈值50cm，具体值根据需求调整
speed = (avgDistance - lastDistance) / ((currentTime - lastTime) / 1000.0); // 速度 = 距离差 / 时间差
speed = -speed/100;
Serial.print("Speed: ");
Serial.print(speed); // 输出速度
Serial.println(" m/s");
} else {
//Serial.println("Speed measurement discarded due to outlier.");
}
}
// 更新上次测量的数据
lastDistance = avgDistance;
lastTime = currentTime;
}
} else {
// Serial.println("Checksum Error!");
}
}
void setup() {
Serial.begin(9600); // 电脑串口
softSerial.begin(115200); // 使用软串口代替 Serial2
// 初始化历史数据为0
for (int i = 0; i < SPEED_HISTORY_SIZE; i++) {
distanceHistory[i] = 0;
}
}
void loop() {
while (softSerial.available()) {
uint8_t byteReceived = softSerial.read();
Read_distance(byteReceived);
}
}
例子3 测速度 显示屏显示
#include <SoftwareSerial.h>
#include <U8g2lib.h> // 引入U8g2库
#define START_BYTE 0x5C // 起始帧标志
#define FRAME_SIZE 4 // 数据帧长度
#define SPEED_HISTORY_SIZE 5 // 记录最近的5次距离用于测速
// OLED显示设置（根据您使用的OLED型号调整）
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE, U8X8_PIN_NONE, U8X8_PIN_NONE);
SoftwareSerial softSerial(4, 5); // (RX, TX) - D4 = RX, D5 = TX
uint8_t frameBuffer[FRAME_SIZE]; // 存储完整数据帧
bool capturing = false; // 是否正在接收数据
uint8_t index_ = 0; // 数据存储索引
unsigned long lastTime = 0; // 上次测量时间
unsigned long currentTime = 0; // 当前时间
float lastDistance = 0; // 上次距离
float speed = 0; // 车辆速度
float distanceHistory[SPEED_HISTORY_SIZE]; // 存储历史距离数据
uint8_t historyIndex = 0; // 历史数据索引
void Read_distance(uint8_t byteReceived) {
if (!capturing) {
// 进入帧头检测
if (byteReceived == START_BYTE) { // 发现帧头
capturing = true;
index_ = 0;
frameBuffer[index_++] = byteReceived;
}
} else {
// 存储数据
frameBuffer[index_++] = byteReceived;
// 收到完整一帧数据
if (index_ >= FRAME_SIZE) {
capturing = false; // 解析完成，停止接收
processFrame(); // 解析数据
}
}
}
// 处理数据帧
void processFrame() {
uint8_t lowByte = frameBuffer[1]; // 低字节 小端模式 低在前
uint8_t highByte = frameBuffer[2]; // 高字节 小端模式
uint8_t checksum = frameBuffer[3]; // 校验字节
// 计算校验和（取反校验）
uint8_t calculatedChecksum = ~(highByte + lowByte);
if (checksum == calculatedChecksum) { // 校验成功
uint16_t distance = (highByte << 8) | lowByte;
if (distance > 65535) { // 200米
// Serial.println("Invalid Distance!"); // 数据无效
} else {
currentTime = millis(); // 获取当前时间
// 计算并输出距离
Serial.print("Distance: ");
Serial.print(distance);
Serial.print(" cm ");
// 记录历史距离
distanceHistory[historyIndex] = distance;
historyIndex = (historyIndex + 1) % SPEED_HISTORY_SIZE;
// 计算历史数据的平均值
float avgDistance = 0;
for (int i = 0; i < SPEED_HISTORY_SIZE; i++) {
avgDistance += distanceHistory[i];
}
avgDistance /= SPEED_HISTORY_SIZE;
// 计算速度（单位 cm/s）
if (lastTime != 0 && currentTime > lastTime) {
// 异常值剔除：如果本次距离与历史平均值差异较大，则认为其为异常值
if (abs(distance - avgDistance) < 50) { // 阈值50cm，具体值根据需求调整
speed = (avgDistance - lastDistance) / ((currentTime - lastTime) / 1000.0); // 速度 = 距离差 / 时间差
speed = -speed/100;
Serial.print("Speed: ");
Serial.print(speed); // 输出速度
Serial.println(" cm/s");
} else {
Serial.println("Speed measurement discarded due to outlier.");
}
}
// 更新上次测量的数据
lastDistance = avgDistance;
lastTime = currentTime;
// 更新OLED显示
u8g2.clearBuffer(); // 清除缓冲区
//u8g2.setFont(u8g2_font_ncenB08_tr); // 设置字体
// 显示当前距离和速度
float distance_f=float(distance)/100.0;
u8g2.drawStr(0, 15, "Distance: ");
u8g2.setCursor(60, 15);
u8g2.print(distance_f);
u8g2.print(" m");
u8g2.drawStr(0, 35, "Speed: ");
u8g2.setCursor(60, 35);
u8g2.print(speed);
u8g2.print(" m/s");
u8g2.sendBuffer(); // 显示缓冲区内容
}
} else {
// Serial.println("Checksum Error!");
}
}
void setup() {
Serial.begin(9600); // 电脑串口
softSerial.begin(115200); // 使用软串口代替 Serial2
// 初始化OLED显示
u8g2.begin();
u8g2.clearBuffer();
u8g2.setFont(u8g2_font_ncenB08_tr); // 设置字体
u8g2.drawStr(0, 15, "Initializing...");
u8g2.sendBuffer(); // 显示初始化文字
// 初始化历史数据为0
for (int i = 0; i < SPEED_HISTORY_SIZE; i++) {
distanceHistory[i] = 0;
}
}
void loop() {
while (softSerial.available()) {
uint8_t byteReceived = softSerial.read();
Read_distance(byteReceived);
}
}
浙公网安备 33010602011771号