修正了骨折复位机器人接口

src/robot/fracture_robot/fracture_robot_interface.cpp

@@ -47,7 +47,7 @@ namespace sophiar {
         static constexpr auto robot_endian = boost::endian::order::little;
         static constexpr auto default_max_velocity = 5.0; // mm/s
         static constexpr auto default_controller_gain = 5.0;
-        static constexpr size_t control_packet_length = 4 * sizeof(int32_t) + 2;
+        static constexpr size_t control_packet_length = 6 * sizeof(int32_t) + 2;
         static constexpr uint16_t control_packet_tail = 0x7fff;
 
         struct screw_info {
@@ -105,8 +105,8 @@ namespace sophiar {
 
         static void encode_arr(const array6_obj::value_type &vec_val,
                                control_value_type &arr_val) {
-            static constexpr int base_freq = 100000; // Hz
-            static constexpr auto dis_per_clock = 0.006; // mm
+            static constexpr int base_freq = 1000000; // Hz
+            static constexpr auto dis_per_clock = 0.00625; // mm
             static constexpr auto minimum_velocity = 0.05; // mm/s, lower than this will cause stop
 
             for (int i = 0; i < 6; ++i) {

tests/data/fracture_robot_interface_config.json

@@ -2,7 +2,15 @@
   "controller_port": 5277,
   "variable_list": [
     {
-      "name": "ur_joint_target_q",
+      "name": "robot_control_mode",
+      "type": "u64int_obj"
+    },
+    {
+      "name": "robot_is_moving",
+      "type": "bool_obj"
+    },
+    {
+      "name": "motor_velocity",
       "type": "array6_obj"
     }
   ],
@@ -11,7 +19,7 @@
       "type": "fracture_robot_interface",
       "name": "frac_robot",
       "init_config": {
-        "com_port": "/dev/ttyACM1",
+        "com_port": "COM4",
         "screw": {
           "top": {
             "origin": [
@@ -43,7 +51,7 @@
         "input_config": {
           "max_velocity": 5.0,
           "controller_gain": 3.0,
-          "control_mode": "cartesian",
+          "control_mode": "direct",
           "current_tcp": "tcp_in_robot_base",
           "target_tcp": "target_tcp_in_robot_base",
           "target_motor_velocity": "motor_velocity"

tests/robot/fracture_robot_interface.cpp

@@ -0,0 +1,47 @@
+#define BOOST_TEST_DYN_LINK
+
+#include "core/basic_obj_types.hpp"
+#include "core/global_defs.h"
+#include "core/sophiar_pool.h"
+#include "utility/debug_utility.hpp"
+#include "utility/variable_helper.hpp"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/test/unit_test.hpp>
+
+#include <spdlog/spdlog.h>
+
+#include <fstream>
+#include <numbers>
+
+using namespace sophiar;
+using namespace std::chrono_literals;
+
+using boost::asio::co_spawn;
+using boost::asio::awaitable;
+using boost::asio::detached;
+
+awaitable<void> test_fracture_robot_direct_control() {
+    auto move_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
+    auto target_index = REQUIRE_VARIABLE(array6_obj, "motor_velocity");
+    co_await coro_wait_for_not_empty<bool_obj>(move_index);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, move_index)->value == false);
+    for (int i = 0; i < 100; ++i) {
+        auto motor_speed = array6_obj::new_instance();
+        motor_speed->value.fill(5);
+        UPDATE_VARIABLE(array6_obj, target_index, motor_speed);
+        co_await coro_sleep(20ms);
+    }
+    co_return;
+}
+
+BOOST_AUTO_TEST_CASE(test_fracture_robot_interface) {
+    spdlog::set_level(spdlog::level::trace);
+    std::ifstream config_file("data/fracture_robot_interface_config.json");
+    BOOST_TEST(config_file.is_open());
+    auto config = nlohmann::json::parse(config_file);
+    BOOST_TEST(initialize(config));
+    co_spawn(*global_context, test_fracture_robot_direct_control(), detached);
+    global_context->run();
+}