Address brent PR

Author: brentru

src/components/servo/controller.cpp

@@ -53,13 +53,13 @@ ServoController::~ServoController() {
 */
 /**************************************************************************/
 bool ServoController::Handle_Servo_Add(pb_istream_t *stream) {
-  if (!_servo_model->DecodeServoAdd(stream)) {
-    WS_DEBUG_PRINTLN("[servo] Error: Failed to decode ServoAdd message!");
+  if (_active_servo_pins >= MAX_SERVOS) {
+    WS_DEBUG_PRINTLN("[servo] Error: Maximum number of servos reached!");
     return false;
   }
 
-  if (_active_servo_pins >= MAX_SERVOS) {
-    WS_DEBUG_PRINTLN("[servo] Error: Maximum number of servos reached!");
+  if (!_servo_model->DecodeServoAdd(stream)) {
+    WS_DEBUG_PRINTLN("[servo] Error: Failed to decode ServoAdd message!");
     return false;
   }
 
@@ -72,15 +72,16 @@ bool ServoController::Handle_Servo_Add(pb_istream_t *stream) {
   bool did_attach = false;
   did_attach = _servo_hardware[_active_servo_pins]->ServoAttach();
 
-  // Write the default minimum to a servo
+  // Write the pulse width to the servo
   if (did_attach) {
     _servo_hardware[_active_servo_pins]->ServoWrite(
         (int)msg_add->min_pulse_width);
     WS_DEBUG_PRINT("[servo] Servo attached to pin: ");
     WS_DEBUG_PRINTLN(msg_add->servo_pin);
     _active_servo_pins++;
   } else {
-    WS_DEBUG_PRINTLN("[servo] Error: Failed to attach servo to pin!");
+    WS_DEBUG_PRINT("[servo] Error: Failed to attach servo to pin !");
+    WS_DEBUG_PRINT(msg_add->servo_pin);
     delete _servo_hardware[_active_servo_pins];
     _servo_hardware[_active_servo_pins] = nullptr;
   }
@@ -129,6 +130,11 @@ bool ServoController::Handle_Servo_Write(pb_istream_t *stream) {
 */
 /**************************************************************************/
 bool ServoController::Handle_Servo_Remove(pb_istream_t *stream) {
+  if (_active_servo_pins <= 0) {
+    WS_DEBUG_PRINTLN("[servo] Error: No active servos!");
+    return false;
+  }
+
   if (!_servo_model->DecodeServoRemove(stream)) {
     WS_DEBUG_PRINTLN("[servo] Error: Failed to decode ServoRemove message!");
     return false;
@@ -142,11 +148,6 @@ bool ServoController::Handle_Servo_Remove(pb_istream_t *stream) {
     return false;
   }
 
-  if (_active_servo_pins <= 0) {
-    WS_DEBUG_PRINTLN("[servo] Error: No active servos!");
-    return false;
-  }
-
   // The destructor of ServoHardware will handle proper detachment
   delete _servo_hardware[servo_idx];
   _servo_hardware[servo_idx] = nullptr;

src/components/servo/hardware.cpp

@@ -86,12 +86,12 @@ bool ServoHardware::ServoDetach() {
 */
 /**************************************************************************/
 bool ServoHardware::ServoAttach() {
-  uint16_t rc = 255;
+  uint16_t rc;
 
 // Attach the servo to the pin
 #ifdef ARDUINO_ARCH_ESP32
   if (!ledcAttach(_pin, _frequency, LEDC_TIMER_WIDTH)) {
-    rc = 255;
+    rc = ERROR_SERVO_ATTACH;
   } else {
     WS_DEBUG_PRINTLN("[servo:hw:L99] Servo attached to pin");
     rc = 1;
@@ -105,7 +105,7 @@ bool ServoHardware::ServoAttach() {
   rc = _servo->attach(_pin, _min_pulse_width, _max_pulse_width);
 #endif
 
-  if (rc == 255) {
+  if (rc == ERROR_SERVO_ATTACH) {
     WS_DEBUG_PRINT("[servo] Error: Failed to attach servo to pin: ");
     WS_DEBUG_PRINTLN(_pin);
     return false;
@@ -122,6 +122,24 @@ bool ServoHardware::ServoAttach() {
 /**************************************************************************/
 uint8_t ServoHardware::GetPin() { return _pin; }
 
+/**************************************************************************/
+/*!
+    @brief  Clamps the pulse width to the min/max range
+    @param  value
+            The value to clamp
+    @returns The clamped value
+*/
+/**************************************************************************/
+int ServoHardware::ClampPulseWidth(int value) {
+  if (value < _min_pulse_width) {
+    value = _min_pulse_width;
+  }
+  if (value > _max_pulse_width) {
+    value = _max_pulse_width;
+  }
+  return value;
+}
+
 /**************************************************************************/
 /*!
     @brief  Writes a value to the servo pin
@@ -141,11 +159,7 @@ void ServoHardware::ServoWrite(int value) {
     WS_DEBUG_PRINTLN("[servo] Error: Servo not attached!");
     return;
   }
-  // Clamp value to a valid pulse_width range
-  if (value < _min_pulse_width)
-    value = _min_pulse_width;
-  if (value > _max_pulse_width)
-    value = _max_pulse_width;
+  value = ClampPulseWidth(value);
   _servo->writeMicroseconds(value);
   WS_DEBUG_PRINT("[servo] Set Pulse Width: ");
   WS_DEBUG_PRINT(value);
@@ -164,11 +178,8 @@ void ServoHardware::ServoWrite(int value) {
 */
 /**************************************************************************/
 void ServoHardware::writeMicroseconds(int value) {
-  // Clamp value to a valid pulse_width range
-  if (value < _min_pulse_width)
-    value = _min_pulse_width;
-  if (value > _max_pulse_width)
-    value = _max_pulse_width;
+  // Clamp the value to the min/max range
+  value = ClampPulseWidth(value);
 
   // Formula from ESP32Servo library
   // https://github.com/madhephaestus/ESP32Servo/blob/master/src/ESP32Servo.cpp

src/components/servo/hardware.h

@@ -26,7 +26,7 @@
 #include <Servo.h>
 #endif
 
-#define MIN_SERVO_PULSE_WIDTH 500 ///< Default min. servo pulse width of 500uS
+#define ERROR_SERVO_ATTACH 255 ///< Error code for servo attach failure
 
 /**************************************************************************/
 /*!
@@ -44,6 +44,7 @@ class ServoHardware {
 
 private:
   bool ServoDetach();
+  int ClampPulseWidth(int value);
 #ifdef ARDUINO_ARCH_ESP32
   // Mocks Servo library API for ESP32x's LEDC manager
   // https://github.com/arduino-libraries/Servo/blob/master/src/Servo.h