求大神帮我看看HC-SR04超声波滤波算法的问题

sventy5

SR04超声波价格便宜，但是存在一定的误差，作为小白的我就按葫芦画瓢写了一个超声波滤波程序，结果不管怎么调试都不能输出正确的值，还请大神们解救{:soso_e109:}

1. int distance1;
   
2. int output1 = 5;
   
3. int intput1 = 6;
   
4. //////////////////////////////////////////////////////
   
5. void setup() {
   
6.   Serial.begin(9600);     // 初始化串口通信
   
7.   pinMode(intput1, INPUT);
   
8.   pinMode(output1, OUTPUT);
   
9.   randomSeed(Filter()); // 产生随机种子
   
10. }
    
11. void loop() {
    
12.   distance1 = Filter();       // 获得滤波器输出值
    
13.   distance1 = distance1;          // 最近一次有效采样的值，该变量为全局变量
    
14.   ////////////////////////////////////////////////////////////////
    
15. 
    
16.   { //第一个超声波
    
17.     digitalWrite(output1, LOW);
    
18.     delayMicroseconds(2);
    
19.     digitalWrite(output1, HIGH);
    
20.     delayMicroseconds(10);
    
21.     intput1 = pulseIn(intput1, HIGH);
    
22.     distance1 = intput1 / 58;
    
23.     distance1 = constrain(distance1, 10, 300);
    
24.   }
    
25.   Serial.print("distance1=");
    
26.   Serial.print(distance1);
    
27.   delay(500);
    
28. }
    
29. //用于随机产生一个100左右的当前值
    
30. int Get_AD() {
    
31.   random(95, 105);
    
32. }
    
33. // 限幅滤波（又称程序判断滤波法）
    
34. #define FILTER_A 100
    
35. int Filter() {
    
36.   int Newintput1;
    
37.   Newintput1 = Get_AD();
    
38.   if (((Newintput1 - intput1) < FILTER_A) || ((intput1 - Newintput1) < FILTER_A))
    
39.     return intput1;
    
40.   else
    
41.     return Newintput1;
    
42. }
    

复制代码