Register HiWonder Servos during runtime to prevent Servo num mismatches

ArendJan · Jan 15, 2025 · f8c9b2d · f8c9b2d
1 parent 91d2ab4
commit f8c9b2d
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 4 deletions.
diff --git a/mirte_telemetrix_cpp/libs/tmx-cpp b/mirte_telemetrix_cpp/libs/tmx-cpp
diff --git a/mirte_telemetrix_cpp/src/modules/hiwonder/hiwonder_servo.cpp b/mirte_telemetrix_cpp/src/modules/hiwonder/hiwonder_servo.cpp
@@ -21,6 +21,12 @@ Hiwonder_servo::Hiwonder_servo(
                  "not found]",
                  this->servo_data->name.c_str(), this->servo_data->id);
 
+  if (this->bus_mod->register_servo_id(this->servo_data->id))
+    RCLCPP_ERROR(logger,
+                 "HiWonder Servo '%s' ID is out of range [Requesed ID %d, but "
+                 "range is 0-253]",
+                 this->servo_data->name.c_str(), this->servo_data->id);
+
   auto range = this->bus_mod->get_range(servo_data->id);
   assert(range.has_value());
   auto [lower, upper] = range.value();
@@ -252,4 +258,4 @@ void Hiwonder_servo::set_motor_speed_service_callback(
   // output: speed from -1000 to 1000
   int16_t speed = std::clamp(req->speed, -100, 100) * 10;
   res->status = this->bus_mod->motor_mode_write(this->servo_data->id, speed);
-}
+}
diff --git a/mirte_telemetrix_cpp/src/modules/hiwonder_module.cpp b/mirte_telemetrix_cpp/src/modules/hiwonder_module.cpp
@@ -28,8 +28,9 @@ HiWonderBus_module::HiWonderBus_module(
 
   // Create a list of ID's
   std::vector<uint8_t> servo_ids;
-  for (auto servo : this->data.servos)
-    servo_ids.push_back(servo->id);
+  // Don't pre-add ids since it can cause errors on missing servos
+  // for (auto servo : this->data.servos)
+  //   servo_ids.push_back(servo->id);
 
   this->bus = std::make_shared<tmx_cpp::HiwonderServo_module>(
       this->data.uart_port, this->data.rx_pin, this->data.tx_pin, servo_ids,
+5 −0		include/tmx_cpp/modules/HiwonderServo.hpp
+52 −1		src/modules/HiwonderServo.cpp