istomina_sofia

ультразвуковой дальномер

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