atdrive-moab/ShaftEncoder.cpp at master · markoloco/atdrive-moab · 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
52
53
54
55
56
57
58
59

#include "ShaftEncoder.hpp"


ShaftEncoder::ShaftEncoder(PinName pinName) {

	_pin = new InterruptIn(pinName, PullUp);

	_pin->rise(callback(this, &ShaftEncoder::_rise_interrupt));
	_pin->fall(callback(this, &ShaftEncoder::_fall_interrupt));

	// My shaft encoder seems to have an asymmetry, so we keep a
	// separate "rise" timer vs "fall" timer
	timer1.start();
	timer2.start();

	rise_pps = 0;
	fall_pps = 0;
	current_pps = 0;  // pulses per second
}


void ShaftEncoder::_rise_interrupt() {

	uint32_t ts = timer1.read_us();
	timer1.reset();

	rise_pps = 1000000.0 / ts;
}


void ShaftEncoder::_fall_interrupt() {
	uint32_t ts = timer2.read_us();
	timer2.reset();

	fall_pps = 1000000.0 / ts;
}


double ShaftEncoder::get_pps() {

	uint32_t ts1 = timer1.read_us();
	uint32_t ts2 = timer2.read_us();

	double max_possible_pps;
	if (ts1 < ts2) {
		max_possible_pps = 1000000.0 / ts1;
	} else {
		max_possible_pps = 1000000.0 / ts2;
	}

	current_pps = 0.5 * (rise_pps + fall_pps);

	if (current_pps > max_possible_pps) {
		return max_possible_pps;
	}

	return current_pps;
}