diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index e859da6a..c5e3f823 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -94,7 +94,7 @@ void Axis::clear_config() {
     config_ = {};
     config_.step_gpio_pin = default_step_gpio_pin_;
     config_.dir_gpio_pin = default_dir_gpio_pin_;
-    config_.can_node_id = axis_num_;
+    config_.can.node_id = axis_num_;
 }
 
 // @brief Sets up all components of the axis,
@@ -206,7 +206,7 @@ bool Axis::do_updates() {
     min_endstop_.update();
     max_endstop_.update();
     bool ret = check_for_errors();
-    odCAN->send_heartbeat(*this);
+    odCAN->send_cyclic(*this);
     return ret;
 }
 
diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp
index f9e4eb7d..5108ca42 100644
--- a/Firmware/MotorControl/axis.hpp
+++ b/Firmware/MotorControl/axis.hpp
@@ -32,6 +32,13 @@ public:
     static LockinConfig_t default_sensorless();
     static LockinConfig_t default_lockin();
 
+    struct CANConfig_t {
+        uint32_t node_id = 0;
+        bool is_extended = false;
+        uint32_t heartbeat_rate_ms = 100;
+        uint32_t encoder_rate_ms = 10;
+    };
+
     struct Config_t {
         bool startup_motor_calibration = false;   //<! run motor calibration at startup, skip otherwise
         bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
@@ -60,9 +67,8 @@ public:
         LockinConfig_t calibration_lockin = default_calibration();
         LockinConfig_t sensorless_ramp = default_sensorless();
         LockinConfig_t general_lockin;
-        uint32_t can_node_id = 0; // Both axes will have the same id to start
-        bool can_node_id_extended = false;
-        uint32_t can_heartbeat_rate_ms = 100;
+
+        CANConfig_t can;
 
         // custom setters
         Axis* parent = nullptr;
@@ -74,6 +80,11 @@ public:
         bool is_homed = false;
     };
 
+    struct CAN_t {
+        uint32_t last_heartbeat = 0;
+        uint32_t last_encoder = 0;
+    };
+
     enum thread_signals {
         M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
     };
@@ -243,8 +254,9 @@ public:
     AxisState& current_state_ = task_chain_.front();
     uint32_t loop_counter_ = 0;
     LockinState lockin_state_ = LOCKIN_STATE_INACTIVE;
-    Homing_t homing_;
-    uint32_t last_heartbeat_ = 0;
+    Homing_t homing_;    
+    CAN_t can_;
+
 
     // watchdog
     uint32_t watchdog_current_value_= 0;
diff --git a/Firmware/communication/can_helpers.hpp b/Firmware/communication/can_helpers.hpp
index 55d1886d..d1e212c8 100644
--- a/Firmware/communication/can_helpers.hpp
+++ b/Firmware/communication/can_helpers.hpp
@@ -21,6 +21,10 @@ struct can_Signal_t {
     const float offset;
 };
 
+struct can_Cyclic_t {
+    uint32_t cycleTime_ms;
+    uint32_t lastTime_ms;
+};
 
 #include <iterator>
 template <typename T>
diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp
index 1f0f52e3..2c5e1c07 100644
--- a/Firmware/communication/can_simple.cpp
+++ b/Firmware/communication/can_simple.cpp
@@ -3,25 +3,14 @@
 
 #include <odrive_main.h>
 
-static constexpr uint8_t NUM_NODE_ID_BITS = 6;
-static constexpr uint8_t NUM_CMD_ID_BITS = 11 - NUM_NODE_ID_BITS;
-
 void CANSimple::handle_can_message(const can_Message_t& msg) {
-    // This functional way of handling the messages is neat and is much cleaner from
-    // a data security point of view, but it will require some tweaking to fix the syntax.
-    //
-    // auto func = callback_map.find(msg.id);
-    // if(func != callback_map.end()){
-    //     func->second(msg);
-    // }
-
     //     Frame
     // nodeID | CMD
     // 6 bits | 5 bits
     uint32_t nodeID = get_node_id(msg.id);
 
     for (auto& axis : axes) {
-        if ((axis.config_.can_node_id == nodeID) && (axis.config_.can_node_id_extended == msg.isExt)) {
+        if ((axis.config_.can.node_id == nodeID) && (axis.config_.can.is_extended == msg.isExt)) {
             doCommand(axis, msg);
             return;
         }
@@ -43,13 +32,16 @@ void CANSimple::doCommand(Axis& axis, const can_Message_t& msg) {
             estop_callback(axis, msg);
             break;
         case MSG_GET_MOTOR_ERROR:
-            get_motor_error_callback(axis, msg);
+            if (msg.rtr)
+                get_motor_error_callback(axis);
             break;
         case MSG_GET_ENCODER_ERROR:
-            get_encoder_error_callback(axis, msg);
+            if (msg.rtr)
+                get_encoder_error_callback(axis);
             break;
         case MSG_GET_SENSORLESS_ERROR:
-            get_sensorless_error_callback(axis, msg);
+            if (msg.rtr)
+                get_sensorless_error_callback(axis);
             break;
         case MSG_SET_AXIS_NODE_ID:
             set_axis_nodeid_callback(axis, msg);
@@ -61,10 +53,12 @@ void CANSimple::doCommand(Axis& axis, const can_Message_t& msg) {
             set_axis_startup_config_callback(axis, msg);
             break;
         case MSG_GET_ENCODER_ESTIMATES:
-            get_encoder_estimates_callback(axis, msg);
+            if (msg.rtr)
+                get_encoder_estimates_callback(axis);
             break;
         case MSG_GET_ENCODER_COUNT:
-            get_encoder_count_callback(axis, msg);
+            if (msg.rtr)
+                get_encoder_count_callback(axis);
             break;
         case MSG_SET_INPUT_POS:
             set_input_pos_callback(axis, msg);
@@ -94,16 +88,19 @@ void CANSimple::doCommand(Axis& axis, const can_Message_t& msg) {
             set_traj_vel_limit_callback(axis, msg);
             break;
         case MSG_GET_IQ:
-            get_iq_callback(axis, msg);
+            if (msg.rtr)
+                get_iq_callback(axis);
             break;
         case MSG_GET_SENSORLESS_ESTIMATES:
-            get_sensorless_estimates_callback(axis, msg);
+            if (msg.rtr)
+                get_sensorless_estimates_callback(axis);
             break;
         case MSG_RESET_ODRIVE:
             NVIC_SystemReset();
             break;
         case MSG_GET_VBUS_VOLTAGE:
-            get_vbus_voltage_callback(axis, msg);
+            if (msg.rtr)
+                get_vbus_voltage_callback(axis);
             break;
         case MSG_CLEAR_ERRORS:
             clear_errors_callback(axis, msg);
@@ -113,7 +110,6 @@ void CANSimple::doCommand(Axis& axis, const can_Message_t& msg) {
     }
 }
 
-
 void CANSimple::nmt_callback(const Axis& axis, const can_Message_t& msg) {
     // Not implemented
 }
@@ -122,131 +118,120 @@ void CANSimple::estop_callback(Axis& axis, const can_Message_t& msg) {
     axis.error_ |= Axis::ERROR_ESTOP_REQUESTED;
 }
 
-void CANSimple::get_motor_error_callback(const Axis& axis, const can_Message_t& msg) {
-    if (msg.rtr) {
-        can_Message_t txmsg;
-        txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
-        txmsg.id += MSG_GET_MOTOR_ERROR;  // heartbeat ID
-        txmsg.isExt = axis.config_.can_node_id_extended;
-        txmsg.len = 8;
+void CANSimple::get_motor_error_callback(const Axis& axis) {
+    can_Message_t txmsg;
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_MOTOR_ERROR;  // heartbeat ID
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
 
-        can_setSignal(txmsg, axis.motor_.error_, 0, 32, true);
+    can_setSignal(txmsg, axis.motor_.error_, 0, 32, true);
 
-        odCAN->write(txmsg);
-    }
+    odCAN->write(txmsg);
 }
 
-void CANSimple::get_encoder_error_callback(const Axis& axis, const can_Message_t& msg) {
-    if (msg.rtr) {
-        can_Message_t txmsg;
-        txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
-        txmsg.id += MSG_GET_ENCODER_ERROR;  // heartbeat ID
-        txmsg.isExt = axis.config_.can_node_id_extended;
-        txmsg.len = 8;
+void CANSimple::get_encoder_error_callback(const Axis& axis) {
+    can_Message_t txmsg;
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_ENCODER_ERROR;  // heartbeat ID
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
 
-        can_setSignal(txmsg, axis.encoder_.error_, 0, 32, true);
+    can_setSignal(txmsg, axis.encoder_.error_, 0, 32, true);
 
-        odCAN->write(txmsg);
-    }
+    odCAN->write(txmsg);
 }
 
-void CANSimple::get_sensorless_error_callback(const Axis& axis, const can_Message_t& msg) {
-    if (msg.rtr) {
-        can_Message_t txmsg;
-        txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
-        txmsg.id += MSG_GET_SENSORLESS_ERROR;  // heartbeat ID
-        txmsg.isExt = axis.config_.can_node_id_extended;
-        txmsg.len = 8;
+void CANSimple::get_sensorless_error_callback(const Axis& axis) {
+    can_Message_t txmsg;
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_SENSORLESS_ERROR;  // heartbeat ID
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
 
-        can_setSignal(txmsg, axis.sensorless_estimator_.error_, 0, 32, true);
+    can_setSignal(txmsg, axis.sensorless_estimator_.error_, 0, 32, true);
 
-        odCAN->write(txmsg);
-    }
+    odCAN->write(txmsg);
 }
 
 void CANSimple::set_axis_nodeid_callback(Axis& axis, const can_Message_t& msg) {
-    axis.config_.can_node_id = can_getSignal<uint32_t>(msg, 0, 32, true);
+    axis.config_.can.node_id = can_getSignal<uint32_t>(msg, 0, 32, true);
 }
 
 void CANSimple::set_axis_requested_state_callback(Axis& axis, const can_Message_t& msg) {
     axis.requested_state_ = static_cast<Axis::AxisState>(can_getSignal<int32_t>(msg, 0, 16, true));
 }
+
 void CANSimple::set_axis_startup_config_callback(Axis& axis, const can_Message_t& msg) {
     // Not Implemented
 }
 
-void CANSimple::get_encoder_estimates_callback(const Axis& axis, const can_Message_t& msg) {
-    if (msg.rtr) {
-        can_Message_t txmsg;
-        txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
-        txmsg.id += MSG_GET_ENCODER_ESTIMATES;  // heartbeat ID
-        txmsg.isExt = axis.config_.can_node_id_extended;
-        txmsg.len = 8;
+void CANSimple::get_encoder_estimates_callback(const Axis& axis) {
+    can_Message_t txmsg;
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_ENCODER_ESTIMATES;  // heartbeat ID
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
 
-        static_assert(sizeof(float) == sizeof(axis.encoder_.pos_estimate_));
-        static_assert(sizeof(float) == sizeof(axis.encoder_.vel_estimate_));
+    static_assert(sizeof(float) == sizeof(axis.encoder_.pos_estimate_));
+    static_assert(sizeof(float) == sizeof(axis.encoder_.vel_estimate_));
 
-        can_setSignal<float>(txmsg, axis.encoder_.pos_estimate_, 0, 32, true);
-        can_setSignal<float>(txmsg, axis.encoder_.vel_estimate_, 32, 32, true);
+    can_setSignal<float>(txmsg, axis.encoder_.pos_estimate_, 0, 32, true);
+    can_setSignal<float>(txmsg, axis.encoder_.vel_estimate_, 32, 32, true);
 
-        odCAN->write(txmsg);
-    }
+    odCAN->write(txmsg);
 }
 
-void CANSimple::get_sensorless_estimates_callback(const Axis& axis, const can_Message_t& msg) {
-    if (msg.rtr) {
-        can_Message_t txmsg;
-        txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
-        txmsg.id += MSG_GET_SENSORLESS_ESTIMATES;  // heartbeat ID
-        txmsg.isExt = axis.config_.can_node_id_extended;
-        txmsg.len = 8;
+void CANSimple::get_sensorless_estimates_callback(const Axis& axis) {
+    can_Message_t txmsg;
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_SENSORLESS_ESTIMATES;  // heartbeat ID
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
 
-        static_assert(sizeof(float) == sizeof(axis.sensorless_estimator_.pll_pos_));
-        static_assert(sizeof(float) == sizeof(axis.sensorless_estimator_.vel_estimate_));
+    static_assert(sizeof(float) == sizeof(axis.sensorless_estimator_.pll_pos_));
+    static_assert(sizeof(float) == sizeof(axis.sensorless_estimator_.vel_estimate_));
 
-        can_setSignal<float>(txmsg, axis.sensorless_estimator_.pll_pos_, 0, 32, true);
-        can_setSignal<float>(txmsg, axis.sensorless_estimator_.vel_estimate_, 32, 32, true);
+    can_setSignal<float>(txmsg, axis.sensorless_estimator_.pll_pos_, 0, 32, true);
+    can_setSignal<float>(txmsg, axis.sensorless_estimator_.vel_estimate_, 32, 32, true);
 
-        odCAN->write(txmsg);
-    }
+    odCAN->write(txmsg);
 }
 
-void CANSimple::get_encoder_count_callback(const Axis& axis, const can_Message_t& msg) {
-    if (msg.rtr) {
-        can_Message_t txmsg;
-        txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
-        txmsg.id += MSG_GET_ENCODER_COUNT;
-        txmsg.isExt = axis.config_.can_node_id_extended;
-        txmsg.len = 8;
+void CANSimple::get_encoder_count_callback(const Axis& axis) {
+    can_Message_t txmsg;
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_ENCODER_COUNT;
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
 
-        can_setSignal<int32_t>(txmsg, axis.encoder_.shadow_count_, 0, 32, true);
-        can_setSignal<int32_t>(txmsg, axis.encoder_.count_in_cpr_, 32, 32, true);
-        odCAN->write(txmsg);
-    }
+    can_setSignal<int32_t>(txmsg, axis.encoder_.shadow_count_, 0, 32, true);
+    can_setSignal<int32_t>(txmsg, axis.encoder_.count_in_cpr_, 32, 32, true);
+    odCAN->write(txmsg);
 }
 
-void CANSimple::set_input_pos_callback( Axis& axis, const can_Message_t& msg) {
+void CANSimple::set_input_pos_callback(Axis& axis, const can_Message_t& msg) {
     axis.controller_.input_pos_ = can_getSignal<float>(msg, 0, 32, true);
     axis.controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.001f, 0);
     axis.controller_.input_torque_ = can_getSignal<int16_t>(msg, 48, 16, true, 0.001f, 0);
     axis.controller_.input_pos_updated();
 }
 
-void CANSimple::set_input_vel_callback( Axis& axis, const can_Message_t& msg) {
+void CANSimple::set_input_vel_callback(Axis& axis, const can_Message_t& msg) {
     axis.controller_.input_vel_ = can_getSignal<float>(msg, 0, 32, true);
     axis.controller_.input_torque_ = can_getSignal<float>(msg, 32, 32, true);
 }
 
-void CANSimple::set_input_torque_callback( Axis& axis, const can_Message_t& msg) {
+void CANSimple::set_input_torque_callback(Axis& axis, const can_Message_t& msg) {
     axis.controller_.input_torque_ = can_getSignal<float>(msg, 0, 32, true);
 }
 
-void CANSimple::set_controller_modes_callback( Axis& axis, const can_Message_t& msg) {
+void CANSimple::set_controller_modes_callback(Axis& axis, const can_Message_t& msg) {
     axis.controller_.config_.control_mode = static_cast<Controller::ControlMode>(can_getSignal<int32_t>(msg, 0, 32, true));
     axis.controller_.config_.input_mode = static_cast<Controller::InputMode>(can_getSignal<int32_t>(msg, 32, 32, true));
 }
 
-void CANSimple::set_vel_limit_callback( Axis& axis, const can_Message_t& msg) {
+void CANSimple::set_vel_limit_callback(Axis& axis, const can_Message_t& msg) {
     axis.controller_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true);
 }
 
@@ -254,51 +239,47 @@ void CANSimple::start_anticogging_callback(const Axis& axis, const can_Message_t
     axis.controller_.start_anticogging_calibration();
 }
 
-void CANSimple::set_traj_vel_limit_callback( Axis& axis, const can_Message_t& msg) {
+void CANSimple::set_traj_vel_limit_callback(Axis& axis, const can_Message_t& msg) {
     axis.trap_traj_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true);
 }
 
-void CANSimple::set_traj_accel_limits_callback( Axis& axis, const can_Message_t& msg) {
+void CANSimple::set_traj_accel_limits_callback(Axis& axis, const can_Message_t& msg) {
     axis.trap_traj_.config_.accel_limit = can_getSignal<float>(msg, 0, 32, true);
     axis.trap_traj_.config_.decel_limit = can_getSignal<float>(msg, 32, 32, true);
 }
 
-void CANSimple::set_traj_inertia_callback( Axis& axis, const can_Message_t& msg) {
+void CANSimple::set_traj_inertia_callback(Axis& axis, const can_Message_t& msg) {
     axis.controller_.config_.inertia = can_getSignal<float>(msg, 0, 32, true);
 }
 
-void CANSimple::get_iq_callback(const Axis& axis, const can_Message_t& msg) {
-    if (msg.rtr) {
-        can_Message_t txmsg;
-        txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
-        txmsg.id += MSG_GET_IQ;
-        txmsg.isExt = axis.config_.can_node_id_extended;
-        txmsg.len = 8;
+void CANSimple::get_iq_callback(const Axis& axis) {
+    can_Message_t txmsg;
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_IQ;
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
 
-        static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_setpoint));
-        static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_measured));
-        can_setSignal<float>(txmsg, axis.motor_.current_control_.Iq_setpoint, 0, 32, true);
-        can_setSignal<float>(txmsg, axis.motor_.current_control_.Iq_measured, 32, 32, true);
+    static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_setpoint));
+    static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_measured));
+    can_setSignal<float>(txmsg, axis.motor_.current_control_.Iq_setpoint, 0, 32, true);
+    can_setSignal<float>(txmsg, axis.motor_.current_control_.Iq_measured, 32, 32, true);
 
-        odCAN->write(txmsg);
-    }
+    odCAN->write(txmsg);
 }
 
-void CANSimple::get_vbus_voltage_callback(const Axis& axis, const can_Message_t& msg) {
-    if (msg.rtr) {
-        can_Message_t txmsg;
+void CANSimple::get_vbus_voltage_callback(const Axis& axis) {
+    can_Message_t txmsg;
 
-        txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
-        txmsg.id += MSG_GET_VBUS_VOLTAGE;
-        txmsg.isExt = axis.config_.can_node_id_extended;
-        txmsg.len = 8;
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_VBUS_VOLTAGE;
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
 
-        uint32_t floatBytes;
-        static_assert(sizeof(vbus_voltage) == sizeof(floatBytes));
-        can_setSignal<float>(txmsg, vbus_voltage, 0, 32, true);
+    uint32_t floatBytes;
+    static_assert(sizeof(vbus_voltage) == sizeof(floatBytes));
+    can_setSignal<float>(txmsg, vbus_voltage, 0, 32, true);
 
-        odCAN->write(txmsg);
-    }
+    odCAN->write(txmsg);
 }
 
 void CANSimple::clear_errors_callback(Axis& axis, const can_Message_t& msg) {
@@ -307,29 +288,31 @@ void CANSimple::clear_errors_callback(Axis& axis, const can_Message_t& msg) {
 
 void CANSimple::send_heartbeat(const Axis& axis) {
     can_Message_t txmsg;
-    txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
     txmsg.id += MSG_ODRIVE_HEARTBEAT;  // heartbeat ID
-    txmsg.isExt = axis.config_.can_node_id_extended;
+    txmsg.isExt = axis.config_.can.is_extended;
     txmsg.len = 8;
 
-    // Axis errors in 1st 32-bit value
-    txmsg.buf[0] = axis.error_;
-    txmsg.buf[1] = axis.error_ >> 8;
-    txmsg.buf[2] = axis.error_ >> 16;
-    txmsg.buf[3] = axis.error_ >> 24;
+    can_setSignal(txmsg, axis.error_, 0, 32, true);
+    can_setSignal(txmsg, axis.current_state_, 32, 32, true);
 
-    // Current state of axis in 2nd 32-bit value
-    txmsg.buf[4] = axis.current_state_;
-    txmsg.buf[5] = axis.current_state_ >> 8;
-    txmsg.buf[6] = axis.current_state_ >> 16;
-    txmsg.buf[7] = axis.current_state_ >> 24;
     odCAN->write(txmsg);
 }
 
-uint32_t CANSimple::get_node_id(uint32_t msgID) {
-    return (msgID >> NUM_CMD_ID_BITS);  // Upper 6 or more bits
-}
+void CANSimple::send_cyclic(Axis& axis) {
+    const uint32_t now = osKernelSysTick();
 
-uint8_t CANSimple::get_cmd_id(uint32_t msgID) {
-    return (msgID & 0x01F);  // Bottom 5 bits
-}
\ No newline at end of file
+    if (axis.config_.can.heartbeat_rate_ms > 0) {
+        if ((now - axis.can_.last_heartbeat) >= axis.config_.can.heartbeat_rate_ms) {
+            send_heartbeat(axis);
+            axis.can_.last_heartbeat = now;
+        }
+    }
+
+    if (axis.config_.can.encoder_rate_ms > 0) {
+        if ((now - axis.can_.last_encoder) >= axis.config_.can.encoder_rate_ms) {
+            get_encoder_estimates_callback(axis);
+            axis.can_.last_encoder = now;
+        }
+    }
+}
diff --git a/Firmware/communication/can_simple.hpp b/Firmware/communication/can_simple.hpp
index fc0cc906..d05b84a4 100644
--- a/Firmware/communication/can_simple.hpp
+++ b/Firmware/communication/can_simple.hpp
@@ -35,47 +35,54 @@ class CANSimple {
     };
 
     static void handle_can_message(const can_Message_t& msg);
-    static void send_heartbeat(const Axis& axis);
     static void doCommand(Axis& axis, const can_Message_t& cmd);
 
+    // Cyclic Senders
+    static void send_heartbeat(const Axis& axis);
+    static void send_cyclic(Axis& axis);
+
    private:
-    static void nmt_callback(const Axis& axis, const can_Message_t& msg);
-    static void estop_callback(Axis& axis, const can_Message_t& msg);
-    static void get_motor_error_callback(const Axis& axis, const can_Message_t& msg);
-    static void get_encoder_error_callback(const Axis& axis, const can_Message_t& msg);
-    static void get_controller_error_callback(const Axis& axis, const can_Message_t& msg);
-    static void get_sensorless_error_callback(const Axis& axis, const can_Message_t& msg);
+    // Get functions (msg.rtr bit must be set)
+    static void get_motor_error_callback(const Axis& axis);
+    static void get_encoder_error_callback(const Axis& axis);
+    static void get_controller_error_callback(const Axis& axis);
+    static void get_sensorless_error_callback(const Axis& axis);
+    static void get_encoder_estimates_callback(const Axis& axis);
+    static void get_encoder_count_callback(const Axis& axis);
+    static void get_iq_callback(const Axis& axis);
+    static void get_sensorless_estimates_callback(const Axis& axis);
+    static void get_vbus_voltage_callback(const Axis& axis);
+
+    // Set functions
     static void set_axis_nodeid_callback(Axis& axis, const can_Message_t& msg);
     static void set_axis_requested_state_callback(Axis& axis, const can_Message_t& msg);
     static void set_axis_startup_config_callback(Axis& axis, const can_Message_t& msg);
-    static void get_encoder_estimates_callback(const Axis& axis, const can_Message_t& msg);
-    static void get_encoder_count_callback(const Axis& axis, const can_Message_t& msg);
     static void set_input_pos_callback(Axis& axis, const can_Message_t& msg);
     static void set_input_vel_callback(Axis& axis, const can_Message_t& msg);
     static void set_input_torque_callback(Axis& axis, const can_Message_t& msg);
     static void set_controller_modes_callback(Axis& axis, const can_Message_t& msg);
     static void set_vel_limit_callback(Axis& axis, const can_Message_t& msg);
-    static void start_anticogging_callback(const Axis& axis, const can_Message_t& msg);
     static void set_traj_vel_limit_callback(Axis& axis, const can_Message_t& msg);
     static void set_traj_accel_limits_callback(Axis& axis, const can_Message_t& msg);
     static void set_traj_inertia_callback(Axis& axis, const can_Message_t& msg);
-    static void get_iq_callback(const Axis& axis, const can_Message_t& msg);
-    static void get_sensorless_estimates_callback(const Axis& axis, const can_Message_t& msg);
-    static void get_vbus_voltage_callback(const Axis& axis, const can_Message_t& msg);
+
+    // Other functions
+    static void nmt_callback(const Axis& axis, const can_Message_t& msg);
+    static void estop_callback(Axis& axis, const can_Message_t& msg);
     static void clear_errors_callback(Axis& axis, const can_Message_t& msg);
+    static void start_anticogging_callback(const Axis& axis, const can_Message_t& msg);
+
+    static constexpr uint8_t NUM_NODE_ID_BITS = 6;
+    static constexpr uint8_t NUM_CMD_ID_BITS = 11 - NUM_NODE_ID_BITS;
 
     // Utility functions
-    static uint32_t get_node_id(uint32_t msgID);
-    static uint8_t get_cmd_id(uint32_t msgID);
+    static constexpr uint32_t get_node_id(uint32_t msgID) {
+        return (msgID >> NUM_CMD_ID_BITS);  // Upper 6 or more bits
+    };
 
-    // Fetch a specific signal from the message
-
-    // This functional way of handling the messages is neat and is much cleaner from
-    // a data security point of view, but it will require some tweaking
-    //
-    // const std::map<uint32_t, std::function<void(can_Message_t&)>> callback_map = {
-    //     {0x000, std::bind(&CANSimple::heartbeat_callback, this, _1)}
-    // };
+    static constexpr uint8_t get_cmd_id(uint32_t msgID) {
+        return (msgID & 0x01F);  // Bottom 5 bits
+    }
 };
 
 #endif
\ No newline at end of file
diff --git a/Firmware/communication/interface_can.cpp b/Firmware/communication/interface_can.cpp
index 3ca0fb9e..df5873a8 100644
--- a/Firmware/communication/interface_can.cpp
+++ b/Firmware/communication/interface_can.cpp
@@ -174,20 +174,15 @@ void ODriveCAN::reinit_can() {
 void ODriveCAN::set_error(Error error) {
     error_ |= error;
 }
+
 // This function is called by each axis.
 // It provides an abstraction from the specific CAN protocol in use
-void ODriveCAN::send_heartbeat(Axis& axis) {
+void ODriveCAN::send_cyclic(Axis &axis) {
     // Handle heartbeat message
-    if (axis.config_.can_heartbeat_rate_ms > 0) {
-        uint32_t now = osKernelSysTick();
-        if ((now - axis.last_heartbeat_) >= axis.config_.can_heartbeat_rate_ms) {
-            switch (config_.protocol) {
-                case PROTOCOL_SIMPLE:
-                    CANSimple::send_heartbeat(axis);
-                    break;
-            }
-            axis.last_heartbeat_ = now;
-        }
+    switch (config_.protocol) {
+        case PROTOCOL_SIMPLE:
+            CANSimple::send_cyclic(axis);
+            break;
     }
 }
 
diff --git a/Firmware/communication/interface_can.hpp b/Firmware/communication/interface_can.hpp
index 4e72b174..715d6625 100644
--- a/Firmware/communication/interface_can.hpp
+++ b/Firmware/communication/interface_can.hpp
@@ -35,7 +35,7 @@ class ODriveCAN : public ODriveIntf::CanIntf {
     volatile bool thread_id_valid_ = false;
     bool start_can_server();
     void can_server_thread();
-    void send_heartbeat(Axis& axis);
+    void send_cyclic(Axis& axis);
     void reinit_can();
 
     void set_error(Error error);
diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml
index 1493e899..73f2debd 100644
--- a/Firmware/odrive-interface.yaml
+++ b/Firmware/odrive-interface.yaml
@@ -417,11 +417,7 @@ interfaces:
               vel: float32
           sensorless_ramp: LockinConfig
           general_lockin: LockinConfig
-          can_node_id:
-            type: uint32
-            doc: Both axes will have the same id to start
-          can_node_id_extended: bool
-          can_heartbeat_rate_ms: uint32
+          can: CANConfig
         gate_driver:
           c_name: gate_driver_exported_
           c_is_class: False
@@ -485,6 +481,14 @@ interfaces:
       finish_on_distance: bool
       finish_on_enc_idx: bool
 
+  ODrive.Axis.CANConfig:
+    c_is_class: False
+    attributes:
+      node_id: uint32
+      is_extended: bool
+      heartbeat_rate_ms: uint32
+      encoder_rate_ms: uint32
+
   ODrive.ThermistorCurrentLimiter:
     c_is_class: False