Skip to content

Commit e4ae80a

Browse files
brentrubrentru
committed
Implement - ServoRemove, fix bugs
1 parent 4cc35be commit e4ae80a

File tree

5 files changed

+98
-31
lines changed

5 files changed

+98
-31
lines changed

‎src/components/servo/controller.cpp‎

Lines changed: 43 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -109,13 +109,7 @@ bool ServoController::Handle_Servo_Write(pb_istream_t *stream) {
109109
}
110110
wippersnapper_servo_ServoWrite *msg_write = _servo_model->GetServoWriteMsg();
111111
uint8_t pin = atoi(msg_write->servo_pin + 1);
112-
int servo_idx = -1;
113-
for (int i = 0; i < _active_servo_pins; i++) {
114-
if (_servo_hardware[i]->GetPin() == pin) {
115-
servo_idx = i;
116-
break;
117-
}
118-
}
112+
int servo_idx = GetServoIndex(pin);
119113
if (servo_idx == -1) {
120114
WS_DEBUG_PRINTLN("[servo] Error: Servo pin not found!");
121115
return false;
@@ -138,5 +132,46 @@ bool ServoController::Handle_Servo_Write(pb_istream_t *stream) {
138132
*/
139133
/**************************************************************************/
140134
bool ServoController::Handle_Servo_Remove(pb_istream_t *stream) {
141-
// just delete the ServoHardware object, it'll deinit itself!
135+
if (!_servo_model->DecodeServoRemove(stream)) {
136+
WS_DEBUG_PRINTLN("[servo] Error: Failed to decode ServoRemove message!");
137+
return false;
138+
}
139+
wippersnapper_servo_ServoRemove *msg_remove =
140+
_servo_model->GetServoRemoveMsg();
141+
uint8_t pin = atoi(msg_remove->servo_pin + 1);
142+
int servo_idx = GetServoIndex(pin);
143+
if (servo_idx == -1) {
144+
WS_DEBUG_PRINTLN("[servo] Error: Servo pin not found!");
145+
return false;
146+
}
147+
148+
if (_active_servo_pins <= 0) {
149+
WS_DEBUG_PRINTLN("[servo] Error: No active servos!");
150+
return false;
151+
}
152+
153+
// The destructor of ServoHardware will handle proper detachment
154+
delete _servo_hardware[servo_idx];
155+
_servo_hardware[servo_idx] = nullptr;
156+
157+
// Shift _active_servo_pins down
158+
for (int i = servo_idx; i < _active_servo_pins - 1; i++) {
159+
_servo_hardware[i] = _servo_hardware[i + 1];
160+
_servo_hardware[i + 1] = nullptr;
161+
}
162+
_servo_hardware[_active_servo_pins - 1] = nullptr;
163+
_active_servo_pins--;
164+
165+
WS_DEBUG_PRINT("[servo] Servo removed from pin: ");
166+
WS_DEBUG_PRINT(msg_remove->servo_pin);
167+
return true;
142168
}
169+
170+
int ServoController::GetServoIndex(uint8_t pin) {
171+
for (int i = 0; i < _active_servo_pins; i++) {
172+
if (_servo_hardware[i]->GetPin() == pin) {
173+
return i;
174+
}
175+
}
176+
return -1;
177+
}

‎src/components/servo/controller.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,6 @@
2525
#else
2626
#define MAX_SERVOS 16 ///< Maximum number of servo objects
2727
#endif
28-
#define MIN_SERVO_PULSE_WIDTH 500 ///< Default min. servo pulse width of 500uS
2928

3029
class Wippersnapper_V2; // Forward declaration
3130
class ServoModel; // Forward declaration
@@ -47,6 +46,7 @@ class ServoController {
4746
bool Handle_Servo_Remove(pb_istream_t *stream);
4847

4948
private:
49+
int GetServoIndex(uint8_t pin);
5050
ServoModel *_servo_model;
5151
ServoHardware *_servo_hardware[MAX_SERVOS] = {nullptr};
5252
int _active_servo_pins; ///< Number of active servo pins

‎src/components/servo/hardware.cpp‎

Lines changed: 52 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -48,32 +48,38 @@ ServoHardware::ServoHardware(int pin, int min_pulse_width, int max_pulse_width,
4848
*/
4949
/**************************************************************************/
5050
ServoHardware::~ServoHardware() {
51-
#ifdef ARDUINO_ARCH_ESP32
52-
if (!_is_attached) {
53-
WS_DEBUG_PRINTLN("[servo] Error: Failed to detach, servo not attached!");
54-
return;
51+
if (!ServoDetach()) {
52+
WS_DEBUG_PRINTLN("[servo] Error: Failed to detach servo!");
53+
} else {
54+
WS_DEBUG_PRINT("[servo] Servo detached from pin: ");
55+
WS_DEBUG_PRINTLN(_pin);
5556
}
56-
if (!ledcDetach(_pin)) {
57-
WS_DEBUG_PRINTLN("[servo] Error: Failed to detach servo from pin!");
58-
return;
57+
}
58+
59+
/**************************************************************************/
60+
/*!
61+
@brief Detaches the servo from the pin and frees the pin for
62+
other uses.
63+
@returns true if successful, false otherwise
64+
*/
65+
/**************************************************************************/
66+
bool ServoHardware::ServoDetach() {
67+
#ifdef ARDUINO_ARCH_ESP32
68+
detach();
69+
if (attached()) {
70+
WS_DEBUG_PRINTLN("[servo]Error: Servo detach failure!");
71+
return false;
5972
}
60-
_is_attached = false;
6173
#else
62-
if (_servo == nullptr) {
63-
WS_DEBUG_PRINTLN("[servo] Error: Failed to detach, servo not created!");
64-
return;
65-
}
66-
if (!_servo->attached()) {
67-
WS_DEBUG_PRINTLN("[servo] Error: Failed to detach, servo not attached!");
68-
return;
74+
if (_servo == nullptr || !_servo->attached()) {
75+
WS_DEBUG_PRINTLN("[servo] Detach Error: Servo not attached!");
76+
return false;
6977
}
7078
_servo->detach();
7179
delete _servo;
7280
_servo = nullptr;
7381
#endif
74-
75-
WS_DEBUG_PRINT("[servo] Servo detached from pin ");
76-
WS_DEBUG_PRINTLN(_pin);
82+
return true;
7783
}
7884

7985
/**************************************************************************/
@@ -95,7 +101,7 @@ bool ServoHardware::ServoAttach() {
95101
}
96102
#else
97103
if (_servo == nullptr) {
98-
WS_DEBUG_PRINTLN("[servo] Error: Failed to detach, servo not created!");
104+
WS_DEBUG_PRINTLN("[servo] Attach Error: Servo not initialized!");
99105
return false;
100106
}
101107
rc = _servo->attach(_pin, _min_pulse_width, _max_pulse_width);
@@ -127,7 +133,7 @@ uint8_t ServoHardware::GetPin() { return _pin; }
127133
/**************************************************************************/
128134
void ServoHardware::ServoWrite(int value) {
129135
#ifdef ARDUINO_ARCH_ESP32
130-
if (!_is_attached) {
136+
if (!attached()) {
131137
WS_DEBUG_PRINTLN("[servo] Error: Servo not attached!");
132138
return;
133139
}
@@ -137,7 +143,12 @@ void ServoHardware::ServoWrite(int value) {
137143
WS_DEBUG_PRINTLN("[servo] Error: Servo not attached!");
138144
return;
139145
}
140-
_servo.writeMicroseconds(value);
146+
// Clamp value to a valid pulse_width range
147+
if (value < _min_pulse_width)
148+
value = _min_pulse_width;
149+
if (value > _max_pulse_width)
150+
value = _max_pulse_width;
151+
_servo->writeMicroseconds(value);
141152
#endif
142153
}
143154

@@ -166,4 +177,24 @@ void ServoHardware::writeMicroseconds(int value) {
166177
if (!ledcWrite(_pin, count))
167178
WS_DEBUG_PRINTLN("[servo] Error: Failed to write to servo pin!");
168179
}
180+
181+
/**************************************************************************/
182+
/*!
183+
@brief Detaches the servo from the LEDC manager and frees the pin for
184+
other uses.
185+
*/
186+
/**************************************************************************/
187+
void ServoHardware::detach() {
188+
if (!attached())
189+
return;
190+
_is_attached = ledcDetach(_pin);
191+
}
192+
193+
/**************************************************************************/
194+
/*!
195+
@brief Returns true if the servo is attached to the pin
196+
@returns true if the servo is attached to the pin, false otherwise
197+
*/
198+
/**************************************************************************/
199+
bool ServoHardware::attached() { return _is_attached; }
169200
#endif

‎src/components/servo/hardware.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@ class ServoHardware {
4343
int frequency);
4444
~ServoHardware();
4545
bool ServoAttach();
46+
bool ServoDetach();
4647
void ServoWrite(int value);
4748
uint8_t GetPin();
4849

‎src/components/servo/model.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@ bool ServoModel::EncodeServoAdded(char *pin_name, bool did_attach) {
7878
memset(&_msg_servo_added, 0, sizeof(_msg_servo_added));
7979
_msg_servo_added.attach_success = did_attach;
8080
strncpy(_msg_servo_added.servo_pin, pin_name,
81-
sizeof(_msg_servo_added.servo_pin));
81+
sizeof(_msg_servo_added.servo_pin) - 1);
8282
// Encode it!
8383
size_t sz_msg;
8484
if (!pb_get_encoded_size(&sz_msg, wippersnapper_servo_ServoAdded_fields,

0 commit comments

Comments
 (0)