diff --git a/CMakeLists.txt b/CMakeLists.txt
index 67c2708..0d3fcb9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -148,7 +148,7 @@ set(TINYMOVR_SOURCES
 #   src/${PROJECT_NAME}/tinymovr_ros.cpp
 # )
 
-add_library(${PROJECT_NAME}_joint_iface ${TINYMOVR_SOURCES} src/socketcan_cpp.cpp src/tinymovr_can.cpp src/tm_joint_iface.cpp)
+add_library(${PROJECT_NAME}_joint_iface ${TINYMOVR_SOURCES} src/socketcan_cpp/socketcan_cpp.cpp src/tm_joint_iface.cpp)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
diff --git a/config/hardware.yaml b/config/hardware.yaml
index a4972a3..becfecc 100644
--- a/config/hardware.yaml
+++ b/config/hardware.yaml
@@ -4,6 +4,7 @@ tinymovr_joint_iface:
       # hardware ID of the actuator
       id: 2
       offset: 3.14159265359
+      delay_us: 1000
       # offset to be added, in radians, to the position of an actuator
       #max-speed: 4.0 # in rad
       command_interface: velocity
\ No newline at end of file
diff --git a/config/velocity_controller.yaml b/config/velocity_controller.yaml
new file mode 100644
index 0000000..8fb8f98
--- /dev/null
+++ b/config/velocity_controller.yaml
@@ -0,0 +1,4 @@
+arm_controller:
+  type: "velocity_controllers/JointVelocityController"
+  joint: arm_1
+  pid: {p: 1.0, i: 0.0, d: 0.0}
\ No newline at end of file
diff --git a/include/socketcan_cpp/socketcan_cpp.hpp b/include/socketcan_cpp/socketcan_cpp.hpp
index a652239..f5f5bf2 100644
--- a/include/socketcan_cpp/socketcan_cpp.hpp
+++ b/include/socketcan_cpp/socketcan_cpp.hpp
@@ -60,4 +60,4 @@ namespace scpp
         std::string m_interface;
         SocketMode m_socket_mode;
     };
-}
+}
\ No newline at end of file
diff --git a/include/tinymovr/can.hpp b/include/tinymovr/can.hpp
index d654aca..6f44d56 100644
--- a/include/tinymovr/can.hpp
+++ b/include/tinymovr/can.hpp
@@ -14,8 +14,8 @@ class Can_ : Node
 {
     public:
 
-        Can_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Can_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         uint32_t get_rate(void);
         void set_rate(uint32_t value);
         uint32_t get_id(void);
diff --git a/include/tinymovr/comms.hpp b/include/tinymovr/comms.hpp
index 50e90b3..813d232 100644
--- a/include/tinymovr/comms.hpp
+++ b/include/tinymovr/comms.hpp
@@ -15,9 +15,9 @@ class Comms_ : Node
 {
     public:
 
-        Comms_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , can(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Comms_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , can(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         Can_ can;
 
 };
diff --git a/include/tinymovr/controller.hpp b/include/tinymovr/controller.hpp
index a6aeb9a..e8b4e09 100644
--- a/include/tinymovr/controller.hpp
+++ b/include/tinymovr/controller.hpp
@@ -18,12 +18,12 @@ class Controller_ : Node
 {
     public:
 
-        Controller_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , position(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , velocity(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , current(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , voltage(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Controller_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , position(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , velocity(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , current(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , voltage(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         uint8_t get_state(void);
         void set_state(uint8_t value);
         uint8_t get_mode(void);
diff --git a/include/tinymovr/current.hpp b/include/tinymovr/current.hpp
index d065c2e..a6e2ba6 100644
--- a/include/tinymovr/current.hpp
+++ b/include/tinymovr/current.hpp
@@ -14,8 +14,8 @@ class Current_ : Node
 {
     public:
 
-        Current_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Current_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_Iq_setpoint(void);
         void set_Iq_setpoint(float value);
         float get_Id_setpoint(void);
diff --git a/include/tinymovr/encoder.hpp b/include/tinymovr/encoder.hpp
index c9a1e03..d05d2e4 100644
--- a/include/tinymovr/encoder.hpp
+++ b/include/tinymovr/encoder.hpp
@@ -14,8 +14,8 @@ class Encoder_ : Node
 {
     public:
 
-        Encoder_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Encoder_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_position_estimate(void);
         float get_velocity_estimate(void);
         uint8_t get_type(void);
diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp
index 09d9dc1..4428625 100644
--- a/include/tinymovr/helpers.hpp
+++ b/include/tinymovr/helpers.hpp
@@ -21,8 +21,12 @@
 #include "Arduino.h"
 #endif
 
-#define EP_BITS (6)
-#define RECV_DELAY_US (160.0f)
+#define CAN_EP_SIZE (12)
+#define CAN_EP_MASK ((1 << CAN_EP_SIZE) - 1)
+#define CAN_SEQ_SIZE (9)
+#define CAN_SEQ_MASK (((1 << CAN_SEQ_SIZE) - 1) << CAN_EP_SIZE)
+#define CAN_DEV_SIZE (8)
+#define CAN_DEV_MASK (((1 << CAN_DEV_SIZE) - 1) << (CAN_EP_SIZE + CAN_SEQ_SIZE))
 
 typedef void (*send_callback)(uint32_t arbitration_id, uint8_t *data, uint8_t dlc, bool rtr);
 typedef bool (*recv_callback)(uint32_t *arbitration_id, uint8_t *data, uint8_t *dlc);
@@ -31,26 +35,28 @@ typedef void (*delay_us_callback)(uint32_t us);
 class Node {
     public:
 
-    Node(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-        can_node_id(_can_node_id), send_cb(_send_cb), recv_cb(_recv_cb), delay_us_cb(_delay_us_cb) {}
+    Node(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+        can_node_id(_can_node_id), send_cb(_send_cb), recv_cb(_recv_cb), delay_us_cb(_delay_us_cb), delay_us_value(_delay_us_value) {}
 
     protected:
     uint8_t can_node_id;
     send_callback send_cb;
     recv_callback recv_cb;
     delay_us_callback delay_us_cb;
+    uint32_t delay_us_value;
     uint8_t _data[8];
     uint8_t _dlc;
-    uint8_t get_arbitration_id(uint8_t cmd_id) {
-        return this->can_node_id << EP_BITS | cmd_id;
+    uint32_t get_arbitration_id(uint32_t cmd_id)
+    {
+        return ((this->can_node_id << (CAN_EP_SIZE + CAN_SEQ_SIZE)) & CAN_DEV_MASK) | (cmd_id & CAN_EP_MASK);
     }
-    void send(uint8_t cmd_id, uint8_t *data, uint8_t data_size, bool rtr)
+    void send(uint32_t cmd_id, uint8_t *data, uint8_t data_size, bool rtr)
     {
-        const uint8_t arb_id = this->get_arbitration_id(cmd_id);
+        const uint32_t arb_id = this->get_arbitration_id(cmd_id);
         this->send_cb(arb_id, data, data_size, rtr);
     }
 
-    bool recv(uint8_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us)
+    bool recv(uint32_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us)
     {
         uint32_t _arbitration_id;
         uint8_t _data[8];
@@ -62,7 +68,7 @@ class Node {
         {
            this->delay_us_cb(delay_us);
         }
-        const uint8_t arb_id = this->get_arbitration_id(cmd_id);
+        const uint32_t arb_id = this->get_arbitration_id(cmd_id);
         while (this->recv_cb(&_arbitration_id, _data, &_data_size))
         {
             if (_arbitration_id == arb_id)
@@ -218,8 +224,6 @@ inline size_t read_le<uint64_t>(uint64_t* value, const uint8_t* buffer) {
 
 template<>
 inline size_t read_le<float>(float* value, const uint8_t* buffer) {
-    //static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
-    //static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
     return read_le(reinterpret_cast<uint32_t*>(value), buffer);
 }
 
diff --git a/include/tinymovr/homing.hpp b/include/tinymovr/homing.hpp
new file mode 100644
index 0000000..1f74286
--- /dev/null
+++ b/include/tinymovr/homing.hpp
@@ -0,0 +1,31 @@
+/*
+* This file was automatically generated using Avlos.
+* https://github.com/tinymovr/avlos
+*
+* Any changes to this file will be overwritten when
+* content is regenerated.
+*/
+
+#pragma once
+
+#include <helpers.hpp>
+#include <stall_detect.hpp>
+
+class Homing_ : Node
+{
+    public:
+
+        Homing_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , stall_detect(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
+        float get_velocity(void);
+        void set_velocity(float value);
+        float get_max_homing_t(void);
+        void set_max_homing_t(float value);
+        float get_retract_dist(void);
+        void set_retract_dist(float value);
+        uint8_t get_warnings(void);
+        Stall_detect_ stall_detect;
+        void home();
+
+};
diff --git a/include/tinymovr/motor.hpp b/include/tinymovr/motor.hpp
index f05c1cf..ce298bf 100644
--- a/include/tinymovr/motor.hpp
+++ b/include/tinymovr/motor.hpp
@@ -14,8 +14,8 @@ class Motor_ : Node
 {
     public:
 
-        Motor_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Motor_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_R(void);
         void set_R(float value);
         float get_L(void);
diff --git a/include/tinymovr/position.hpp b/include/tinymovr/position.hpp
index 946f420..3ea3937 100644
--- a/include/tinymovr/position.hpp
+++ b/include/tinymovr/position.hpp
@@ -14,8 +14,8 @@ class Position_ : Node
 {
     public:
 
-        Position_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Position_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_setpoint(void);
         void set_setpoint(float value);
         float get_p_gain(void);
diff --git a/include/tinymovr/scheduler.hpp b/include/tinymovr/scheduler.hpp
index 18e5dce..2fe9bbc 100644
--- a/include/tinymovr/scheduler.hpp
+++ b/include/tinymovr/scheduler.hpp
@@ -14,10 +14,8 @@ class Scheduler_ : Node
 {
     public:
 
-        Scheduler_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
-        uint32_t get_total(void);
-        uint32_t get_busy(void);
+        Scheduler_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         uint8_t get_errors(void);
 
 };
diff --git a/include/tinymovr/stall_detect.hpp b/include/tinymovr/stall_detect.hpp
new file mode 100644
index 0000000..9088c58
--- /dev/null
+++ b/include/tinymovr/stall_detect.hpp
@@ -0,0 +1,26 @@
+/*
+* This file was automatically generated using Avlos.
+* https://github.com/tinymovr/avlos
+*
+* Any changes to this file will be overwritten when
+* content is regenerated.
+*/
+
+#pragma once
+
+#include <helpers.hpp>
+
+class Stall_detect_ : Node
+{
+    public:
+
+        Stall_detect_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
+        float get_velocity(void);
+        void set_velocity(float value);
+        float get_delta_pos(void);
+        void set_delta_pos(float value);
+        float get_t(void);
+        void set_t(float value);
+
+};
diff --git a/include/tinymovr/tinymovr.hpp b/include/tinymovr/tinymovr.hpp
index 031b8b0..400e186 100644
--- a/include/tinymovr/tinymovr.hpp
+++ b/include/tinymovr/tinymovr.hpp
@@ -15,15 +15,21 @@
 #include <motor.hpp>
 #include <encoder.hpp>
 #include <traj_planner.hpp>
+#include <homing.hpp>
 #include <watchdog.hpp>
 
-static uint32_t avlos_proto_hash = 3273002564;
+static uint32_t avlos_proto_hash = 4118115615;
 
 enum errors_flags
 {
     ERRORS_NONE = 0,
     ERRORS_UNDERVOLTAGE = (1 << 0), 
-    ERRORS_DRIVER_FAULT = (1 << 1)
+    ERRORS_DRIVER_FAULT = (1 << 1), 
+    ERRORS_CHARGE_PUMP_FAULT_STAT = (1 << 2), 
+    ERRORS_CHARGE_PUMP_FAULT = (1 << 3), 
+    ERRORS_DRV10_DISABLE = (1 << 4), 
+    ERRORS_DRV32_DISABLE = (1 << 5), 
+    ERRORS_DRV54_DISABLE = (1 << 6)
 };
 
 enum scheduler_errors_flags
@@ -68,21 +74,42 @@ enum traj_planner_errors_flags
     TRAJ_PLANNER_ERRORS_VCRUISE_OVER_LIMIT = (1 << 1)
 };
 
+enum homing_warnings_flags
+{
+    HOMING_WARNINGS_NONE = 0,
+    HOMING_WARNINGS_HOMING_TIMEOUT = (1 << 0)
+};
+
+enum motor_type_options
+{
+    MOTOR_TYPE_HIGH_CURRENT = 0, 
+    MOTOR_TYPE_GIMBAL = 1
+};
+
+enum encoder_type_options
+{
+    ENCODER_TYPE_INTERNAL = 0, 
+    ENCODER_TYPE_HALL = 1
+};
+
 class Tinymovr : Node
 {
     public:
 
-        Tinymovr(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , scheduler(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , controller(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , comms(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , motor(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , encoder(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , traj_planner(_can_node_id, _send_cb, _recv_cb, _delay_us_cb)
-            , watchdog(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Tinymovr(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , scheduler(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , controller(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , comms(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , motor(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , encoder(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , traj_planner(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , homing(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value)
+            , watchdog(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         uint32_t get_protocol_hash(void);
         uint32_t get_uid(void);
+        void get_fw_version(char out_value[]);
+        uint32_t get_hw_revision(void);
         float get_Vbus(void);
         float get_Ibus(void);
         float get_power(void);
@@ -92,12 +119,14 @@ class Tinymovr : Node
         void save_config();
         void erase_config();
         void reset();
+        void enter_dfu();
         Scheduler_ scheduler;
         Controller_ controller;
         Comms_ comms;
         Motor_ motor;
         Encoder_ encoder;
         Traj_planner_ traj_planner;
+        Homing_ homing;
         Watchdog_ watchdog;
 
 };
diff --git a/include/tinymovr/traj_planner.hpp b/include/tinymovr/traj_planner.hpp
index cd751d9..e4b96a4 100644
--- a/include/tinymovr/traj_planner.hpp
+++ b/include/tinymovr/traj_planner.hpp
@@ -14,14 +14,20 @@ class Traj_planner_ : Node
 {
     public:
 
-        Traj_planner_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Traj_planner_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_max_accel(void);
         void set_max_accel(float value);
         float get_max_decel(void);
         void set_max_decel(float value);
         float get_max_vel(void);
         void set_max_vel(float value);
+        float get_t_accel(void);
+        void set_t_accel(float value);
+        float get_t_decel(void);
+        void set_t_decel(float value);
+        float get_t_total(void);
+        void set_t_total(float value);
         void move_to(float pos_setpoint);
         void move_to_tlimit(float pos_setpoint);
         uint8_t get_errors(void);
diff --git a/include/tinymovr/velocity.hpp b/include/tinymovr/velocity.hpp
index 50bf210..e9fb5c0 100644
--- a/include/tinymovr/velocity.hpp
+++ b/include/tinymovr/velocity.hpp
@@ -14,8 +14,8 @@ class Velocity_ : Node
 {
     public:
 
-        Velocity_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Velocity_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_setpoint(void);
         void set_setpoint(float value);
         float get_limit(void);
diff --git a/include/tinymovr/voltage.hpp b/include/tinymovr/voltage.hpp
index 1a4f10d..661bb4e 100644
--- a/include/tinymovr/voltage.hpp
+++ b/include/tinymovr/voltage.hpp
@@ -14,8 +14,8 @@ class Voltage_ : Node
 {
     public:
 
-        Voltage_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Voltage_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         float get_Vq_setpoint(void);
 
 };
diff --git a/include/tinymovr/watchdog.hpp b/include/tinymovr/watchdog.hpp
index a2f9693..538eb1e 100644
--- a/include/tinymovr/watchdog.hpp
+++ b/include/tinymovr/watchdog.hpp
@@ -14,8 +14,8 @@ class Watchdog_ : Node
 {
     public:
 
-        Watchdog_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb):
-            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {};
+        Watchdog_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value):
+            Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {};
         bool get_enabled(void);
         void set_enabled(bool value);
         bool get_triggered(void);
diff --git a/include/tinymovr_can.hpp b/include/tinymovr_can.hpp
deleted file mode 100644
index 12a92c3..0000000
--- a/include/tinymovr_can.hpp
+++ /dev/null
@@ -1,22 +0,0 @@
-
-#pragma once
-
-#include "socketcan_cpp/socketcan_cpp.hpp"
-#include <ros/ros.h>
-
-namespace tinymovr_ros
-{
-
-class TinymovrCAN
-{
-public:
-    void init();
-    bool read_frame(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_len);
-    bool write_frame(uint32_t node_id, uint32_t ep_id, const uint8_t *data, uint8_t data_len);
-    bool write_frame(uint32_t arbitration_id, const uint8_t *data, uint8_t data_len);
-    uint32_t make_arbitration_id(uint32_t node_id, uint32_t command_id);
-private:
-    scpp::SocketCan socket_can;
-};
-
-}
diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp
index ec00d5b..efde441 100644
--- a/include/tm_joint_iface.hpp
+++ b/include/tm_joint_iface.hpp
@@ -10,7 +10,6 @@
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
 
-#include <tinymovr_can.hpp>
 #include <tinymovr/tinymovr.hpp>
 
 using namespace std;
@@ -26,6 +25,7 @@ class TinymovrJoint : public hardware_interface::RobotHW
     bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh);
     void read(const ros::Time& /*time*/, const ros::Duration& /*period*/);
     void write(const ros::Time& /*time*/, const ros::Duration& /*period*/);
+    void shutdown() ;
 
 protected:
     ros::NodeHandle nh_;
diff --git a/launch/tinymovr_joint_iface_node.launch b/launch/tinymovr_joint_iface_node.launch
index 2c030dc..317a05e 100644
--- a/launch/tinymovr_joint_iface_node.launch
+++ b/launch/tinymovr_joint_iface_node.launch
@@ -1,5 +1,8 @@
 <?xml version="1.0"?>
 <launch>
   <rosparam command="load" file="$(find tinymovr_ros)/config/hardware.yaml" />
+  <rosparam command="load" file="$(find tinymovr_ros)/config/velocity_controller.yaml" />
   <node name="tinymovr_joint_iface" pkg="tinymovr_ros" type="tinymovr_joint_iface_node" output="screen"/>
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" args="arm_controller"/>
 </launch>
\ No newline at end of file
diff --git a/package.xml b/package.xml
index 3274eb6..5da5719 100644
--- a/package.xml
+++ b/package.xml
@@ -49,6 +49,11 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>velocity_controllers</depend>
+  <depend>position_controllers</depend>
+  <depend>effort_controllers</depend>
+
   <build_depend>controller_interface</build_depend>
   <build_depend>controller_manager</build_depend>
   <build_depend>geometry_msgs</build_depend>
diff --git a/src/socketcan_cpp.cpp b/src/socketcan_cpp/socketcan_cpp.cpp
similarity index 91%
rename from src/socketcan_cpp.cpp
rename to src/socketcan_cpp/socketcan_cpp.cpp
index 1084a68..a62b5ad 100644
--- a/src/socketcan_cpp.cpp
+++ b/src/socketcan_cpp/socketcan_cpp.cpp
@@ -49,9 +49,8 @@ unsigned char can_len2dlc(unsigned char len)
 
 namespace scpp
 {
-    SocketCan::SocketCan()
-    {
-    }
+    SocketCan::SocketCan() {}
+
     SocketCanStatus SocketCan::open(const std::string & can_interface, int32_t read_timeout_ms, SocketMode mode)
     {
         m_interface = can_interface;
@@ -128,11 +127,19 @@ namespace scpp
             perror("bind");
             return STATUS_BIND_ERROR;
         }
+        struct can_filter rfilter[1];
+        rfilter[0].can_id   = ~0x00000700;
+        rfilter[0].can_mask = 0x1FFFFF00;
+        if (setsockopt(m_socket, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter)) < 0) {
+            perror("setsockopt");
+            return STATUS_SOCKET_CREATE_ERROR; // You should define this status
+        }
 #else
         printf("Your operating system does not support socket can! \n");
 #endif
         return STATUS_OK;
     }
+
     SocketCanStatus SocketCan::write(const CanFrame & msg)
     {
 #ifdef HAVE_SOCKETCAN_HEADERS
@@ -140,6 +147,9 @@ namespace scpp
         memset(&frame, 0, sizeof(frame)); /* init CAN FD frame, e.g. LEN = 0 */
         //convert CanFrame to canfd_frame
         frame.can_id = msg.id;
+        if (msg.id > 0x7FF) {
+            frame.can_id |= CAN_EFF_FLAG; // Set extended frame format bit
+        }
         frame.len = msg.len;
         frame.flags = msg.flags;
         memcpy(frame.data, msg.data, msg.len);
@@ -159,19 +169,17 @@ namespace scpp
 #endif
         return STATUS_OK;
     }
+
     SocketCanStatus SocketCan::read(CanFrame & msg)
     {
 #ifdef HAVE_SOCKETCAN_HEADERS
         struct canfd_frame frame;
-
-        // Read in a CAN frame
         auto num_bytes = ::read(m_socket, &frame, CANFD_MTU);
         if (num_bytes != CAN_MTU && num_bytes != CANFD_MTU)
         {
-            //perror("Can read error");
+            perror("Can read error");
             return STATUS_READ_ERROR;
         }
-
         msg.id = frame.can_id;
         msg.len = frame.len;
         msg.flags = frame.flags;
@@ -181,6 +189,7 @@ namespace scpp
 #endif
         return STATUS_OK;
     }
+
     SocketCanStatus SocketCan::close()
     {
 #ifdef HAVE_SOCKETCAN_HEADERS
@@ -192,8 +201,9 @@ namespace scpp
     {
         return m_interface;
     }
+
     SocketCan::~SocketCan()
     {
         close();
     }
-}
+}
\ No newline at end of file
diff --git a/src/tinymovr/can.cpp b/src/tinymovr/can.cpp
index 028d78d..33b1da0 100644
--- a/src/tinymovr/can.cpp
+++ b/src/tinymovr/can.cpp
@@ -11,8 +11,8 @@
 uint32_t Can_::get_rate(void)
 {
     uint32_t value = 0;
-    this->send(41, this->_data, 0, true);
-    if (this->recv(41, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(42, this->_data, 0, true);
+    if (this->recv(42, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -22,14 +22,14 @@ uint32_t Can_::get_rate(void)
 void Can_::set_rate(uint32_t value)
 {
     write_le(value, this->_data);
-    this->send(41, this->_data, sizeof(uint32_t), false);
+    this->send(42, this->_data, sizeof(uint32_t), false);
 }
 
 uint32_t Can_::get_id(void)
 {
     uint32_t value = 0;
-    this->send(42, this->_data, 0, true);
-    if (this->recv(42, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(43, this->_data, 0, true);
+    if (this->recv(43, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -39,7 +39,7 @@ uint32_t Can_::get_id(void)
 void Can_::set_id(uint32_t value)
 {
     write_le(value, this->_data);
-    this->send(42, this->_data, sizeof(uint32_t), false);
+    this->send(43, this->_data, sizeof(uint32_t), false);
 }
 
 
diff --git a/src/tinymovr/controller.cpp b/src/tinymovr/controller.cpp
index 54e7c6b..9bd6447 100644
--- a/src/tinymovr/controller.cpp
+++ b/src/tinymovr/controller.cpp
@@ -11,8 +11,8 @@
 uint8_t Controller_::get_state(void)
 {
     uint8_t value = 0;
-    this->send(14, this->_data, 0, true);
-    if (this->recv(14, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(15, this->_data, 0, true);
+    if (this->recv(15, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -22,14 +22,14 @@ uint8_t Controller_::get_state(void)
 void Controller_::set_state(uint8_t value)
 {
     write_le(value, this->_data);
-    this->send(14, this->_data, sizeof(uint8_t), false);
+    this->send(15, this->_data, sizeof(uint8_t), false);
 }
 
 uint8_t Controller_::get_mode(void)
 {
     uint8_t value = 0;
-    this->send(15, this->_data, 0, true);
-    if (this->recv(15, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(16, this->_data, 0, true);
+    if (this->recv(16, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -39,14 +39,14 @@ uint8_t Controller_::get_mode(void)
 void Controller_::set_mode(uint8_t value)
 {
     write_le(value, this->_data);
-    this->send(15, this->_data, sizeof(uint8_t), false);
+    this->send(16, this->_data, sizeof(uint8_t), false);
 }
 
 uint8_t Controller_::get_warnings(void)
 {
     uint8_t value = 0;
-    this->send(16, this->_data, 0, true);
-    if (this->recv(16, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(17, this->_data, 0, true);
+    if (this->recv(17, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -56,8 +56,8 @@ uint8_t Controller_::get_warnings(void)
 uint8_t Controller_::get_errors(void)
 {
     uint8_t value = 0;
-    this->send(17, this->_data, 0, true);
-    if (this->recv(17, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(18, this->_data, 0, true);
+    if (this->recv(18, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -67,27 +67,27 @@ uint8_t Controller_::get_errors(void)
 
 void Controller_::calibrate()
 {
-    this->send(35, this->_data, 0, true);
+    this->send(36, this->_data, 0, true);
 }
 
 void Controller_::idle()
 {
-    this->send(36, this->_data, 0, true);
+    this->send(37, this->_data, 0, true);
 }
 
 void Controller_::position_mode()
 {
-    this->send(37, this->_data, 0, true);
+    this->send(38, this->_data, 0, true);
 }
 
 void Controller_::velocity_mode()
 {
-    this->send(38, this->_data, 0, true);
+    this->send(39, this->_data, 0, true);
 }
 
 void Controller_::current_mode()
 {
-    this->send(39, this->_data, 0, true);
+    this->send(40, this->_data, 0, true);
 }
 
 float Controller_::set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint)
@@ -98,10 +98,10 @@ float Controller_::set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint)
     write_le(vel_setpoint, this->_data + data_len);
     data_len += sizeof(vel_setpoint);
 
-    this->send(40, this->_data, data_len, false);
+    this->send(41, this->_data, data_len, false);
     float value = 0;
     this->send(17, this->_data, 0, true);
-    if (this->recv(17, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    if (this->recv(17, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
diff --git a/src/tinymovr/current.cpp b/src/tinymovr/current.cpp
index 3a72f4b..c7873f8 100644
--- a/src/tinymovr/current.cpp
+++ b/src/tinymovr/current.cpp
@@ -11,8 +11,8 @@
 float Current_::get_Iq_setpoint(void)
 {
     float value = 0;
-    this->send(26, this->_data, 0, true);
-    if (this->recv(26, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(27, this->_data, 0, true);
+    if (this->recv(27, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -22,14 +22,14 @@ float Current_::get_Iq_setpoint(void)
 void Current_::set_Iq_setpoint(float value)
 {
     write_le(value, this->_data);
-    this->send(26, this->_data, sizeof(float), false);
+    this->send(27, this->_data, sizeof(float), false);
 }
 
 float Current_::get_Id_setpoint(void)
 {
     float value = 0;
-    this->send(27, this->_data, 0, true);
-    if (this->recv(27, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(28, this->_data, 0, true);
+    if (this->recv(28, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -39,8 +39,8 @@ float Current_::get_Id_setpoint(void)
 float Current_::get_Iq_limit(void)
 {
     float value = 0;
-    this->send(28, this->_data, 0, true);
-    if (this->recv(28, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(29, this->_data, 0, true);
+    if (this->recv(29, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -50,14 +50,14 @@ float Current_::get_Iq_limit(void)
 void Current_::set_Iq_limit(float value)
 {
     write_le(value, this->_data);
-    this->send(28, this->_data, sizeof(float), false);
+    this->send(29, this->_data, sizeof(float), false);
 }
 
 float Current_::get_Iq_estimate(void)
 {
     float value = 0;
-    this->send(29, this->_data, 0, true);
-    if (this->recv(29, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(30, this->_data, 0, true);
+    if (this->recv(30, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -67,8 +67,8 @@ float Current_::get_Iq_estimate(void)
 float Current_::get_bandwidth(void)
 {
     float value = 0;
-    this->send(30, this->_data, 0, true);
-    if (this->recv(30, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(31, this->_data, 0, true);
+    if (this->recv(31, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -78,14 +78,14 @@ float Current_::get_bandwidth(void)
 void Current_::set_bandwidth(float value)
 {
     write_le(value, this->_data);
-    this->send(30, this->_data, sizeof(float), false);
+    this->send(31, this->_data, sizeof(float), false);
 }
 
 float Current_::get_Iq_p_gain(void)
 {
     float value = 0;
-    this->send(31, this->_data, 0, true);
-    if (this->recv(31, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(32, this->_data, 0, true);
+    if (this->recv(32, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -95,8 +95,8 @@ float Current_::get_Iq_p_gain(void)
 float Current_::get_max_Ibus_regen(void)
 {
     float value = 0;
-    this->send(32, this->_data, 0, true);
-    if (this->recv(32, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(33, this->_data, 0, true);
+    if (this->recv(33, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -106,14 +106,14 @@ float Current_::get_max_Ibus_regen(void)
 void Current_::set_max_Ibus_regen(float value)
 {
     write_le(value, this->_data);
-    this->send(32, this->_data, sizeof(float), false);
+    this->send(33, this->_data, sizeof(float), false);
 }
 
 float Current_::get_max_Ibrake(void)
 {
     float value = 0;
-    this->send(33, this->_data, 0, true);
-    if (this->recv(33, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(34, this->_data, 0, true);
+    if (this->recv(34, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -123,7 +123,7 @@ float Current_::get_max_Ibrake(void)
 void Current_::set_max_Ibrake(float value)
 {
     write_le(value, this->_data);
-    this->send(33, this->_data, sizeof(float), false);
+    this->send(34, this->_data, sizeof(float), false);
 }
 
 
diff --git a/src/tinymovr/encoder.cpp b/src/tinymovr/encoder.cpp
index d919bf8..9622c48 100644
--- a/src/tinymovr/encoder.cpp
+++ b/src/tinymovr/encoder.cpp
@@ -11,8 +11,8 @@
 float Encoder_::get_position_estimate(void)
 {
     float value = 0;
-    this->send(52, this->_data, 0, true);
-    if (this->recv(52, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(53, this->_data, 0, true);
+    if (this->recv(53, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -22,8 +22,8 @@ float Encoder_::get_position_estimate(void)
 float Encoder_::get_velocity_estimate(void)
 {
     float value = 0;
-    this->send(53, this->_data, 0, true);
-    if (this->recv(53, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(54, this->_data, 0, true);
+    if (this->recv(54, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -33,8 +33,8 @@ float Encoder_::get_velocity_estimate(void)
 uint8_t Encoder_::get_type(void)
 {
     uint8_t value = 0;
-    this->send(54, this->_data, 0, true);
-    if (this->recv(54, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(55, this->_data, 0, true);
+    if (this->recv(55, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -44,14 +44,14 @@ uint8_t Encoder_::get_type(void)
 void Encoder_::set_type(uint8_t value)
 {
     write_le(value, this->_data);
-    this->send(54, this->_data, sizeof(uint8_t), false);
+    this->send(55, this->_data, sizeof(uint8_t), false);
 }
 
 float Encoder_::get_bandwidth(void)
 {
     float value = 0;
-    this->send(55, this->_data, 0, true);
-    if (this->recv(55, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(56, this->_data, 0, true);
+    if (this->recv(56, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -61,14 +61,14 @@ float Encoder_::get_bandwidth(void)
 void Encoder_::set_bandwidth(float value)
 {
     write_le(value, this->_data);
-    this->send(55, this->_data, sizeof(float), false);
+    this->send(56, this->_data, sizeof(float), false);
 }
 
 bool Encoder_::get_calibrated(void)
 {
     bool value = 0;
-    this->send(56, this->_data, 0, true);
-    if (this->recv(56, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(57, this->_data, 0, true);
+    if (this->recv(57, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -78,8 +78,8 @@ bool Encoder_::get_calibrated(void)
 uint8_t Encoder_::get_errors(void)
 {
     uint8_t value = 0;
-    this->send(57, this->_data, 0, true);
-    if (this->recv(57, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(58, this->_data, 0, true);
+    if (this->recv(58, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
diff --git a/src/tinymovr/homing.cpp b/src/tinymovr/homing.cpp
new file mode 100644
index 0000000..697b292
--- /dev/null
+++ b/src/tinymovr/homing.cpp
@@ -0,0 +1,79 @@
+/*
+* This file was automatically generated using Avlos.
+* https://github.com/tinymovr/avlos
+*
+* Any changes to this file will be overwritten when
+* content is regenerated.
+*/
+
+#include <homing.hpp>
+
+float Homing_::get_velocity(void)
+{
+    float value = 0;
+    this->send(68, this->_data, 0, true);
+    if (this->recv(68, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+
+void Homing_::set_velocity(float value)
+{
+    write_le(value, this->_data);
+    this->send(68, this->_data, sizeof(float), false);
+}
+
+float Homing_::get_max_homing_t(void)
+{
+    float value = 0;
+    this->send(69, this->_data, 0, true);
+    if (this->recv(69, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+
+void Homing_::set_max_homing_t(float value)
+{
+    write_le(value, this->_data);
+    this->send(69, this->_data, sizeof(float), false);
+}
+
+float Homing_::get_retract_dist(void)
+{
+    float value = 0;
+    this->send(70, this->_data, 0, true);
+    if (this->recv(70, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+
+void Homing_::set_retract_dist(float value)
+{
+    write_le(value, this->_data);
+    this->send(70, this->_data, sizeof(float), false);
+}
+
+uint8_t Homing_::get_warnings(void)
+{
+    uint8_t value = 0;
+    this->send(71, this->_data, 0, true);
+    if (this->recv(71, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+
+
+void Homing_::home()
+{
+    this->send(75, this->_data, 0, true);
+}
+
+
diff --git a/src/tinymovr/motor.cpp b/src/tinymovr/motor.cpp
index db9ff2d..2eb11bc 100644
--- a/src/tinymovr/motor.cpp
+++ b/src/tinymovr/motor.cpp
@@ -11,8 +11,8 @@
 float Motor_::get_R(void)
 {
     float value = 0;
-    this->send(43, this->_data, 0, true);
-    if (this->recv(43, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(44, this->_data, 0, true);
+    if (this->recv(44, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -22,14 +22,14 @@ float Motor_::get_R(void)
 void Motor_::set_R(float value)
 {
     write_le(value, this->_data);
-    this->send(43, this->_data, sizeof(float), false);
+    this->send(44, this->_data, sizeof(float), false);
 }
 
 float Motor_::get_L(void)
 {
     float value = 0;
-    this->send(44, this->_data, 0, true);
-    if (this->recv(44, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(45, this->_data, 0, true);
+    if (this->recv(45, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -39,14 +39,14 @@ float Motor_::get_L(void)
 void Motor_::set_L(float value)
 {
     write_le(value, this->_data);
-    this->send(44, this->_data, sizeof(float), false);
+    this->send(45, this->_data, sizeof(float), false);
 }
 
 uint8_t Motor_::get_pole_pairs(void)
 {
     uint8_t value = 0;
-    this->send(45, this->_data, 0, true);
-    if (this->recv(45, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(46, this->_data, 0, true);
+    if (this->recv(46, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -56,14 +56,14 @@ uint8_t Motor_::get_pole_pairs(void)
 void Motor_::set_pole_pairs(uint8_t value)
 {
     write_le(value, this->_data);
-    this->send(45, this->_data, sizeof(uint8_t), false);
+    this->send(46, this->_data, sizeof(uint8_t), false);
 }
 
 uint8_t Motor_::get_type(void)
 {
     uint8_t value = 0;
-    this->send(46, this->_data, 0, true);
-    if (this->recv(46, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(47, this->_data, 0, true);
+    if (this->recv(47, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -73,14 +73,14 @@ uint8_t Motor_::get_type(void)
 void Motor_::set_type(uint8_t value)
 {
     write_le(value, this->_data);
-    this->send(46, this->_data, sizeof(uint8_t), false);
+    this->send(47, this->_data, sizeof(uint8_t), false);
 }
 
 float Motor_::get_offset(void)
 {
     float value = 0;
-    this->send(47, this->_data, 0, true);
-    if (this->recv(47, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(48, this->_data, 0, true);
+    if (this->recv(48, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -90,14 +90,14 @@ float Motor_::get_offset(void)
 void Motor_::set_offset(float value)
 {
     write_le(value, this->_data);
-    this->send(47, this->_data, sizeof(float), false);
+    this->send(48, this->_data, sizeof(float), false);
 }
 
 int8_t Motor_::get_direction(void)
 {
     int8_t value = 0;
-    this->send(48, this->_data, 0, true);
-    if (this->recv(48, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(49, this->_data, 0, true);
+    if (this->recv(49, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -107,14 +107,14 @@ int8_t Motor_::get_direction(void)
 void Motor_::set_direction(int8_t value)
 {
     write_le(value, this->_data);
-    this->send(48, this->_data, sizeof(int8_t), false);
+    this->send(49, this->_data, sizeof(int8_t), false);
 }
 
 bool Motor_::get_calibrated(void)
 {
     bool value = 0;
-    this->send(49, this->_data, 0, true);
-    if (this->recv(49, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(50, this->_data, 0, true);
+    if (this->recv(50, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -124,8 +124,8 @@ bool Motor_::get_calibrated(void)
 float Motor_::get_I_cal(void)
 {
     float value = 0;
-    this->send(50, this->_data, 0, true);
-    if (this->recv(50, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(51, this->_data, 0, true);
+    if (this->recv(51, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -135,14 +135,14 @@ float Motor_::get_I_cal(void)
 void Motor_::set_I_cal(float value)
 {
     write_le(value, this->_data);
-    this->send(50, this->_data, sizeof(float), false);
+    this->send(51, this->_data, sizeof(float), false);
 }
 
 uint8_t Motor_::get_errors(void)
 {
     uint8_t value = 0;
-    this->send(51, this->_data, 0, true);
-    if (this->recv(51, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(52, this->_data, 0, true);
+    if (this->recv(52, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
diff --git a/src/tinymovr/position.cpp b/src/tinymovr/position.cpp
index 4f52c52..6848657 100644
--- a/src/tinymovr/position.cpp
+++ b/src/tinymovr/position.cpp
@@ -11,8 +11,8 @@
 float Position_::get_setpoint(void)
 {
     float value = 0;
-    this->send(18, this->_data, 0, true);
-    if (this->recv(18, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(19, this->_data, 0, true);
+    if (this->recv(19, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -22,14 +22,14 @@ float Position_::get_setpoint(void)
 void Position_::set_setpoint(float value)
 {
     write_le(value, this->_data);
-    this->send(18, this->_data, sizeof(float), false);
+    this->send(19, this->_data, sizeof(float), false);
 }
 
 float Position_::get_p_gain(void)
 {
     float value = 0;
-    this->send(19, this->_data, 0, true);
-    if (this->recv(19, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(20, this->_data, 0, true);
+    if (this->recv(20, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -39,7 +39,7 @@ float Position_::get_p_gain(void)
 void Position_::set_p_gain(float value)
 {
     write_le(value, this->_data);
-    this->send(19, this->_data, sizeof(float), false);
+    this->send(20, this->_data, sizeof(float), false);
 }
 
 
diff --git a/src/tinymovr/scheduler.cpp b/src/tinymovr/scheduler.cpp
index 07be947..de9bd30 100644
--- a/src/tinymovr/scheduler.cpp
+++ b/src/tinymovr/scheduler.cpp
@@ -8,33 +8,11 @@
 
 #include <scheduler.hpp>
 
-uint32_t Scheduler_::get_total(void)
-{
-    uint32_t value = 0;
-    this->send(11, this->_data, 0, true);
-    if (this->recv(11, this->_data, &(this->_dlc), RECV_DELAY_US)) 
-    {
-        read_le(&value, this->_data);
-    }
-    return value;
-}
-
-uint32_t Scheduler_::get_busy(void)
-{
-    uint32_t value = 0;
-    this->send(12, this->_data, 0, true);
-    if (this->recv(12, this->_data, &(this->_dlc), RECV_DELAY_US)) 
-    {
-        read_le(&value, this->_data);
-    }
-    return value;
-}
-
 uint8_t Scheduler_::get_errors(void)
 {
     uint8_t value = 0;
-    this->send(13, this->_data, 0, true);
-    if (this->recv(13, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(14, this->_data, 0, true);
+    if (this->recv(14, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
diff --git a/src/tinymovr/stall_detect.cpp b/src/tinymovr/stall_detect.cpp
new file mode 100644
index 0000000..b3a5d93
--- /dev/null
+++ b/src/tinymovr/stall_detect.cpp
@@ -0,0 +1,63 @@
+/*
+* This file was automatically generated using Avlos.
+* https://github.com/tinymovr/avlos
+*
+* Any changes to this file will be overwritten when
+* content is regenerated.
+*/
+
+#include <stall_detect.hpp>
+
+float Stall_detect_::get_velocity(void)
+{
+    float value = 0;
+    this->send(72, this->_data, 0, true);
+    if (this->recv(72, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+
+void Stall_detect_::set_velocity(float value)
+{
+    write_le(value, this->_data);
+    this->send(72, this->_data, sizeof(float), false);
+}
+
+float Stall_detect_::get_delta_pos(void)
+{
+    float value = 0;
+    this->send(73, this->_data, 0, true);
+    if (this->recv(73, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+
+void Stall_detect_::set_delta_pos(float value)
+{
+    write_le(value, this->_data);
+    this->send(73, this->_data, sizeof(float), false);
+}
+
+float Stall_detect_::get_t(void)
+{
+    float value = 0;
+    this->send(74, this->_data, 0, true);
+    if (this->recv(74, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+
+void Stall_detect_::set_t(float value)
+{
+    write_le(value, this->_data);
+    this->send(74, this->_data, sizeof(float), false);
+}
+
+
+
diff --git a/src/tinymovr/tinymovr.cpp b/src/tinymovr/tinymovr.cpp
index 82fe78e..f4e977b 100644
--- a/src/tinymovr/tinymovr.cpp
+++ b/src/tinymovr/tinymovr.cpp
@@ -10,7 +10,7 @@ uint32_t Tinymovr::get_protocol_hash(void)
 {
     uint32_t value = 0;
     this->send(0, this->_data, 0, true);
-    if (this->recv(0, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    if (this->recv(0, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -20,7 +20,26 @@ uint32_t Tinymovr::get_uid(void)
 {
     uint32_t value = 0;
     this->send(1, this->_data, 0, true);
-    if (this->recv(1, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    if (this->recv(1, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+void Tinymovr::get_fw_version(char out_value[])
+{
+    this->send(2, this->_data, 0, true);
+    this->_dlc = 0;
+    if (this->recv(2, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        memcpy(out_value, this->_data, this->_dlc);
+    }
+}
+uint32_t Tinymovr::get_hw_revision(void)
+{
+    uint32_t value = 0;
+    this->send(3, this->_data, 0, true);
+    if (this->recv(3, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -29,8 +48,8 @@ uint32_t Tinymovr::get_uid(void)
 float Tinymovr::get_Vbus(void)
 {
     float value = 0;
-    this->send(2, this->_data, 0, true);
-    if (this->recv(2, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(4, this->_data, 0, true);
+    if (this->recv(4, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -39,8 +58,8 @@ float Tinymovr::get_Vbus(void)
 float Tinymovr::get_Ibus(void)
 {
     float value = 0;
-    this->send(3, this->_data, 0, true);
-    if (this->recv(3, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(5, this->_data, 0, true);
+    if (this->recv(5, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -49,8 +68,8 @@ float Tinymovr::get_Ibus(void)
 float Tinymovr::get_power(void)
 {
     float value = 0;
-    this->send(4, this->_data, 0, true);
-    if (this->recv(4, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(6, this->_data, 0, true);
+    if (this->recv(6, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -59,8 +78,8 @@ float Tinymovr::get_power(void)
 float Tinymovr::get_temp(void)
 {
     float value = 0;
-    this->send(5, this->_data, 0, true);
-    if (this->recv(5, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(7, this->_data, 0, true);
+    if (this->recv(7, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -69,8 +88,8 @@ float Tinymovr::get_temp(void)
 bool Tinymovr::get_calibrated(void)
 {
     bool value = 0;
-    this->send(6, this->_data, 0, true);
-    if (this->recv(6, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(8, this->_data, 0, true);
+    if (this->recv(8, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -79,8 +98,8 @@ bool Tinymovr::get_calibrated(void)
 uint8_t Tinymovr::get_errors(void)
 {
     uint8_t value = 0;
-    this->send(7, this->_data, 0, true);
-    if (this->recv(7, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(9, this->_data, 0, true);
+    if (this->recv(9, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -89,16 +108,21 @@ uint8_t Tinymovr::get_errors(void)
 
 void Tinymovr::save_config()
 {
-    this->send(8, this->_data, 0, true);
+    this->send(10, this->_data, 0, true);
 }
 
 void Tinymovr::erase_config()
 {
-    this->send(9, this->_data, 0, true);
+    this->send(11, this->_data, 0, true);
 }
 
 void Tinymovr::reset()
 {
-    this->send(10, this->_data, 0, true);
+    this->send(12, this->_data, 0, true);
+}
+
+void Tinymovr::enter_dfu()
+{
+    this->send(13, this->_data, 0, true);
 }
 
diff --git a/src/tinymovr/traj_planner.cpp b/src/tinymovr/traj_planner.cpp
index 604a48d..058ec08 100644
--- a/src/tinymovr/traj_planner.cpp
+++ b/src/tinymovr/traj_planner.cpp
@@ -11,8 +11,8 @@
 float Traj_planner_::get_max_accel(void)
 {
     float value = 0;
-    this->send(58, this->_data, 0, true);
-    if (this->recv(58, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(59, this->_data, 0, true);
+    if (this->recv(59, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -22,14 +22,14 @@ float Traj_planner_::get_max_accel(void)
 void Traj_planner_::set_max_accel(float value)
 {
     write_le(value, this->_data);
-    this->send(58, this->_data, sizeof(float), false);
+    this->send(59, this->_data, sizeof(float), false);
 }
 
 float Traj_planner_::get_max_decel(void)
 {
     float value = 0;
-    this->send(59, this->_data, 0, true);
-    if (this->recv(59, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(60, this->_data, 0, true);
+    if (this->recv(60, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -39,14 +39,14 @@ float Traj_planner_::get_max_decel(void)
 void Traj_planner_::set_max_decel(float value)
 {
     write_le(value, this->_data);
-    this->send(59, this->_data, sizeof(float), false);
+    this->send(60, this->_data, sizeof(float), false);
 }
 
 float Traj_planner_::get_max_vel(void)
 {
     float value = 0;
-    this->send(60, this->_data, 0, true);
-    if (this->recv(60, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(61, this->_data, 0, true);
+    if (this->recv(61, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -56,7 +56,58 @@ float Traj_planner_::get_max_vel(void)
 void Traj_planner_::set_max_vel(float value)
 {
     write_le(value, this->_data);
-    this->send(60, this->_data, sizeof(float), false);
+    this->send(61, this->_data, sizeof(float), false);
+}
+
+float Traj_planner_::get_t_accel(void)
+{
+    float value = 0;
+    this->send(62, this->_data, 0, true);
+    if (this->recv(62, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+
+void Traj_planner_::set_t_accel(float value)
+{
+    write_le(value, this->_data);
+    this->send(62, this->_data, sizeof(float), false);
+}
+
+float Traj_planner_::get_t_decel(void)
+{
+    float value = 0;
+    this->send(63, this->_data, 0, true);
+    if (this->recv(63, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+
+void Traj_planner_::set_t_decel(float value)
+{
+    write_le(value, this->_data);
+    this->send(63, this->_data, sizeof(float), false);
+}
+
+float Traj_planner_::get_t_total(void)
+{
+    float value = 0;
+    this->send(64, this->_data, 0, true);
+    if (this->recv(64, this->_data, &(this->_dlc), this->delay_us_value)) 
+    {
+        read_le(&value, this->_data);
+    }
+    return value;
+}
+
+void Traj_planner_::set_t_total(float value)
+{
+    write_le(value, this->_data);
+    this->send(64, this->_data, sizeof(float), false);
 }
 
 
@@ -66,7 +117,7 @@ void Traj_planner_::move_to(float pos_setpoint)
     write_le(pos_setpoint, this->_data + data_len);
     data_len += sizeof(pos_setpoint);
 
-    this->send(61, this->_data, data_len, false);
+    this->send(65, this->_data, data_len, false);
 }
 
 void Traj_planner_::move_to_tlimit(float pos_setpoint)
@@ -75,13 +126,13 @@ void Traj_planner_::move_to_tlimit(float pos_setpoint)
     write_le(pos_setpoint, this->_data + data_len);
     data_len += sizeof(pos_setpoint);
 
-    this->send(62, this->_data, data_len, false);
+    this->send(66, this->_data, data_len, false);
 }
 uint8_t Traj_planner_::get_errors(void)
 {
     uint8_t value = 0;
-    this->send(63, this->_data, 0, true);
-    if (this->recv(63, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(67, this->_data, 0, true);
+    if (this->recv(67, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
diff --git a/src/tinymovr/velocity.cpp b/src/tinymovr/velocity.cpp
index 901b6fe..318995e 100644
--- a/src/tinymovr/velocity.cpp
+++ b/src/tinymovr/velocity.cpp
@@ -11,8 +11,8 @@
 float Velocity_::get_setpoint(void)
 {
     float value = 0;
-    this->send(20, this->_data, 0, true);
-    if (this->recv(20, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(21, this->_data, 0, true);
+    if (this->recv(21, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -22,14 +22,14 @@ float Velocity_::get_setpoint(void)
 void Velocity_::set_setpoint(float value)
 {
     write_le(value, this->_data);
-    this->send(20, this->_data, sizeof(float), false);
+    this->send(21, this->_data, sizeof(float), false);
 }
 
 float Velocity_::get_limit(void)
 {
     float value = 0;
-    this->send(21, this->_data, 0, true);
-    if (this->recv(21, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(22, this->_data, 0, true);
+    if (this->recv(22, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -39,14 +39,14 @@ float Velocity_::get_limit(void)
 void Velocity_::set_limit(float value)
 {
     write_le(value, this->_data);
-    this->send(21, this->_data, sizeof(float), false);
+    this->send(22, this->_data, sizeof(float), false);
 }
 
 float Velocity_::get_p_gain(void)
 {
     float value = 0;
-    this->send(22, this->_data, 0, true);
-    if (this->recv(22, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(23, this->_data, 0, true);
+    if (this->recv(23, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -56,14 +56,14 @@ float Velocity_::get_p_gain(void)
 void Velocity_::set_p_gain(float value)
 {
     write_le(value, this->_data);
-    this->send(22, this->_data, sizeof(float), false);
+    this->send(23, this->_data, sizeof(float), false);
 }
 
 float Velocity_::get_i_gain(void)
 {
     float value = 0;
-    this->send(23, this->_data, 0, true);
-    if (this->recv(23, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(24, this->_data, 0, true);
+    if (this->recv(24, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -73,14 +73,14 @@ float Velocity_::get_i_gain(void)
 void Velocity_::set_i_gain(float value)
 {
     write_le(value, this->_data);
-    this->send(23, this->_data, sizeof(float), false);
+    this->send(24, this->_data, sizeof(float), false);
 }
 
 float Velocity_::get_deadband(void)
 {
     float value = 0;
-    this->send(24, this->_data, 0, true);
-    if (this->recv(24, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(25, this->_data, 0, true);
+    if (this->recv(25, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -90,14 +90,14 @@ float Velocity_::get_deadband(void)
 void Velocity_::set_deadband(float value)
 {
     write_le(value, this->_data);
-    this->send(24, this->_data, sizeof(float), false);
+    this->send(25, this->_data, sizeof(float), false);
 }
 
 float Velocity_::get_increment(void)
 {
     float value = 0;
-    this->send(25, this->_data, 0, true);
-    if (this->recv(25, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(26, this->_data, 0, true);
+    if (this->recv(26, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -107,7 +107,7 @@ float Velocity_::get_increment(void)
 void Velocity_::set_increment(float value)
 {
     write_le(value, this->_data);
-    this->send(25, this->_data, sizeof(float), false);
+    this->send(26, this->_data, sizeof(float), false);
 }
 
 
diff --git a/src/tinymovr/voltage.cpp b/src/tinymovr/voltage.cpp
index 151fb16..754640b 100644
--- a/src/tinymovr/voltage.cpp
+++ b/src/tinymovr/voltage.cpp
@@ -11,8 +11,8 @@
 float Voltage_::get_Vq_setpoint(void)
 {
     float value = 0;
-    this->send(34, this->_data, 0, true);
-    if (this->recv(34, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(35, this->_data, 0, true);
+    if (this->recv(35, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
diff --git a/src/tinymovr/watchdog.cpp b/src/tinymovr/watchdog.cpp
index f400346..35548b2 100644
--- a/src/tinymovr/watchdog.cpp
+++ b/src/tinymovr/watchdog.cpp
@@ -11,8 +11,8 @@
 bool Watchdog_::get_enabled(void)
 {
     bool value = 0;
-    this->send(64, this->_data, 0, true);
-    if (this->recv(64, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(76, this->_data, 0, true);
+    if (this->recv(76, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -22,14 +22,14 @@ bool Watchdog_::get_enabled(void)
 void Watchdog_::set_enabled(bool value)
 {
     write_le(value, this->_data);
-    this->send(64, this->_data, sizeof(bool), false);
+    this->send(76, this->_data, sizeof(bool), false);
 }
 
 bool Watchdog_::get_triggered(void)
 {
     bool value = 0;
-    this->send(65, this->_data, 0, true);
-    if (this->recv(65, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(77, this->_data, 0, true);
+    if (this->recv(77, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -39,8 +39,8 @@ bool Watchdog_::get_triggered(void)
 float Watchdog_::get_timeout(void)
 {
     float value = 0;
-    this->send(66, this->_data, 0, true);
-    if (this->recv(66, this->_data, &(this->_dlc), RECV_DELAY_US)) 
+    this->send(78, this->_data, 0, true);
+    if (this->recv(78, this->_data, &(this->_dlc), this->delay_us_value)) 
     {
         read_le(&value, this->_data);
     }
@@ -50,7 +50,7 @@ float Watchdog_::get_timeout(void)
 void Watchdog_::set_timeout(float value)
 {
     write_le(value, this->_data);
-    this->send(66, this->_data, sizeof(float), false);
+    this->send(78, this->_data, sizeof(float), false);
 }
 
 
diff --git a/src/tinymovr_can.cpp b/src/tinymovr_can.cpp
deleted file mode 100644
index fa2f804..0000000
--- a/src/tinymovr_can.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-
-#include <tinymovr_can.hpp>
-
-namespace tinymovr_ros
-{
-
-void TinymovrCAN::init()
-{
-    auto status = socket_can.open("can0");
-    if (scpp::STATUS_OK == status)
-    {
-        ROS_INFO("Socketcan opened successfully");
-    }
-    else
-    {
-        ROS_ERROR("Cant' open Socketcan: %d", status);
-        exit(1);
-    }
-}
-
-bool TinymovrCAN::read_frame(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_len)
-{
-    scpp::CanFrame fr;
-
-    if (scpp::STATUS_OK != socket_can.read(fr))
-    {
-        return false;
-    }
-
-    memcpy(data, fr.data, 8);
-    *data_len = fr.len;
-    *arbitration_id = fr.id;
-
-    return true;
-}
-
-bool TinymovrCAN::write_frame(uint32_t node_id, uint32_t ep_id, const uint8_t *data, uint8_t data_len)
-{
-    return write_frame(make_arbitration_id(node_id, ep_id), data, data_len);
-}
-
-bool TinymovrCAN::write_frame(uint32_t arbitration_id, const uint8_t *data, uint8_t data_len)
-{
-    scpp::CanFrame cf_to_write;
-
-    cf_to_write.id = arbitration_id;
-    cf_to_write.len = data_len;
-    for (int i = 0; i < data_len; ++i)
-        cf_to_write.data[i] = data[i];
-    auto write_sc_status = socket_can.write(cf_to_write);
-    if (write_sc_status != scpp::STATUS_OK)
-        return false;
-    return true;
-}
-
-uint32_t TinymovrCAN::make_arbitration_id(uint32_t node_id, uint32_t command_id)
-{
-    return node_id << 6 | command_id;
-}
-
-}
\ No newline at end of file
diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp
index 9f69efe..7f80549 100644
--- a/src/tm_joint_iface.cpp
+++ b/src/tm_joint_iface.cpp
@@ -5,7 +5,11 @@
 
 #include <linux/can.h>
 #include <linux/can/raw.h>
+#include <stdexcept>
 
+#include <ros/console.h>
+
+#include "socketcan_cpp/socketcan_cpp.hpp"
 #include <tm_joint_iface.hpp>
 
 using namespace std;
@@ -14,7 +18,32 @@ namespace tinymovr_ros
 {
 
  
-TinymovrCAN tmcan;
+scpp::SocketCan socket_can;
+
+const char* SocketCanErrorToString(scpp::SocketCanStatus status) {
+    switch (status) {
+        case scpp::STATUS_OK:
+            return "No error";
+        case scpp::STATUS_SOCKET_CREATE_ERROR:
+            return "SocketCAN socket creation error";
+        case scpp::STATUS_INTERFACE_NAME_TO_IDX_ERROR:
+            return "SocketCAN interface name to index error";
+        case scpp::STATUS_MTU_ERROR:
+            return "SocketCAN maximum transfer unit error";
+        case scpp::STATUS_CANFD_NOT_SUPPORTED:
+            return "SocketCAN flexible data-rate not supported on this interface";
+        case scpp::STATUS_ENABLE_FD_SUPPORT_ERROR:
+            return "Error enabling SocketCAN flexible-data-rate support";
+        case scpp::STATUS_WRITE_ERROR:
+            return "SocketCAN write error";
+        case scpp::STATUS_READ_ERROR:
+            return "SocketCAN read error";
+        case scpp::STATUS_BIND_ERROR:
+            return "SocketCAN bind error";
+        default:
+            return "Unknown SocketCAN error";
+    }
+}
 
 // ---------------------------------------------------------------
 /*
@@ -24,14 +53,30 @@ TinymovrCAN tmcan;
  *
  *  arbitration_id: the frame arbitration id
  *  data: pointer to the data array to be transmitted
- *  data_size: the size of transmitted data
+ *  data_length: the size of transmitted data
  *  rtr: if the frame is of request transmit type (RTR)
  */
-void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr)
+void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_length, bool rtr)
 {
-    if (!tmcan.write_frame(arbitration_id, data, data_size))
+    ROS_DEBUG_STREAM("Attempting to write CAN frame with arbitration_id: " << arbitration_id);
+
+    scpp::CanFrame cf_to_write;
+
+    cf_to_write.id = arbitration_id;
+    if (rtr) {
+        cf_to_write.id |= CAN_RTR_FLAG; // Set RTR flag if rtr argument
+    }
+    cf_to_write.len = data_length;
+    for (int i = 0; i < data_length; ++i)
+        cf_to_write.data[i] = data[i];
+    auto write_status = socket_can.write(cf_to_write);
+    if (write_status != scpp::STATUS_OK)
     {
-        throw "CAN write error";
+        throw std::runtime_error(SocketCanErrorToString(write_status));
+    }
+    else
+    {
+        ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << arbitration_id << " written successfully.");
     }
 }
 
@@ -42,15 +87,27 @@ void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr
  *
  *  arbitration_id: the frame arbitration id pointer
  *  data: pointer to the data array to be received
- *  data_size: pointer to the variable that will hold the size of received data
+ *  data_length: pointer to the variable that will hold the size of received data
  */
-bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_size)
+bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_length)
 {
-    if (!tmcan.read_frame(arbitration_id, data, data_size))
+    ROS_DEBUG_STREAM("Attempting to read CAN frame...");
+
+    scpp::CanFrame fr;
+    scpp::SocketCanStatus read_status = socket_can.read(fr);
+    if (read_status == scpp::STATUS_OK)
     {
-        throw "CAN read error";
+        *arbitration_id = fr.id & CAN_EFF_MASK;
+        *data_length = fr.len;
+        std::copy(fr.data, fr.data + fr.len, data);
+        ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << *arbitration_id << " read successfully.");
+        return true;
+    }
+    else
+    {
+        throw std::runtime_error(SocketCanErrorToString(read_status));
+        return false;
     }
-    return true;
 }
 
 /*
@@ -62,7 +119,7 @@ bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_size)
  */
 void delay_us_cb(uint32_t us)
 {
-  ros::Duration(us * 1e-6).sleep();
+    ros::Duration(us * 1e-6).sleep();
 }
 // ---------------------------------------------------------------
 
@@ -78,19 +135,23 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
     bool got_all_params = true;
 
     // build servo instances from configuration
-    if (got_all_params &= robot_hw_nh.getParam("joints", servos_param)) {
+    if (got_all_params &= robot_hw_nh.getParam("/tinymovr_joint_iface/joints", servos_param)) {
             ROS_ASSERT(servos_param.getType() == XmlRpc::XmlRpcValue::TypeStruct);
         try {
             for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = servos_param.begin(); it != servos_param.end(); ++it) {
 
-                ROS_DEBUG_STREAM("servo: " << (std::string)(it->first));
+                std::string current_joint_name = static_cast<std::string>(it->first);
+                ROS_INFO_STREAM("servo: " << current_joint_name);
+                joint_name.push_back(current_joint_name); // Store joint name
 
                 id_t id;
+                int delay_us;
                 if (it->second.hasMember("id"))
                 {
                     id = static_cast<int>(servos_param[it->first]["id"]);
-                    ROS_DEBUG_STREAM("\tid: " << (int)id);
-                    servos.push_back(Tinymovr(id, &send_cb, &recv_cb, &delay_us_cb));
+                    delay_us = static_cast<int>(servos_param[it->first]["delay_us"]);
+                    ROS_INFO_STREAM("\tid: " << id << " delay_us: " << delay_us);
+                    servos.push_back(Tinymovr(id, &send_cb, &recv_cb, &delay_us_cb, delay_us));
                     servo_modes.push_back(servos_param[it->first]["command_interface"]);
                 }
                 else
@@ -112,8 +173,8 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
     if (!got_all_params) {
         std::string sub_namespace = robot_hw_nh.getNamespace();
         std::string error_message = "One or more of the following parameters "
-                                    "were not set:\n"
-            + sub_namespace + "/servos";
+                            "were not set:\n"
+            + sub_namespace + "/tinymovr_joint_iface/joints";
         ROS_FATAL_STREAM(error_message);
         return false;
     }
@@ -127,18 +188,39 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
     joint_velocity_state.resize(num_joints, 0.0);
     joint_effort_state.resize(num_joints, 0.0);
 
-    tmcan.init();
+    const scpp::SocketCanStatus status = socket_can.open("can0");
+    if (scpp::STATUS_OK == status)
+    {
+        ROS_INFO("Socketcan opened successfully");
+    }
+    else
+    {
+        ROS_ERROR("Cant' open Socketcan: %d", status);
+        exit(1);
+    }
 
     // initialize servos with correct mode
     for (int i=0; i<num_joints; i++)
     {
+        ROS_INFO("Asserting spec compatibility");
+        ROS_ASSERT(servos[i].get_protocol_hash() == avlos_proto_hash);
+        ROS_INFO("Asserting calibrated");
         ROS_ASSERT((servos[i].encoder.get_calibrated() == true) && (servos[i].motor.get_calibrated() == true));
+    }
+
+    for (int i=0; i<num_joints; i++)
+    { 
+        ROS_INFO("Setting state");
         servos[i].controller.set_state(2);
-        servos[i].controller.set_mode(_str2mode(servo_modes[i]));
+        const uint8_t mode = _str2mode(servo_modes[i]);
+        ROS_INFO("Setting mode to %u", mode);
+        servos[i].controller.set_mode(mode);
         ros::Duration(0.001).sleep();
-        ROS_ASSERT((servos[i].controller.get_state() == 2) && (servos[i].controller.get_mode() == 2));
+        ROS_INFO("Asserting state and mode");
+        ROS_ASSERT((servos[i].controller.get_state() == 2) && (servos[i].controller.get_mode() == 1));
     }
 
+    ROS_INFO("Registering Interfaces");
     // register state interfaces
     for (int i=0; i<num_joints; i++)
     {
@@ -151,16 +233,18 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
             );
         joint_state_interface.registerHandle(state_handle);
         
+        const uint8_t mode = _str2mode(servo_modes[i]);
+
         // connect and register the joint position interface
-        hardware_interface::JointHandle pos_handle(joint_state_interface.getHandle("Joint"), &joint_position_command[i]);
+        hardware_interface::JointHandle pos_handle(joint_state_interface.getHandle(joint_name[i]), &joint_position_command[i]);
         joint_pos_interface.registerHandle(pos_handle);
         
         // connect and register the joint velocity interface
-        hardware_interface::JointHandle vel_handle(joint_state_interface.getHandle("Joint"), &joint_velocity_command[i]);
+        hardware_interface::JointHandle vel_handle(joint_state_interface.getHandle(joint_name[i]), &joint_velocity_command[i]);
         joint_vel_interface.registerHandle(vel_handle);
         
         // connect and register the joint effort interface
-        hardware_interface::JointHandle eff_handle(joint_state_interface.getHandle("Joint"), &joint_effort_command[i]);
+        hardware_interface::JointHandle eff_handle(joint_state_interface.getHandle(joint_name[i]), &joint_effort_command[i]);
         joint_eff_interface.registerHandle(eff_handle);
     }
 
@@ -222,4 +306,14 @@ void TinymovrJoint::write(const ros::Time& time, const ros::Duration& period)
     }
 }
 
+void TinymovrJoint::shutdown() 
+{
+    ROS_INFO("Explicitly shutting down Tinymovr instances from hardware interface.");
+    for (int i=0; i<num_joints; i++)
+    { 
+        servos[i].controller.set_state(0);
+        ros::Duration(0.001).sleep();
+    }
+}
+
 }
\ No newline at end of file
diff --git a/src/tm_joint_iface_node.cpp b/src/tm_joint_iface_node.cpp
index 999f0c3..a0bdc57 100644
--- a/src/tm_joint_iface_node.cpp
+++ b/src/tm_joint_iface_node.cpp
@@ -10,7 +10,9 @@ int main(int argc, char **argv)
     ros::AsyncSpinner spinner(1);
     spinner.start();
     ros::NodeHandle nh;
+    
     tinymovr_ros::TinymovrJoint tm_joint;
+
     bool init_success = tm_joint.init(nh, nh);
 
     controller_manager::ControllerManager cm(&tm_joint, nh);
@@ -26,6 +28,8 @@ int main(int argc, char **argv)
         rate.sleep();
     }
 
+    tm_joint.shutdown();
+
     spinner.stop();
     return 0;
 }