Stargate/UltrasonicSensor.cpp at dev · cojosh/Stargate · 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
47
48
49
50
51
#include "UltrasonicSensor.h"
#include "DEBUG.h"

UltrasonicSensor::UltrasonicSensor(uint8_t trigger, uint8_t echo): trigger(trigger), echo(echo), lastTrigger(0) {
	DEBUG_PRINT("USensor initializing to:\n")

	pinMode(trigger, OUTPUT);
	pinMode(echo, INPUT);
	digitalWrite(trigger, LOW);
	delayMicroseconds(2);

	DEBUG_PRINT("Trigger: ")
	DEBUG_PRINT(this->trigger)
	DEBUG_PRINT("\nEcho: ")
	DEBUG_PRINT(this->echo)
	DEBUG_PRINT("\n")

	unsigned long first = ping();

	DEBUG_PRINT("First ping is: ")
    DEBUG_PRINT(first)
	DEBUG_PRINT("\n")

	firstPulse = first;
}

UltrasonicSensor::~UltrasonicSensor() {}

unsigned long UltrasonicSensor::ping() {
	digitalWrite(trigger, HIGH);
	delayMicroseconds(10);
	digitalWrite(trigger, LOW);

	unsigned long ret = pulseIn(echo, HIGH);

	DEBUG_PRINT("ping is: ")
	DEBUG_PRINT(ret)
	DEBUG_PRINT("\n")

	return ret;
}

bool UltrasonicSensor::disturbanceEvent() {
	float change = ((float)ping()) / ((float)firstPulse);

	DEBUG_PRINT("Diff to first pusle: ")
	DEBUG_PRINT(change)
	DEBUG_PRINT("\n")

	return change <= 0.9 ? true : false;
}