Skip to content

Commit de50089

Browse files
brentrubrentru
committed
Implement - ServoAdd, ServoAdded API
1 parent a6d8755 commit de50089

File tree

6 files changed

+197
-73
lines changed

6 files changed

+197
-73
lines changed

‎src/components/servo/controller.cpp‎

Lines changed: 39 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -20,17 +20,16 @@
2020
*/
2121
/**************************************************************************/
2222
ServoController::ServoController() {
23-
23+
_servo_model = new ServoModel();
24+
_active_servo_pins = 0;
2425
}
2526

2627
/**************************************************************************/
2728
/*!
2829
@brief Destructor
2930
*/
3031
/**************************************************************************/
31-
ServoController::~ServoController() {
32-
33-
}
32+
ServoController::~ServoController() {}
3433

3534
/**************************************************************************/
3635
/*!
@@ -41,7 +40,40 @@ ServoController::~ServoController() {
4140
*/
4241
/**************************************************************************/
4342
bool ServoController::Handle_Servo_Add(pb_istream_t *stream) {
44-
43+
if (!_servo_model->DecodeServoAdd(stream)) {
44+
WS_DEBUG_PRINTLN("[servo] Error: Failed to decode ServoAdd message!");
45+
return false;
46+
}
47+
48+
wippersnapper_servo_ServoAdd *msg_add = _servo_model->GetServoAddMsg();
49+
uint8_t pin = atoi(msg_add->servo_pin + 1);
50+
_servo_hardware[_active_servo_pins] = new ServoHardware(
51+
pin, (int)msg_add->min_pulse_width, (int)msg_add->max_pulse_width,
52+
(int)msg_add->servo_freq);
53+
// Attempt to attach the servo to the pin
54+
bool did_attach = false;
55+
did_attach = _servo_hardware[_active_servo_pins]->ServoAttach();
56+
57+
// Write the default minimum to a servo
58+
if (!did_attach) {
59+
_servo_hardware[_active_servo_pins]->ServoWrite(MIN_SERVO_PULSE_WIDTH);
60+
WS_DEBUG_PRINT("[servo] Servo attached to pin: ");
61+
WS_DEBUG_PRINT(msg_add->servo_pin);
62+
_active_servo_pins++;
63+
} else {
64+
WS_DEBUG_PRINTLN("[servo] Error: Failed to attach servo to pin!");
65+
delete _servo_hardware[_active_servo_pins];
66+
_servo_hardware[_active_servo_pins] = nullptr;
67+
}
68+
69+
// Publish ServoAdded message to IO
70+
_servo_model->EncodeServoAdded(msg_add->servo_pin, did_attach);
71+
if (!WsV2.PublishSignal(wippersnapper_signal_DeviceToBroker_servo_added_tag,
72+
_servo_model->GetServoAddedMsg())) {
73+
WS_DEBUG_PRINTLN("[servo] Error: Failed publishing a ServoAdded message!");
74+
return false;
75+
}
76+
return true;
4577
}
4678

4779
/**************************************************************************/
@@ -52,9 +84,7 @@ bool ServoController::Handle_Servo_Add(pb_istream_t *stream) {
5284
@returns True if successful, False otherwise
5385
*/
5486
/**************************************************************************/
55-
bool ServoController::Handle_Servo_Write(pb_istream_t *stream) {
56-
57-
}
87+
bool ServoController::Handle_Servo_Write(pb_istream_t *stream) {}
5888

5989
/**************************************************************************/
6090
/*!
@@ -64,6 +94,4 @@ bool ServoController::Handle_Servo_Write(pb_istream_t *stream) {
6494
@returns True if successful, False otherwise
6595
*/
6696
/**************************************************************************/
67-
bool ServoController::Handle_Servo_Remove(pb_istream_t *stream) {
68-
69-
}
97+
bool ServoController::Handle_Servo_Remove(pb_istream_t *stream) {}

‎src/components/servo/controller.h‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,9 @@
1919
#include "model.h"
2020

2121
#ifdef ARDUINO_ARCH_RP2040
22-
#define MAX_SERVOS 8 ///< Maximum number of servo objects for RP2040, https://arduino-pico.readthedocs.io/en/latest/servo.html
22+
#define MAX_SERVOS \
23+
8 ///< Maximum number of servo objects for RP2040,
24+
///< https://arduino-pico.readthedocs.io/en/latest/servo.html
2325
#else
2426
#define MAX_SERVOS 16 ///< Maximum number of servo objects
2527
#endif
@@ -43,9 +45,11 @@ class ServoController {
4345
bool Handle_Servo_Add(pb_istream_t *stream);
4446
bool Handle_Servo_Write(pb_istream_t *stream);
4547
bool Handle_Servo_Remove(pb_istream_t *stream);
48+
4649
private:
4750
ServoModel *_servo_model;
4851
ServoHardware *_servo_hardware[MAX_SERVOS] = {nullptr};
52+
int _active_servo_pins; ///< Number of active servo pins
4953
};
5054
extern Wippersnapper_V2 WsV2; ///< Wippersnapper V2 instance
5155
#endif // WS_SERVO_CONTROLLER_H

‎src/components/servo/hardware.cpp‎

Lines changed: 76 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -16,82 +16,111 @@
1616

1717
/**************************************************************************/
1818
/*!
19-
@brief Constructor
20-
*/
21-
/**************************************************************************/
22-
ServoHardware::ServoHardware() {
23-
24-
}
25-
26-
/**************************************************************************/
27-
/*!
28-
@brief Destructor
29-
*/
30-
/**************************************************************************/
31-
ServoHardware::~ServoHardware() {
32-
33-
}
34-
35-
/**************************************************************************/
36-
/*!
37-
@brief Attaches a pin to a servo
19+
@brief Constructs a ServoHardware object
3820
@param pin
39-
The pin to attach
40-
@param frequency
41-
The servo frequency (in Hz)
21+
The GPIO pin to attach the servo to
4222
@param min_pulse_width
43-
The minimum pulse width (in microseconds)
23+
The minimum pulse width, in microseconds
4424
@param max_pulse_width
45-
The maximum pulse width (in microseconds)
46-
@returns True if successful, False otherwise
25+
The maximum pulse width, in microseconds
26+
@param frequency
27+
The frequency of the PWM signal, in Hz (50Hz is sent from IO)
4728
*/
4829
/**************************************************************************/
49-
bool ServoHardware::AttachPin(uint8_t pin, uint32_t frequency, uint32_t min_pulse_width, uint32_t max_pulse_width) {
50-
30+
ServoHardware::ServoHardware(int pin, int min_pulse_width, int max_pulse_width,
31+
int frequency) {
32+
_pin = pin;
33+
_min_pulse_width = min_pulse_width;
34+
_max_pulse_width = max_pulse_width;
35+
_frequency = frequency;
36+
37+
#ifndef ARDUINO_ARCH_ESP32
38+
_servo = new Servo();
39+
#else
40+
_is_attached = false;
41+
#endif
5142
}
5243

5344
/**************************************************************************/
5445
/*!
55-
@brief Detaches a pin from a servo
56-
@returns True if successful, False otherwise
46+
@brief Destructor
5747
*/
5848
/**************************************************************************/
59-
bool ServoHardware::DetachPin() {
60-
61-
}
49+
ServoHardware::~ServoHardware() {}
6250

6351
/**************************************************************************/
6452
/*!
65-
@brief Writes a pulse width to the servo
66-
@param pulse_width
67-
The pulse width (in microseconds) to write
68-
@returns True if successful, False otherwise
53+
@brief Attempts to attach a servo to a GPIO pin.
54+
@returns true if successful, false otherwise.
6955
*/
7056
/**************************************************************************/
71-
bool ServoHardware::WritePulseWidth(uint32_t pulse_width) {
72-
57+
bool ServoHardware::ServoAttach() {
58+
uint16_t rc = 255;
59+
60+
// Attach the servo to the pin
61+
#ifdef ARDUINO_ARCH_ESP32
62+
if (!ledcAttach(_pin, _frequency, LEDC_TIMER_WIDTH)) {
63+
rc = 255;
64+
} else {
65+
rc = 1;
66+
_is_attached = true;
67+
}
68+
#else
69+
rc = _servo.attach(_pin, _min_pulse_width, _max_pulse_width);
70+
#endif
71+
72+
if (rc == 255) {
73+
WS_DEBUG_PRINT("[servo] Error: Failed to attach servo to pin: ");
74+
WS_DEBUG_PRINTLN(_pin);
75+
return false;
76+
}
77+
78+
return true;
7379
}
7480

7581
/**************************************************************************/
7682
/*!
77-
@brief Returns the pin number
78-
@returns The pin number
83+
@brief Writes a value to the servo pin
84+
@param value
85+
The value to write to the servo pin
7986
*/
8087
/**************************************************************************/
81-
uint8_t ServoHardware::GetPin() {
82-
88+
void ServoHardware::ServoWrite(int value) {
89+
#ifdef ARDUINO_ARCH_ESP32
90+
writeMicroseconds(value);
91+
#else
92+
_servo.writeMicroseconds(value);
93+
#endif
8394
}
8495

8596
#ifdef ARDUINO_ARCH_ESP32
8697
/**************************************************************************/
8798
/*!
88-
@brief Abstraction for ESP32's servo write API
99+
@brief Mocks writeMicroseconds() call in arduino/servo api for
100+
ESP32x's LEDC manager.
89101
@param value
90-
The value to write
91-
@returns True if successful, False otherwise
102+
The value to write to the servo pin.
92103
*/
93104
/**************************************************************************/
94-
bool ServoHardware::servoWrite(uint32_t value) {
95-
105+
void ServoHardware::writeMicroseconds(int value) {
106+
if (!_is_attached) {
107+
WS_DEBUG_PRINTLN("[servo] Error: Servo not attached!");
108+
return;
109+
}
110+
111+
// Clamp value to a valid pulse_width range
112+
if (value < _min_pulse_width)
113+
value = _min_pulse_width;
114+
if (value > _max_pulse_width)
115+
value = _max_pulse_width;
116+
117+
// Formula from ESP32Servo library
118+
// https://github.com/madhephaestus/ESP32Servo/blob/master/src/ESP32Servo.cpp
119+
// count = (pulse_high_width / (pulse_period/2**timer_width))
120+
// 50Hz servo = 20ms pulse_period
121+
uint32_t count =
122+
((double)value / ((double)20000 / (double)pow(2, LEDC_TIMER_WIDTH)));
123+
if (!ledcWrite(_pin, count))
124+
WS_DEBUG_PRINTLN("[servo] Error: Failed to write to servo pin!");
96125
}
97126
#endif

‎src/components/servo/hardware.h‎

Lines changed: 35 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,21 @@
1616
#define WS_SERVO_HARDWARE_H
1717
#include "Wippersnapper_V2.h"
1818

19+
#ifdef ARDUINO_ARCH_ESP32
20+
#include "esp32-hal-ledc.h"
21+
#include "esp_err.h"
22+
23+
#define LEDC_TIMER_WIDTH \
24+
12 ///< timer width to request from LEDC manager component, in bits (NOTE:
25+
///< While ESP32x can go up to 16 bit timer width, ESP32-S2 does not work
26+
///< at this resolution. So, for the purposes of keeping this library
27+
///< compatible with multiple ESP32x platforms, the timer width has been
28+
///< scaled down to 10 bits and the calculation adjusted accordingly)
29+
#else
30+
#include <Servo.h>
31+
#endif
32+
33+
#define MIN_SERVO_PULSE_WIDTH 500 ///< Default min. servo pulse width of 500uS
1934

2035
/**************************************************************************/
2136
/*!
@@ -24,8 +39,27 @@
2439
/**************************************************************************/
2540
class ServoHardware {
2641
public:
27-
ServoHardware();
42+
ServoHardware(int pin, int min_pulse_width, int max_pulse_width,
43+
int frequency);
2844
~ServoHardware();
45+
bool ServoAttach();
46+
void ServoWrite(int value);
47+
2948
private:
49+
#ifdef ARDUINO_ARCH_ESP32
50+
// Mocks Servo library API for ESP32x's LEDC manager
51+
// https://github.com/arduino-libraries/Servo/blob/master/src/Servo.h
52+
bool attached();
53+
void detach();
54+
void writeMicroseconds(int value);
55+
bool _is_attached;
56+
#endif
57+
#ifndef ARDUINO_ARCH_ESP32
58+
Servo *_servo = nullptr;
59+
#endif
60+
uint8_t _pin;
61+
int _max_pulse_width;
62+
int _min_pulse_width;
63+
int _frequency;
3064
};
3165
#endif // WS_SERVO_HARDWARE_H

0 commit comments

Comments
 (0)