-
Notifications
You must be signed in to change notification settings - Fork 0
/
KemaUS.cpp
68 lines (59 loc) · 1.42 KB
/
KemaUS.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
// (c) https://vk.com/myrobotics
#include "Arduino.h"
#include "KemaUS.h"
KemaUS::KemaUS(int trig1,int echo1,int trig2,int echo2)
{
_trig1 = trig1;
_echo1 = echo1;
_trig2 = trig2;
_echo2 = echo2;
_durationL = 0;
_durationR = 0;
_dataBuff[0] = 0;
_dataBuff[1] = 0;
_dataBuff[2] = 0;
_dataBuff[3] = 0;
_dataBuff[4] = 0;
_prevData = 0;
_curData = 0;
pinMode(_trig1, OUTPUT);
pinMode(_echo1, INPUT);
pinMode(_trig2, OUTPUT);
pinMode(_echo2, INPUT);
}
int KemaUS::pingL()
{
digitalWrite(_trig1, LOW);
digitalWrite(_trig1, HIGH);
_durationL = pulseIn(_echo1, HIGH, 3000);
return int((_durationL/2)/3.4);
}
int KemaUS::pingR()
{
digitalWrite(_trig2, LOW);
digitalWrite(_trig2, HIGH);
_durationR = pulseIn(_echo2, HIGH, 3000);
return int((_durationR/2)/3.4);
}
float KemaUS::objSpeed()
{
int summ = 0;
int i;
for (i = 0; i < 5; i = i + 1)
summ += _dataBuff[i];
_prevData = float(summ)/5;
int tempBuff[5];
for (i = 0; i < 4; i = i + 1)
tempBuff[i] = _dataBuff[i+1];
tempBuff[4] = int((_durationR/2)/3.4);
_dataBuff[0] = tempBuff[0];
_dataBuff[1] = tempBuff[1];
_dataBuff[2] = tempBuff[2];
_dataBuff[3] = tempBuff[3];
_dataBuff[4] = tempBuff[4];
summ = 0;
for (i = 0; i < 5; i = i + 1)
summ += _dataBuff[i];
_curData = float(summ)/5;
return (_curData - _prevData);
}