diff --git a/boards/nucleo_h563zi.overlay b/boards/nucleo_h563zi.overlay
index bf92017..50dda36 100644
--- a/boards/nucleo_h563zi.overlay
+++ b/boards/nucleo_h563zi.overlay
@@ -4,6 +4,8 @@
         led1 = &yellow_led_1;
         led2 = &green_led_1;
 
+        rotenc0 = &as5600;
+        stepper0 = &stepper0;
         servo0 = &pwm_servo0;
     };
 
@@ -17,6 +19,19 @@
 		};
 	};
 
+    stepper0: tmc2209_motor {
+        compatible = "adi,tmc2209";
+
+        en-gpios = <&gpioe 4 GPIO_ACTIVE_LOW>;
+        msx-gpios = <&gpiod 0 GPIO_ACTIVE_HIGH>,
+                    <&gpiod 1 GPIO_ACTIVE_HIGH>;
+        step-gpios = <&gpioe 6 GPIO_ACTIVE_HIGH>;
+        dir-gpios = <&gpioe 5 GPIO_ACTIVE_HIGH>;
+        //dual-edge-step;
+        micro-step-res = <64>;
+        counter = <&counter6>;
+        status = "okay";
+    };
 };
 
 &uart4 {
@@ -38,6 +53,15 @@
     status = "okay";
 };
 
+&timers6 {
+    status = "okay";
+    st,prescaler = <25>;
+
+    counter6: counter {
+        status = "okay";
+    };
+};
+
 &timers5 {
     status = "okay";
     st,prescaler = <7>;
@@ -56,4 +80,16 @@
         pinctrl-0 = <&tim4_ch3_pb8>;
         pinctrl-names = "default";
     };
-};
\ No newline at end of file
+};
+
+&i2c2 {
+    status = "okay";
+    pinctrl-0 = <&i2c2_scl_pf1 &i2c2_sda_pf0>;
+	pinctrl-names = "default";
+
+    as5600: as5600@36 {
+        compatible = "ams,as5600";
+        reg = <0x36>;   // I2C address of AS5600
+        label = "AS5600";
+    };
+};
diff --git a/prj.conf b/prj.conf
index d872671..c236dce 100644
--- a/prj.conf
+++ b/prj.conf
@@ -46,5 +46,16 @@ CONFIG_CAN=y
 CONFIG_CAN_FD_MODE=y
 CONFIG_CAN_STM32_FDCAN=y
 
+# sensor + i2c
+CONFIG_SENSOR=y
+CONFIG_I2C=y
+CONFIG_I2C_STM32=y
+
+# stepper
+CONFIG_STEPPER=y
+CONFIG_STEPPER_ADI_TMC2209=y
+CONFIG_STEPPER_ADI_TMC=y
+CONFIG_STEPPER_ADI_TMC_UART=y
+
 # debug
 CONFIG_DEBUG_OPTIMIZATIONS=n
\ No newline at end of file
diff --git a/src/devices.cpp b/src/devices.cpp
index abff511..216417e 100644
--- a/src/devices.cpp
+++ b/src/devices.cpp
@@ -27,6 +27,14 @@ bool verify_are_devices_available(void) {
         printk("Counter 5 not ready\n");
         devices_ready = false;
     }
+    if (!device_is_ready(rotenc0)) {
+        printk("Rotory encoder 0 not ready\n");
+        devices_ready = false;
+    }
+    if (!device_is_ready(stepper0)) {
+        printk("Stepper 0 not ready\n");
+        devices_ready = false;
+    }
 
     return devices_ready;
 }
\ No newline at end of file
diff --git a/src/devices.hpp b/src/devices.hpp
index 7b931bd..1fab3bc 100644
--- a/src/devices.hpp
+++ b/src/devices.hpp
@@ -7,6 +7,7 @@
 #include <zephyr/devicetree.h>
 #include <zephyr/drivers/gpio.h>
 #include <zephyr/drivers/pwm.h>
+#include <zephyr/drivers/stepper.h>
 #else
 #include <device.h>
 #include <devicetree.h>
@@ -17,6 +18,8 @@ static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
 static const struct gpio_dt_spec led1 = GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios);
 static const struct gpio_dt_spec led2 = GPIO_DT_SPEC_GET(DT_ALIAS(led2), gpios);
 
+static const struct device *rotenc0 = DEVICE_DT_GET(DT_ALIAS(rotenc0));
+static const struct device *stepper0 = DEVICE_DT_GET(DT_ALIAS(stepper0));
 static const struct pwm_dt_spec servo0 = PWM_DT_SPEC_GET(DT_ALIAS(servo0));
 static const struct device *counter5 = DEVICE_DT_GET(DT_NODELABEL(counter5));
 
diff --git a/src/servo_controller/servo_controller.cpp b/src/servo_controller/servo_controller.cpp
index e29fc10..e8b656d 100644
--- a/src/servo_controller/servo_controller.cpp
+++ b/src/servo_controller/servo_controller.cpp
@@ -1,9 +1,13 @@
 #include "servo_controller.hpp"
 #include "devices.hpp"
+#include "rcl/types.h"
 #include "rcl_util.hpp"
+#include "zephyr/sys/printk.h"
 
 #include <std_msgs/msg/float32.h>
 #include <zephyr/drivers/counter.h>
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/drivers/stepper.h>
 #include <zephyr/kernel.h>
 #include <rmw_microros/rmw_microros.h>
 
@@ -57,6 +61,12 @@ void counter_cb(const struct device *dev, uint8_t chan_id, uint32_t ticks, void
 
 rcl_ret_t ServoController::setup(rclc_support_t* support, rclc_executor_t* executor)
 {
+    uint32_t speed_hz = 6400;  // desired step frequency in Hz
+    uint32_t interval_ns = 1000000000U / speed_hz;  // ns per step
+    stepper_set_microstep_interval(stepper0, interval_ns);
+    stepper_enable(stepper0);
+    stepper_run(stepper0, STEPPER_DIRECTION_POSITIVE);
+
     RCL_RETURN_ON_ERROR(rclc_node_init_default(
         &ServoController::node, 
         "servo_controller",
@@ -164,8 +174,21 @@ void ServoController::servo_pid() {
 rcl_ret_t ServoController::publish() {
     std_msgs__msg__Float32 msg;
     std_msgs__msg__Float32__init(&msg);
+    struct sensor_value val;
 
-    msg.data = ServoController::current_angle;
+    if (sensor_sample_fetch(rotenc0)) {
+        printk("failed to fetch sensor value\n");
+        return RCL_RET_OK;
+    }
+    
+    if (sensor_channel_get(rotenc0, SENSOR_CHAN_ROTATION, &val)) {
+        printk("failed to get sensor value\n");
+        return RCL_RET_OK;
+    }   
+    printk("angle: %d.%d\n", val.val1, val.val2);
+
+    //msg.data = ServoController::current_angle;
+    msg.data = (float)val.val1 + (float)val.val1 / 1000000;
 
     return rcl_publish(&ServoController::servo0_publisher, &msg, NULL);
 }