-
Notifications
You must be signed in to change notification settings - Fork 1
/
angular_servo.cpp
112 lines (97 loc) · 3.11 KB
/
angular_servo.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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
#include <iostream>
#include <csignal>
#include <cstdlib>
#include <functional>
#include <pigpiod_if2.h>
#include "angular_servo.hpp"
using namespace std;
Servo::Servo(int pi, int pin) {
__pi = pi;
__pin = pin;
int rc;
rc = set_mode(__pi, __pin, PI_OUTPUT);
if (rc < 0){
throw "Invalid GPIO pin or mode Error!";
}
// initializing to zero with pigpio is no input
rc = set_servo_pulsewidth(__pi, __pin, 0);
if (rc < 0) {
throw "Invalid user GPIO pin or pulsewidth Error!";
}
}
int Servo::getPulseWidth() {
int rc = get_servo_pulsewidth(__pi, __pin);
if (rc < 0) {
throw "Invalid user GPIO pin Error!";
}
return rc;
}
void Servo::setPulseWidth(int pulseWidth){
int rc = set_servo_pulsewidth(__pi, __pin, pulseWidth);
if (rc < 0) {
throw "Invalid user GPIO pin or pulsewidth Error!";
}
}
AngularServo::AngularServo(int pi, int pin, float minAngle, float maxAngle, int minPulseWidthUs, int maxPulseWidthUs) : Servo(pi, pin) {
__minAngle = minAngle;
__maxAngle = maxAngle;
__minPulseWidthUs = minPulseWidthUs;
__maxPulseWidthUs = maxPulseWidthUs;
}
void AngularServo::setAngle(float angle){
__angle = angle;
// make sure angle is within bounds
if (__angle < __minAngle){
__angle = __minAngle;
}
if (__angle > __maxAngle){
__angle = __maxAngle;
}
int pulseWidth = __minPulseWidthUs + (__angle - __minAngle) * (__maxPulseWidthUs - __minPulseWidthUs) / (__maxAngle - __minAngle);
setPulseWidth(pulseWidth);
}
int AngularServo::getAngle(){
int pulseWidth = getPulseWidth();
float angle = __minAngle + (pulseWidth - __minPulseWidthUs) * (__maxAngle - __minAngle) / (__maxPulseWidthUs - __minPulseWidthUs);
return angle;
}
bool isPigpiodRunning() {
int result = system("pgrep pigpiod");
if (result == 0) {
// pigpiod daemon is running
return true;
} else {
// pigpiod daemon is not running
return false;
}
}
void killPigpiod() {
if (isPigpiodRunning()) {
int result = system("sudo killall pigpiod -q");
if (result == 0) {
// Successfully killed the pigpiod daemon
cout << "pigpiod daemon killed successfully" << endl;
} else {
// An error occurred while trying to kill the pigpiod daemon
cerr << "Error killing pigpiod daemon. Return code: " << result << endl;
}
} else {
cout << "pigpiod daemon is not running" << endl;
}
}
void startPigpiod() {
if (!isPigpiodRunning()) {
cout << "pigpio daemon not running. attempting to start it." << endl;
system("sudo systemctl start pigpiod");
sleep(1);
if (isPigpiodRunning()) {
// Successfully started the pigpiod daemon
cout << "pigpiod daemon started successfully" << endl;
} else {
// An error occurred while trying to start the pigpiod daemon
cerr << "Error starting pigpiod daemon." << endl;
}
} else {
cout << "pigpiod daemon is already running" << endl;
}
}