Alexcnc

distancija

Dec 5th, 2021 (edited)
634
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // -------НАСТРОЙКИ-------
  2.  
  3. // сонар
  4. #define ECHO 2
  5. #define TRIG 3
  6. #define sensVCC 4
  7.  
  8. // дисплей
  9. #define dispGND 5
  10. byte DIO = 6;
  11. byte RCLK = 7;
  12. byte SCLK = 8;
  13. #define dispVCC 9
  14.  
  15. // индикация
  16. #define ledPin 13 // выход управления
  17.  
  18. // создаём дисплей
  19. #include <TM74HC595Display.h>
  20. #include <TimerOne.h>
  21. TM74HC595Display disp(SCLK, RCLK, DIO);
  22. unsigned char SYM[47];
  23.  
  24. // крутая библиотека сонара
  25. #include <NewPing.h>
  26. NewPing sonar(TRIG, ECHO, 400);
  27.  
  28. float dist_3[3] = {0.0, 0.0, 0.0};   // массив для хранения трёх последних измерений
  29. float middle, dist, dist_filtered;
  30. float k;
  31. byte i, delta;
  32. unsigned long dispIsrTimer, sensTimer;
  33.  
  34. void setup() {
  35.   //Serial.begin(9600);
  36.   symbols();            // создать символы для отображения на дисплее
  37.  
  38.   // настройка пинов
  39.   pinMode(sensVCC, OUTPUT);
  40.   pinMode(dispGND, OUTPUT);
  41.   pinMode(dispVCC, OUTPUT);
  42.   pinMode(ledPin, OUTPUT);
  43.  
  44.   // подаём сигналы на пины
  45.   digitalWrite(sensVCC, 1);
  46.   digitalWrite(dispGND, 0);
  47.   digitalWrite(dispVCC, 1);
  48.  
  49.   }
  50.  
  51. void loop() {
  52.   if (millis() - sensTimer > 50) {                          // измерение и вывод каждые 50 мс
  53.     // счётчик от 0 до 2
  54.     // каждую итерацию таймера i последовательно принимает значения 0, 1, 2, и так по кругу
  55.     if (i > 1) i = 0;
  56.     else i++;
  57.  
  58.     dist_3[i] = (float)sonar.ping() / 57.5;                 // получить расстояние в текущую ячейку массива
  59.     dist = middle_of_3(dist_3[0], dist_3[1], dist_3[2]);    // фильтровать медианным фильтром из 3ёх последних измерений
  60.  
  61.     delta = abs(dist_filtered - dist);                      // расчёт изменения с предыдущим
  62.     if (delta > 1) k = 0.7;                                 // если большое - резкий коэффициент
  63.     else k = 0.1;                                           // если маленькое - плавный коэффициент
  64.  
  65.     dist_filtered = dist * k + dist_filtered * (1 - k);     // фильтр "бегущее среднее"
  66.     disp.clear();                                           // очистить дисплей
  67.     disp.float_dot(dist_filtered, 1);                       // вывести
  68.  
  69.  
  70.     if(dist_filtered >= 60)                                 // если растояние больше
  71.     {
  72.       digitalWrite (ledPin,HIGH);                           // устанавливаем выход управления в высокий уровень
  73.      
  74.     }
  75.      else                                                   // иначе
  76.      {
  77.       digitalWrite (ledPin, LOW);                           // устанавливаем выход управления в низкий уровень
  78.      }
  79.     sensTimer = millis();                                   // сбросить таймер
  80.     //Serial.println(dist_filtered);
  81.   }
  82.  
  83.   if (micros() - dispIsrTimer > 20) {       // таймер динамической индикации (по-русски: КОСТЫЛЬ!)
  84.     disp.timerIsr();                        // "пнуть" дисплей
  85.     dispIsrTimer = micros();                // сбросить таймер
  86.   }
  87. }
  88.  
  89. // медианный фильтр из 3ёх значений
  90. float middle_of_3(float a, float b, float c) {
  91.   if ((a <= b) && (a <= c)) {
  92.     middle = (b <= c) ? b : c;
  93.   }
  94.   else {
  95.     if ((b <= a) && (b <= c)) {
  96.       middle = (a <= c) ? a : c;
  97.     }
  98.     else {
  99.       middle = (a <= b) ? a : b;
  100.     }
  101.   }
  102.   return middle;
  103. }
  104.  
  105. // символы для дисплея
  106. void symbols() {
  107.   // обычные
  108.   SYM[0] = 0xC0; //0
  109.   SYM[1] = 0xF9; //1
  110.   SYM[2] = 0xA4; //2
  111.   SYM[3] = 0xB0; //3
  112.   SYM[4] = 0x99; //4
  113.   SYM[5] = 0x92; //5
  114.   SYM[6] = 0x82; //6
  115.   SYM[7] = 0xF8; //7
  116.   SYM[8] = 0x80; //8
  117.   SYM[9] = 0x90; //9
  118.  
  119.   // с точкой
  120.   SYM[10] = 0b01000000; //0.
  121.   SYM[11] = 0b01111001; //1.
  122.   SYM[12] = 0b00100100; //2.
  123.   SYM[13] = 0b00110000; //3.
  124.   SYM[14] = 0b00011001; //4.
  125.   SYM[15] = 0b00010010; //5.
  126.   SYM[16] = 0b00000010; //6.
  127.   SYM[17] = 0b01111000; //7.
  128.   SYM[18] = 0b00000000; //8.
  129.   SYM[19] = 0b00010000; //9.
  130. }
RAW Paste Data