-
Notifications
You must be signed in to change notification settings - Fork 2
/
ESCController.cpp
97 lines (88 loc) · 2.18 KB
/
ESCController.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
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
#include "ESCController.h"
void ESCController::initialize(int controlled_esc[4], int ESCSanity[4], int frequency, int min, int max)
{
for (int i=0;i<4;i++) {
ESC[i] = controlled_esc[i];
gpioSetPWMfrequency(ESC[i], frequency);
gpioSetPWMrange(ESC[i], 1000000/frequency);
sanity[i] = ESCSanity[i];
}
outMin = min;
outMax = max;
}
void ESCController::allSetTo(int width)
{
for (int i=0;i<4;i++) {
gpioPWM(ESC[i], width);
}
}
void ESCController::arming()
{
if (!armed) {
allSetTo(1000);
gpioSleep(PI_TIME_RELATIVE, 5, 0);
armed = true;
}
}
void ESCController::stopMotor()
{
if (armed) {
allSetTo(1000);
}
}
void ESCController::reset()
{
for (int i=0;i<4;i++) {
oldESC[i] = outMin;
}
}
void ESCController::YPRT(int yprt[4])
{
int i;
int ESCOutput[4];
ESCOutput[0] = outMin+yprt[3] + yprt[0] - yprt[1] + yprt[2];
ESCOutput[1] = outMin+yprt[3] - yprt[0] + yprt[1] + yprt[2];
ESCOutput[2] = outMin+yprt[3] + yprt[0] + yprt[1] - yprt[2];
ESCOutput[3] = outMin+yprt[3] - yprt[0] - yprt[1] - yprt[2];
// Limit to range
int min,max;
int diff;
max = *std::max_element(ESCOutput, ESCOutput+4);
if (max > outMax) {
diff = outMax - max;
for (i=0;i<4;i++) {
ESCOutput[i] += diff;
}
for (i=0;i<4;i++) {
if (ESCOutput[i] < outMin) {
ESCOutput[i] = outMin;
}
}
}
min = *std::min_element(ESCOutput, ESCOutput+4);
if (min < outMin) {
diff = outMin - min;
for (i=0;i<4;i++) {
ESCOutput[i] += diff;
}
for (i=0;i<4;i++) {
if (ESCOutput[i] > outMax) {
ESCOutput[i] = outMax;
}
}
}
// Sanity checker
for (i=0;i<4;i++) {
if (ESCOutput[i] - oldESC[i] > sanity[i]) {
ESCOutput[i] = oldESC[i] + sanity[i];
}
if (ESCOutput[i] - oldESC[i] < -sanity[i]) {
ESCOutput[i] = oldESC[i] - sanity[i];
}
oldESC[i] = ESCOutput[i];
}
// Output
for (i=0;i<4;i++) {
gpioPWM(ESC[i], ESCOutput[i]);
}
}