-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathspeedometer.h
117 lines (92 loc) · 4.09 KB
/
speedometer.h
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
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
#ifndef SPEEDOMETER_H
#define SPEEDOMETER_H
#include <wiringPi.h>
#include <chrono>
#include <cmath>
static auto begin = std::chrono::steady_clock::now();
static auto end = std::chrono::steady_clock::now();
static int wheel_diameter_mm;
static int wheel_circumference_mm;
static int time_between_interrupt_duration_us;
class Speedometer_Wheel {
public:
static void myInterrupt() {
end = std::chrono::steady_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count();
// avoid spurious interrupts
if (duration >= time_between_interrupt_duration_us) {
double duration_ms = duration / 1000;
double distance_travelled_mm = wheel_circumference_mm * (1000/duration_ms);
double distance_travelled_meters = distance_travelled_mm / 1000;
double distance_travelled_in_km = distance_travelled_meters / 1000;
double speed_in_kmh = (distance_travelled_in_km * 3600);
std::cout << "speed_in_kmh: " << speed_in_kmh << "km/h" << "\n";
begin = std::chrono::steady_clock::now();
}
}
void start(int ISR_PIN, int mode) {
pinMode(ISR_PIN, INPUT);
wiringPiISR(ISR_PIN, mode, myInterrupt);
pullUpDnControl(ISR_PIN, PUD_UP);
}
void init(int WHEEL_DIAMETER_MM, int TIME_BETWEEN_INTERRUPTS_DURATION_US) {
wheel_diameter_mm = WHEEL_DIAMETER_MM;
wheel_circumference_mm = wheel_diameter_mm * M_PI;
time_between_interrupt_duration_us = TIME_BETWEEN_INTERRUPTS_DURATION_US;
}
};
static auto begin1 = std::chrono::steady_clock::now();
static auto end1 = std::chrono::steady_clock::now();
static int wheel_diameter_mm1;
static int wheel_circumference_mm1;
static int time_between_interrupt_duration_us1;
static double erpm_divider;
static double gear_ratio;
static double erpm_counter = 0;
static int duration_2;
class Speedometer_Motor {
public:
static void myInterrupt() {
end1 = std::chrono::steady_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end1 - begin1).count();
// avoid spurious interrupts
if (duration >= time_between_interrupt_duration_us1) {
std::cout << "ERPM_Counter " << erpm_counter << "\n";
if (erpm_counter < erpm_divider) {
erpm_counter++;
duration_2 += duration;
} else {
erpm_counter = 0;
double duration_ms = (duration_2 / erpm_divider) / 1000;
double erps = 1000/duration_ms;
double erpm = erps * 60;
double rpm = erpm / erpm_divider;
double rps = erps / erpm_divider;
// double distance_travelled_mm = wheel_circumference_mm * (1000/((double)duration_2 / 1000));
double distance_travelled_mm = wheel_circumference_mm * 1/(1 / rps);
distance_travelled_mm = distance_travelled_mm / gear_ratio;
double distance_travelled_meters = distance_travelled_mm / 1000;
double distance_travelled_in_km = distance_travelled_meters / 1000;
double speed_in_kmh = (distance_travelled_in_km * 3600);
duration_2 = 0;
std::cout << "speed_in_kmh: " << speed_in_kmh << "km/h" << "\n";
std::cout << "Motor RPM: " << rpm << "\n";
}
begin1 = std::chrono::steady_clock::now();
}
}
void start(int ISR_PIN, int mode) {
pinMode(ISR_PIN, INPUT);
wiringPiISR(ISR_PIN, mode, myInterrupt);
pullUpDnControl(ISR_PIN, PUD_UP);
}
void init(int WHEEL_DIAMETER_MM, int TIME_BETWEEN_INTERRUPTS_DURATION_US,
int MOTOR_SPROCKET_TEETH, int WHEEL_SPROCKET_TEETH, double ERPM_DIVIDER) {
wheel_diameter_mm1 = WHEEL_DIAMETER_MM;
wheel_circumference_mm1 = wheel_diameter_mm1 * M_PI;
time_between_interrupt_duration_us1 = TIME_BETWEEN_INTERRUPTS_DURATION_US;
erpm_divider = ERPM_DIVIDER;
gear_ratio = MOTOR_SPROCKET_TEETH / WHEEL_SPROCKET_TEETH;
}
};
#endif