Try and fix random disconnects

2026-04-17 07:40:08 +02:00
parent 35081044de
commit a69c0b97e9
2 changed files with 42 additions and 0 deletions
--- a/toid_control/src/toid_control.cpp
+++ b/toid_control/src/toid_control.cpp
@@ -82,6 +82,15 @@ hardware_interface::return_type toid::StepperInterface::write(const rclcpp::Time
        boost::system::error_code ec;
        ec = serial_port_.close(ec);
        ec = serial_port_.open(serial_port_name_, ec);
+        if(!ec.failed()) {
+            try {
+                for(int i = 0; i < 64; i++) {
+                    asio::write(serial_port_, asio::buffer("\x00"));
+                }
+            } catch(const std::runtime_error& err) {
+                
+            }
+        }
    }
    return hardware_interface::return_type::OK;
 }