Implement - ServoWrite, boundary checks around handling the servohardware objects

Author: brentru

src/components/servo/controller.cpp

@@ -84,7 +84,33 @@ bool ServoController::Handle_Servo_Add(pb_istream_t *stream) {
     @returns True if successful, False otherwise
 */
 /**************************************************************************/
-bool ServoController::Handle_Servo_Write(pb_istream_t *stream) {}
+bool ServoController::Handle_Servo_Write(pb_istream_t *stream) {
+  if (!_servo_model->DecodeServoWrite(stream)) {
+    WS_DEBUG_PRINTLN("[servo] Error: Failed to decode ServoWrite message!");
+    return false;
+  }
+  wippersnapper_servo_ServoWrite *msg_write = _servo_model->GetServoWriteMsg();
+  uint8_t pin = atoi(msg_write->servo_pin + 1);
+  // find the pin by using getpin()
+  int servo_idx = -1;
+  for (int i = 0; i < _active_servo_pins; i++) {
+    if (_servo_hardware[i]->GetPin() == pin) {
+      servo_idx = i;
+      break;
+    }
+  }
+  if (servo_idx == -1) {
+    WS_DEBUG_PRINTLN("[servo] Error: Servo pin not found!");
+    return false;
+  }
+  // Write the pulse width to the servo
+  _servo_hardware[servo_idx]->ServoWrite(msg_write->pulse_width);
+  WS_DEBUG_PRINT("[servo] Set Pulse Width: ");
+  WS_DEBUG_PRINT(msg_write->pulse_width);
+  WS_DEBUG_PRINT(" mS on pin: ");
+  WS_DEBUG_PRINT(msg_write->servo_pin);
+  return true;
+}
 
 /**************************************************************************/
 /*!
@@ -94,4 +120,6 @@ bool ServoController::Handle_Servo_Write(pb_istream_t *stream) {}
     @returns True if successful, False otherwise
 */
 /**************************************************************************/
-bool ServoController::Handle_Servo_Remove(pb_istream_t *stream) {}
+bool ServoController::Handle_Servo_Remove(pb_istream_t *stream) {
+  // just delete the ServoHardware object, it'll deinit itself!
+}

src/components/servo/hardware.cpp

@@ -43,10 +43,38 @@ ServoHardware::ServoHardware(int pin, int min_pulse_width, int max_pulse_width,
 
 /**************************************************************************/
 /*!
-    @brief  Destructor
+    @brief  Detaches the servo from the pin and frees the pin for
+            other uses.
 */
 /**************************************************************************/
-ServoHardware::~ServoHardware() {}
+ServoHardware::~ServoHardware() {
+#ifdef ARDUINO_ARCH_ESP32
+  if (!_is_attached) {
+    WS_DEBUG_PRINTLN("[servo] Error: Failed to detach, servo not attached!");
+    return;
+  }
+  if (!ledcDetach(_pin)) {
+    WS_DEBUG_PRINTLN("[servo] Error: Failed to detach servo from pin!");
+    return;
+  }
+  _is_attached = false;
+#else
+  if (_servo == nullptr) {
+    WS_DEBUG_PRINTLN("[servo] Error: Failed to detach, servo not created!");
+    return;
+  }
+  if (!_servo->attached()) {
+    WS_DEBUG_PRINTLN("[servo] Error: Failed to detach, servo not attached!");
+    return;
+  }
+  _servo->detach();
+  delete _servo;
+  _servo = nullptr;
+#endif
+
+  WS_DEBUG_PRINT("[servo] Servo detached from pin ");
+  WS_DEBUG_PRINTLN(_pin);
+}
 
 /**************************************************************************/
 /*!
@@ -66,6 +94,10 @@ bool ServoHardware::ServoAttach() {
     _is_attached = true;
   }
 #else
+  if (_servo == nullptr) {
+    WS_DEBUG_PRINTLN("[servo] Error: Failed to detach, servo not created!");
+    return false;
+  }
   rc = _servo.attach(_pin, _min_pulse_width, _max_pulse_width);
 #endif
 
@@ -87,8 +119,16 @@ bool ServoHardware::ServoAttach() {
 /**************************************************************************/
 void ServoHardware::ServoWrite(int value) {
 #ifdef ARDUINO_ARCH_ESP32
+  if (!_is_attached) {
+    WS_DEBUG_PRINTLN("[servo] Error: Servo not attached!");
+    return;
+  }
   writeMicroseconds(value);
 #else
+  if (_servo == nullptr || !_servo->attached()) {
+    WS_DEBUG_PRINTLN("[servo] Error: Servo not attached!");
+    return;
+  }
   _servo.writeMicroseconds(value);
 #endif
 }
@@ -103,11 +143,6 @@ void ServoHardware::ServoWrite(int value) {
 */
 /**************************************************************************/
 void ServoHardware::writeMicroseconds(int value) {
-  if (!_is_attached) {
-    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;

src/components/servo/hardware.h

@@ -44,6 +44,7 @@ class ServoHardware {
   ~ServoHardware();
   bool ServoAttach();
   void ServoWrite(int value);
+  uint8_t GetPin() { return _pin; }
 
 private:
 #ifdef ARDUINO_ARCH_ESP32