From 66e0a88e0dbe3f0824018a2f6cab7d0efcbe02e6 Mon Sep 17 00:00:00 2001
From: jwchoi00 <jwchoi0017@gmail.com>
Date: Tue, 29 Aug 2023 03:25:01 +0900
Subject: [PATCH] motor direction change

---
 .../config/diffbot_controllers.yaml                | 14 +++++++-------
 .../launch/odrive_diffbot.launch.py                |  2 +-
 .../urdf/odrive.ros2_control.xacro                 |  4 ++--
 odrive_demo_description/urdf/odrive.urdf.xacro     |  4 ++--
 .../src/odrive_hardware_interface.cpp              |  6 +++++-
 5 files changed, 17 insertions(+), 13 deletions(-)

diff --git a/odrive_demo_bringup/config/diffbot_controllers.yaml b/odrive_demo_bringup/config/diffbot_controllers.yaml
index cb636c9..5466d9f 100644
--- a/odrive_demo_bringup/config/diffbot_controllers.yaml
+++ b/odrive_demo_bringup/config/diffbot_controllers.yaml
@@ -40,18 +40,18 @@ diffbot_base_controller:
     linear.x.has_velocity_limits: true
     linear.x.has_acceleration_limits: true
     linear.x.has_jerk_limits: false
-    linear.x.max_velocity: 1.0
-    linear.x.min_velocity: -1.0
-    linear.x.max_acceleration: 1.0
+    linear.x.max_velocity: 3.0
+    linear.x.min_velocity: -3.0
+    linear.x.max_acceleration: 3.0
     linear.x.max_jerk: 0.0
     linear.x.min_jerk: 0.0
 
     angular.z.has_velocity_limits: true
     angular.z.has_acceleration_limits: true
     angular.z.has_jerk_limits: false
-    angular.z.max_velocity: 1.0
-    angular.z.min_velocity: -1.0
-    angular.z.max_acceleration: 1.0
-    angular.z.min_acceleration: -1.0
+    angular.z.max_velocity: 60.0
+    angular.z.min_velocity: -60.0
+    angular.z.max_acceleration: 60.0
+    angular.z.min_acceleration: -60.0
     angular.z.max_jerk: 0.0
     angular.z.min_jerk: 0.0
diff --git a/odrive_demo_bringup/launch/odrive_diffbot.launch.py b/odrive_demo_bringup/launch/odrive_diffbot.launch.py
index cd39928..1c66711 100644
--- a/odrive_demo_bringup/launch/odrive_diffbot.launch.py
+++ b/odrive_demo_bringup/launch/odrive_diffbot.launch.py
@@ -88,5 +88,5 @@ def generate_launch_description():
         robot_state_pub_node,
         joint_state_broadcaster_spawner,
         robot_controller_spawner,
-        rviz_node
+        rviz_node,
     ])
diff --git a/odrive_demo_description/urdf/odrive.ros2_control.xacro b/odrive_demo_description/urdf/odrive.ros2_control.xacro
index e3007e3..79206c1 100644
--- a/odrive_demo_description/urdf/odrive.ros2_control.xacro
+++ b/odrive_demo_description/urdf/odrive.ros2_control.xacro
@@ -18,7 +18,7 @@
           <param name="serial_number">${serial_number}</param>
           <param name="axis">0</param>
           <param name="enable_watchdog">1</param>
-          <param name="watchdog_timeout">0.1</param>
+          <param name="watchdog_timeout">0.5</param>
         </joint>
       </xacro:if>
 
@@ -27,7 +27,7 @@
           <param name="serial_number">${serial_number}</param>
           <param name="axis">1</param>
           <param name="enable_watchdog">1</param>
-          <param name="watchdog_timeout">0.1</param>
+          <param name="watchdog_timeout">0.5</param>
         </joint>
       </xacro:if>
     </ros2_control>
diff --git a/odrive_demo_description/urdf/odrive.urdf.xacro b/odrive_demo_description/urdf/odrive.urdf.xacro
index c5a3cca..b9ce75c 100644
--- a/odrive_demo_description/urdf/odrive.urdf.xacro
+++ b/odrive_demo_description/urdf/odrive.urdf.xacro
@@ -13,7 +13,7 @@
     <joint name="joint0" type="continuous">
       <parent link="world"/>
       <child link="link0"/>
-      <axis xyz="0 0 1"/>
+      <axis xyz="1.0 0.0 0.0"/>
     </joint>
   </xacro:if>
 
@@ -22,7 +22,7 @@
     <joint name="joint1" type="continuous">
       <parent link="world"/>
       <child link="link1"/>
-      <axis xyz="0 0 1"/>
+      <axis xyz="1.0 0.0 0.0"/>
     </joint>
   </xacro:if>
 
diff --git a/odrive_hardware_interface/src/odrive_hardware_interface.cpp b/odrive_hardware_interface/src/odrive_hardware_interface.cpp
index 3a70657..771b676 100644
--- a/odrive_hardware_interface/src/odrive_hardware_interface.cpp
+++ b/odrive_hardware_interface/src/odrive_hardware_interface.cpp
@@ -344,7 +344,11 @@ return_type ODriveHardwareInterface::write()
           input_pos));
 
       case integration_level_t::VELOCITY:
-        input_vel = hw_commands_velocities_[i] / 2 / M_PI;
+        if (i == 0) {  // Only for joint1
+          input_vel = hw_commands_velocities_[i] / 2 / M_PI;  // Negative velocity for joint1
+        } else {
+          input_vel = -hw_commands_velocities_[i] / 2 / M_PI;   // Keep the same velocity for other joints
+        }
         CHECK(odrive->write(
           serial_numbers_[1][i], AXIS__CONTROLLER__INPUT_VEL + per_axis_offset * axes_[i],
           input_vel));