mb1013/MB1013_Pulse.cpp at master · Team5593/mb1013 · GitHub

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
#include "MB1013_Pulse.h"

MB1013_Pulse::MB1013_Pulse(DigitalInput *input, DigitalOutput *enable):
	input(input),
	enable(enable)
{
	enable->Set(false);
}

double MB1013_Pulse::Get()
{
	bool timeout_start;

	enable->Set(true); // enable sensor

	timeout_start = getTime();
	while (input->Get() == false)
	{
		if (getTime() - timeout_start >= timeout)
			return -1;
	}

	double start_time = getTime();

	timeout_start = getTime();
	while (input->Get() == false)
	{
		if (getTime() - timeout_start >= timeout)
			return -1;
	}

	double end_time = getTime();

	double reading = end_time - start_time;
	reading /= 1000000; // convert from seconds to uS
	reading *= 1000; // convert from mm to meters

	enable->Set(false); // disable sensor

	return reading;
}

inline double MB1013_Pulse::getTime()
{
	return Timer::GetFPGATimestamp();
}