diff --git a/auto_driver_description/controllers/controllers.yaml b/auto_driver_description/controllers/controllers.yaml
new file mode 100644
index 0000000..86bdd80
--- /dev/null
+++ b/auto_driver_description/controllers/controllers.yaml
@@ -0,0 +1,30 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+velocity_controller:
+  ros__parameters:
+    joints:
+      - yaw_joint
+      - pitch_joint
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - yaw_joint
+      - pitch_joint
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
diff --git a/auto_driver_description/launch/auto_driver.launch.py b/auto_driver_description/launch/auto_driver.launch.py
index 74d4d13..91ac718 100644
--- a/auto_driver_description/launch/auto_driver.launch.py
+++ b/auto_driver_description/launch/auto_driver.launch.py
@@ -31,40 +31,46 @@ def generate_launch_description():
         package_name), "urdf", robot_name + ".urdf.xacro")
     robot_description_config = xacro.process_file(robot_description)
 
-    # controller_config = os.path.join(
-    #     get_package_share_directory(
-    #         package_name), "controllers", "controllers.yaml"
-    # )
+    controller_config = os.path.join(
+        get_package_share_directory(
+            package_name), "controllers", "controllers.yaml"
+    )
 
     return LaunchDescription([
-        # Node(
-        #     package="controller_manager",
-        #     executable="ros2_control_node",
-        #     parameters=[
-        #         {"robot_description": robot_description_config.toxml()}, controller_config],
-        #     output="screen",
-        # ),
+        Node(
+            package='rostackchan_example',
+            executable='random_move',
+            name='random_move',
+            output='screen'
+        ),
+        Node(
+            package="controller_manager",
+            executable="ros2_control_node",
+            parameters=[
+                {"robot_description": robot_description_config.toxml()}, controller_config],
+            output="screen",
+        ),
 
-        # Node(
-        #     package="controller_manager",
-        #     executable="spawner",
-        #     arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
-        #     output="screen",
-        # ),
+        Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+            output="screen",
+        ),
 
-        # Node(
-        #     package="controller_manager",
-        #     executable="spawner",
-        #     arguments=["velocity_controller", "-c", "/controller_manager"],
-        #     output="screen",
-        # ),
+        Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=["velocity_controller", "-c", "/controller_manager"],
+            output="screen",
+        ),
 
-        # Node(
-        #     package="controller_manager",
-        #     executable="spawner",
-        #     arguments=["joint_trajectory_controller", "-c", "/controller_manager"],
-        #     output="screen",
-        # ),
+        Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=["joint_trajectory_controller", "-c", "/controller_manager"],
+            output="screen",
+        ),
 
         Node(
             package="robot_state_publisher",
diff --git a/auto_driver_description/urdf/auto_driver.ros2_control.xacro b/auto_driver_description/urdf/auto_driver.ros2_control.xacro
new file mode 100644
index 0000000..b7d18db
--- /dev/null
+++ b/auto_driver_description/urdf/auto_driver.ros2_control.xacro
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="auto_driver_ros2_control" params="name">
+
+    <ros2_control name="${name}" type="system">
+      <hardware>
+        <plugin>feetech_scs_hardware/FeetechScsHardware</plugin>
+        <param name="usb_port">/dev/ttyUSB0</param>
+        <param name="baud_rate">1000000</param>
+      </hardware>
+      <joint name="yaw_joint">
+        <param name="id">1</param>
+        <command_interface name="position">
+          <param name="min">0.0</param>
+          <param name="max">2.0</param>
+        </command_interface>
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="pitch_joint">
+        <param name="id">2</param>
+        <command_interface name="position">
+          <param name="min">1.4</param>
+          <param name="max">2.0</param>
+        </command_interface>
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+    </ros2_control>
+
+  </xacro:macro>
+
+</robot>
\ No newline at end of file
diff --git a/auto_driver_description/urdf/auto_driver.urdf.xacro b/auto_driver_description/urdf/auto_driver.urdf.xacro
index ce16183..60a1a17 100644
--- a/auto_driver_description/urdf/auto_driver.urdf.xacro
+++ b/auto_driver_description/urdf/auto_driver.urdf.xacro
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot name="auto_driver" xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:include filename="$(find auto_driver_description)/urdf/auto_driver.xacro" />
-  <!-- <xacro:include filename="$(find auto_driver_description)/urdf/auto_driver.ros2_control.xacro" /> -->
+  <xacro:include filename="$(find auto_driver_description)/urdf/auto_driver.ros2_control.xacro" />
 
   <link name="world"/>
 
@@ -9,5 +9,5 @@
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:auto_driver>
 
-  <!-- <xacro:auto_driver_ros2_control name="auto_driver_ros2_control" /> -->
+  <xacro:auto_driver_ros2_control name="auto_driver_ros2_control" />
 </robot>
\ No newline at end of file
diff --git a/auto_driver_description/urdf/auto_driver.xacro b/auto_driver_description/urdf/auto_driver.xacro
index 78e265d..56d7dc3 100644
--- a/auto_driver_description/urdf/auto_driver.xacro
+++ b/auto_driver_description/urdf/auto_driver.xacro
@@ -5,14 +5,14 @@
 <xacro:include filename="$(find auto_driver_description)/urdf/auto_driver.material.xacro" />
 
 <xacro:macro name="auto_driver" params="parent *origin">
-  <joint name="origin_to_link1" type="fixed">
+  <joint name="origin_to_base_frame" type="fixed">
     <xacro:insert_block name="origin" />
     <parent link="${parent}" />
-    <child link="link1" />
+    <child link="base_frame" />
     <axis xyz="0 0 1" />
   </joint>
 
-  <link name="link1">
+  <link name="base_frame">
     <visual>
       <origin xyz="0 0 0" rpy="0 -1.5708 0"/>
       <geometry>
@@ -39,16 +39,16 @@
 
   <link name="fake_1_to_2" />
   <joint name="fake_1_to_2_joint" type="fixed">
-    <parent link="link1" />
+    <parent link="base_frame" />
     <child link="fake_1_to_2" />
-    <origin xyz="0 0 0.19" rpy="0 0 0" />
+    <origin xyz="-0.05 0 0.18" rpy="0 0 0" />
   </joint>
 
 
-  <joint name="yaw_joint" type="fixed">
+  <joint name="yaw_joint" type="revolute">
     <parent link="fake_1_to_2"/>
     <child link="yaw_link"/>
-    <origin xyz="0.0 0.0 0.005" rpy="0 0 -1.57079632679"/>
+    <origin xyz="0 0 0.005" rpy="0 0 0"/>
     <axis xyz="0 0 -1"/>
     <limit velocity="4.8" effort="1" lower="0.0" upper="2.0" />
   </joint>
@@ -82,7 +82,7 @@
   <joint name="fake_2_to_3_joint" type="fixed">
     <parent link="yaw_link" />
     <child link="fake_2_to_3" />
-    <origin xyz="0 0 0" rpy="0 0 0" />
+    <origin xyz="0 0 0" rpy="-1.57079632679 0 0" />
   </joint>
 
   <link name="pitch_link">
@@ -110,11 +110,11 @@
     </inertial>
   </link>
 
-  <joint name="pitch_joint" type="fixed">
+  <joint name="pitch_joint" type="revolute">
     <parent link="fake_2_to_3"/>
     <child link="pitch_link"/>
     <origin xyz="0.0 0.0 0.005" rpy="0 0 0"/>
-    <axis xyz="0 0 -1"/>
+    <axis xyz="1 0 0"/>
     <limit velocity="4.8" effort="1" lower="0.0" upper="2.0" />
   </joint>