diff --git a/fw/src/.clang-format b/fw/src/.clang-format
index 09ac0c6..44c2cf0 100644
--- a/fw/src/.clang-format
+++ b/fw/src/.clang-format
@@ -1,3 +1,3 @@
 Language: Cpp
 BasedOnStyle:  google
-ColumnLimit: 120
\ No newline at end of file
+ColumnLimit: 140
\ No newline at end of file
diff --git a/fw/src/DRV8323S.hpp b/fw/src/DRV8323S.hpp
index 322c3db..9930c2a 100644
--- a/fw/src/DRV8323S.hpp
+++ b/fw/src/DRV8323S.hpp
@@ -199,6 +199,9 @@ void drv8323s_set_sen_lvl(struct drv8323s_foc_driver *dev,
 
 void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
                    int CSn, int en, SPIClass *spi) {
+  if(dev == NULL || spi == NULL) {
+    return;
+  }
   dev->CSn = CSn;
   dev->spi = spi;
   dev->en = en;
@@ -211,7 +214,7 @@ void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
   /* Set 3PWM Mode */
   drv8323s_set_PWM_mode(dev, DRV8323S_PWM_MODE_3PWM);
   delay(1);
-  // drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED);
+  drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED);
   delay(1);
   drv8323s_set_sen_lvl(dev, DRV8323S_SEN_LVL_0_25V);
   delay(1);
diff --git a/fw/src/main.cpp b/fw/src/main.cpp
index 4d82d21..ff35b1d 100644
--- a/fw/src/main.cpp
+++ b/fw/src/main.cpp
@@ -5,6 +5,7 @@ reg.pid_position: [380.0, 0.0, 0.20000000298023224, 4.999999873689376e-05, 10000
 
 #include <cmath>
 
+#include "BLDCMotor.h"
 #include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
 #include "encoders/as5047/AS5047.h"
 #include "esp32-hal.h"
@@ -15,7 +16,7 @@ reg.pid_position: [380.0, 0.0, 0.20000000298023224, 4.999999873689376e-05, 10000
 #define VCS_REVISION_ID 0
 
 #include "Arduino.h"
-#include "pin_def.h"
+#include "pin_def_v5.h"
 
 #define USE_USBSERIAL
 #ifdef USE_USBSERIAL
@@ -35,25 +36,26 @@ USBCDC usbserial;
 #include "canard.c"
 #include "udral_servo.hpp"
 #define spi_dev SPI
+SPIClass spi_drv(FSPI);
 // #include "Wire.h"
 #include <SimpleFOC.h>
 
+#include "DRV8323S.hpp"
+#include "HybridStepperMotor.h"
 #include "SimpleFOCDrivers.h"
 #include "encoders/abs_inc_combine/AbsIncCombineSensor.h"
 #include "encoders/as5047/MagneticSensorAS5047.h"
 #include "encoders/calibrated/CalibratedSensor.h"
-
-#include "DRV8323S.hpp"
-#include "HybridStepperMotor.h"
 #include "esp32-hal-adc.h"
 #include "esp32-hal-gpio.h"
 
 #define TEMP(v0, b, rt, r1, vdd) ((1.0 / ((1.0 / 298.15) + ((1.0 / b) * log((v0 * r1) / (rt * (vdd - v0)))))) - 273.15)
 #define temp_pcb (TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, 3.3))
+#define temp_mot (analogRead(TERMISTOR_EXT) > 0.95 ? NAN : (TEMP(analogRead(TERMISTOR_EXT) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, 3.3)))
 
 #define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
-//* 20.8f/ 21033.75)
-//#define ESPHWENC
+
+// #define ESPHWENC
 #define MOTOR
 #define FW_NO_WATCHDOG
 
@@ -67,13 +69,11 @@ USBCDC usbserial;
 // };
 #endif
 
-const int voltage_lpf = 50;
-const float max_voltage_limit = 2.3;
-
+float max_voltage_limit = 0;
 
 #ifdef ESPHWENC
 #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
-ESP32HWEncoder encoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted
+ESP32HWEncoder encoder(ENC_A, ENC_B, 4096 / 4);  // The Index pin can be omitted
 #else
 Encoder encoder(ENC_A, ENC_B, 4096 / 4);
 void doA1() { encoder.handleA(); }
@@ -87,16 +87,15 @@ CalibratedSensor encoder_calibrated(abs_inc_sensor, sizeof(encoder_calibration_l
                                     encoder_calibration_lut);
 // MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36);
 
-#ifdef MOTOR
 struct drv8323s_foc_driver drv8323s;
 // BLDCMotor motor = BLDCMotor(100); // 24N22P
-//HybridStepperMotor motor = HybridStepperMotor(50);  //, 3.7, 10, 4.5 / 1000);
 
-
-BLDCMotor motor = BLDCMotor(7, 0.7, 360, 0.15);
+HybridStepperMotor motorHybridStepper = HybridStepperMotor(50);  //, 3.7, 10, 4.5 / 1000);
+BLDCMotor motorBLDC = BLDCMotor(7, 0.7, 360, 0.15);
+FOCMotor *motor = NULL;
 // BLDCMotor motor = BLDCMotor(7);
-//BLDCDriver3PWM driver = BLDCDriver3PWM(16, 5, 17, 4);
-#endif
+// BLDCDriver3PWM driver = BLDCDriver3PWM(16, 5, 17, 4);
+
 void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
   Wire.beginTransmission(addr);
   Wire.write(reg);  // MAN:0x2E
@@ -139,21 +138,25 @@ void mutexed_state_sync(UdralServoState *const state, const float hz) {
   xSemaphoreTake(mutex, portMAX_DELAY);
   if (!init_done) {
     init_done = true;
+    motor = state->motor_type == MOTOR_TYPE_HYBRID_STEPPER ? (FOCMotor *)&motorHybridStepper : (FOCMotor *)&motorBLDC;
+    state->max_voltage.value.count = 2;
+    state->max_voltage.value.elements[0] = 0.01; /* max voltage*/
+    state->max_voltage.value.elements[1] = 0.01; /* aligning voltage */
     state->pid_position.value.count = 6;
-    state->pid_position.value.elements[0] = motor.P_angle.P;
-    state->pid_position.value.elements[1] = motor.P_angle.I;
-    state->pid_position.value.elements[2] = motor.P_angle.D;
-    state->pid_position.value.elements[3] = motor.LPF_angle.Tf;
-    state->pid_position.value.elements[4] = motor.P_angle.output_ramp;
-    state->pid_position.value.elements[5] = motor.P_angle.limit;
+    state->pid_position.value.elements[0] = motor->P_angle.P;
+    state->pid_position.value.elements[1] = motor->P_angle.I;
+    state->pid_position.value.elements[2] = motor->P_angle.D;
+    state->pid_position.value.elements[3] = motor->LPF_angle.Tf;
+    state->pid_position.value.elements[4] = motor->P_angle.output_ramp;
+    state->pid_position.value.elements[5] = motor->P_angle.limit;
     state->pid_velocity.value.count = 7;
-    state->pid_velocity.value.elements[0] = motor.PID_velocity.P;
-    state->pid_velocity.value.elements[1] = motor.PID_velocity.I;
-    state->pid_velocity.value.elements[2] = motor.PID_velocity.D;
-    state->pid_velocity.value.elements[3] = motor.LPF_velocity.Tf;
-    state->pid_velocity.value.elements[4] = motor.PID_velocity.output_ramp;
-    state->pid_velocity.value.elements[5] = motor.PID_velocity.limit;
-    state->pid_velocity.value.elements[6] = motor.motion_downsample;
+    state->pid_velocity.value.elements[0] = motor->PID_velocity.P;
+    state->pid_velocity.value.elements[1] = motor->PID_velocity.I;
+    state->pid_velocity.value.elements[2] = motor->PID_velocity.D;
+    state->pid_velocity.value.elements[3] = motor->LPF_velocity.Tf;
+    state->pid_velocity.value.elements[4] = motor->PID_velocity.output_ramp;
+    state->pid_velocity.value.elements[5] = motor->PID_velocity.limit;
+    state->pid_velocity.value.elements[6] = motor->motion_downsample;
   }
   servo_state.target_force = state->target_force;
   servo_state.target_acceleration = state->target_acceleration;
@@ -210,7 +213,8 @@ void setup() {
   digitalWrite(CAL_PIN, 0);  // enable
 
   pinMode(SPI_MISO, INPUT_PULLUP);
-  SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
+  spi_dev.begin(SPI_CLK, SPI_MISO, SPI_MOSI);
+  spi_drv.begin(SPI_DRV_CLK, SPI_DRV_MISO, SPI_DRV_MOSI, SPI_DRV_SC);
   // SPI.setFrequency(100000);
   //  initialise magnetic sensor hardware
   encoder.pullup = Pullup::USE_INTERN;
@@ -222,67 +226,22 @@ void setup() {
 
   abs_inc_sensor.init();
 
+  drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN,&spi_drv); //&spi_drv);
+
+  motorHybridStepper.linkSensor(&encoder_absolute);
+  motorHybridStepper.linkDriver(drv8323s.focdriver);
+  motorBLDC.linkSensor(&encoder_absolute);
+  motorBLDC.linkDriver(drv8323s.focdriver);
+
   Serial.printf("offset: %f\nvalue: %d", abs_inc_sensor.offset, abs_inc_sensor.getAngle());
-#ifdef MOTOR
+
   SimpleFOCDebug::enable(&Serial);
-  drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &spi_dev);
-  digitalWrite(INLABC, 1);  // enable
-  // driver.init(&SPI);
-  // status();
-  motor.linkSensor(&encoder_absolute);
-  //motor.linkSensor(&encoder_calibrated);
-  motor.linkDriver(drv8323s.focdriver);
-  // motor.foc_modulation = FOCModulationType::SinePWM;
-  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
-
-  motor.voltage_sensor_align = 1.5;
-  motor.voltage_limit = max_voltage_limit;
-  // motor.controller = MotionControlType::velocity_openloop;
-  // motor.controller = MotionControlType::velocity;
-  motor.controller = MotionControlType::torque;
-  motor.torque_controller = TorqueControlType::voltage;
-  motor.useMonitoring(Serial);
-  drv8323s.focdriver->voltage_power_supply = vdrive_read;
-  motor.init();
-  delay(1000);
-
-  static Preferences pref;
-  pref.begin("simpleFOC");
-
-  // set voltage to run calibration
-  encoder_calibrated.voltage_calibration = 3;
-  encoder_calibrated.init();
-  // Running calibration
-  const char *encoder_calibration_lut_str = "enc_cal_lut";
-  const char *zero_electric_angle_str = "zero_el_angle";
-  const char *sensor_direction_str = "enc_dir_str";
-  const int settle_time_ms = 150;
-  if (motor.sensor == &encoder_calibrated && (!pref.isKey(encoder_calibration_lut_str) || pref.getBytesLength(encoder_calibration_lut_str) != sizeof(encoder_calibration_lut))) {
-    encoder_calibrated.calibrate(motor, settle_time_ms);
-    pref.putBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
-    pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
-    pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
-
-  } else {
-    pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
-    Serial.println("Skipping calibration");
-  }
-
-  if (!pref.isKey(zero_electric_angle_str) || motor.sensor != &encoder_calibrated) {
-    motor.initFOC();
-    pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
-    pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
-  } else {
-    motor.zero_electric_angle = pref.getFloat(zero_electric_angle_str);
-    motor.sensor_direction = (Direction)pref.getInt(sensor_direction_str);
-    motor.initFOC();
-  }
-  pref.end();
-
-  motor.move(0);
-  digitalWrite(INLABC, 0);  // enable
-#endif
-
+  digitalWrite(LED_PIN, 1);  // disable
+  delay(100);
+  digitalWrite(LED_PIN, 0);  // enable
+  delay(400);
+  digitalWrite(LED_PIN, 1);  // disable
+  delay(4000);
   xTaskCreatePinnedToCore(&comm_task,       // Function name of the task
                           "comm",           // Name of the task (e.g. for debugging)
                           65536,            // Stack size (bytes)
@@ -294,12 +253,7 @@ void setup() {
   // esp_task_wdt_init(WDT_TIMEOUT_s, true);  // enable panic so ESP32 restarts
   // esp_task_wdt_add(NULL);                  // add current thread to WDT watch
 
-  digitalWrite(LED_PIN, 1);  // disable
-  delay(100);
-  digitalWrite(LED_PIN, 0);  // enable
-  delay(400);
-  digitalWrite(LED_PIN, 1);  // disable
-  delay(100);
+
   while (!init_done) {
     delay(10);
   }
@@ -314,100 +268,189 @@ void setup() {
   );
 }
 
+bool init_motor() {
+#ifndef MOTOR
+  return false;
+#endif
+  if (motor == NULL) {
+    return false;
+  }
+  static bool is_init = false;
+  if (!is_init) {
+    if (motor->voltage_limit <= 0 || motor->voltage_sensor_align <= 0) {
+      return false;
+    }
+    is_init = true;
+  } else {
+    return true;
+  }
+
+  digitalWrite(INLABC, 1);  // enable
+  // driver.init(&SPI);
+  // status();
+  // motor->foc_modulation = FOCModulationType::SinePWM;
+  motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
+
+  // motor->controller = MotionControlType::velocity_openloop;
+  // motor->controller = MotionControlType::velocity;
+  motor->controller = MotionControlType::torque;
+  motor->torque_controller = TorqueControlType::voltage;
+  motor->useMonitoring(Serial);
+  drv8323s.focdriver->voltage_power_supply = vdrive_read;
+  motor->init();
+  delay(1000);
+
+  static Preferences pref;
+  pref.begin("simpleFOC");
+
+  // set voltage to run calibration
+  encoder_calibrated.voltage_calibration = motor->voltage_sensor_align;
+  encoder_calibrated.init();
+  // Running calibration
+  const char *encoder_calibration_lut_str = "enc_cal_lut";
+  const char *zero_electric_angle_str = "zero_el_angle";
+  const char *sensor_direction_str = "enc_dir_str";
+  const int settle_time_ms = 150;
+
+  const bool calibration_saveable = motor->sensor == &encoder_calibrated;
+
+  if (calibration_saveable &&
+      (!pref.isKey(encoder_calibration_lut_str) || pref.getBytesLength(encoder_calibration_lut_str) != sizeof(encoder_calibration_lut))) {
+    encoder_calibrated.calibrate(*motor, settle_time_ms);
+    pref.putBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
+    pref.putFloat(zero_electric_angle_str, motor->zero_electric_angle);
+    pref.putInt(sensor_direction_str, (int)motor->sensor_direction);
+  } else {
+    pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0], sizeof(encoder_calibration_lut));
+    Serial.println("Skipping calibration");
+  }
+
+  if (pref.isKey(zero_electric_angle_str) && calibration_saveable) {
+    motor->zero_electric_angle = pref.getFloat(zero_electric_angle_str);
+    motor->sensor_direction = (Direction)pref.getInt(sensor_direction_str);
+    motor->initFOC();
+  } else {
+    motor->initFOC();
+    if (calibration_saveable) {
+      pref.putFloat(zero_electric_angle_str, motor->zero_electric_angle);
+      pref.putInt(sensor_direction_str, (int)motor->sensor_direction);
+    }
+  }
+  pref.end();
+
+  motor->move(0);
+  digitalWrite(INLABC, 0);  // enable
+  return true;
+}
+
 int i = 0;
 void loop() {}
 
 void foc_task_loop() {
-#ifdef MOTOR
+  if(motor == NULL) {
+    delay(10);
+    return;
+  }
   bool armed = false;
   /* adc reading */
   drv8323s.focdriver->voltage_power_supply = vdrive_read;
   const float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
+  const float curr_mot_temp_kelvin = temp_mot + 273.15f;
 
   xSemaphoreTake(mutex, portMAX_DELAY);
   armed = servo_state.arming.armed;
+  max_voltage_limit = servo_state.max_voltage.value.count <= 0 ? 0 : servo_state.max_voltage.value.elements[0];
+  motor->voltage_sensor_align = servo_state.max_voltage.value.count <= 1 ? 0 : servo_state.max_voltage.value.elements[1];
 
   float target;
   if (!std::isnan(servo_state.target_position)) {
-    if (motor.controller != MotionControlType::angle) {
-      motor.controller = MotionControlType::angle;
-      Serial.println("MotionControlType::angle");
+    auto target_control_type = (is_openloop(servo_state.motor_type) ? MotionControlType::angle_openloop : MotionControlType::angle);
+    if (motor->controller != MotionControlType::angle) {
+      motor->controller = MotionControlType::angle;
+      Serial.printf("MotionControlType::angle%s\n", is_openloop(servo_state.motor_type) ? "_openloop" : "");
     }
     target = servo_state.target_position;
 
-    motor.velocity_limit = std::isnan(servo_state.target_velocity) ? 10000 : servo_state.target_velocity;
-    motor.voltage_limit =
+    motor->velocity_limit = std::isnan(servo_state.target_velocity) ? 10000 : servo_state.target_velocity;
+    motor->voltage_limit =
         std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force));
   } else if (!std::isnan(servo_state.target_velocity)) {
-    if (motor.controller != MotionControlType::velocity) {
-      motor.controller = MotionControlType::velocity;
-      Serial.println("MotionControlType::velocity");
+    auto target_control_type = (is_openloop(servo_state.motor_type) ? MotionControlType::velocity_openloop : MotionControlType::velocity);
+    if (motor->controller != target_control_type) {
+      motor->controller = target_control_type;
+      Serial.printf("MotionControlType::velocity%s\n", is_openloop(servo_state.motor_type) ? "_openloop" : "");
     }
     target = servo_state.target_velocity;
-    motor.voltage_limit =
+    motor->voltage_limit =
         std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force));
   } else {
-    motor.voltage_limit =
+    if (is_openloop(servo_state.motor_type)) {
+      armed = false;
+    }
+    motor->voltage_limit =
         std::isnan(servo_state.target_force) ? max_voltage_limit : std::min(max_voltage_limit, std::abs(servo_state.target_force));
     target = std::isnan(servo_state.target_force) ? 0 : servo_state.target_force;
-    if (motor.controller != MotionControlType::torque) {
-      motor.controller = MotionControlType::torque;
+    if (motor->controller != MotionControlType::torque) {
+      motor->controller = MotionControlType::torque;
       Serial.printf("MotionControlType::torque %f\n", target);
     }
   }
 
   servo_state.controller_temperature = curr_pcb_temp_kelvin;
-  servo_state.motor_temperature = NAN;
+  servo_state.motor_temperature = curr_mot_temp_kelvin;
 
-  float curr_angle = motor.sensor->getAngle();
+  float curr_angle = motor->sensor->getAngle();
   servo_state.curr_position = curr_angle;
-  servo_state.curr_force = motor.voltage.d;
+  servo_state.curr_force = motor->voltage.d;
 
   servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
-  servo_state.current = motor.current.d;
+  servo_state.current = motor->current.d;
 
-  motor.PID_velocity.P = servo_state.pid_velocity.value.count <= 0 ? 0 : servo_state.pid_velocity.value.elements[0];
-  motor.PID_velocity.I = servo_state.pid_velocity.value.count <= 1 ? 0 : servo_state.pid_velocity.value.elements[1];
-  motor.PID_velocity.D = servo_state.pid_velocity.value.count <= 2 ? 0 : servo_state.pid_velocity.value.elements[2];
-  motor.LPF_velocity.Tf = servo_state.pid_velocity.value.count <= 3 ? 0.01 : servo_state.pid_velocity.value.elements[3];
-  motor.PID_velocity.output_ramp = servo_state.pid_velocity.value.count <= 4 ? 300 : servo_state.pid_velocity.value.elements[4];
-  motor.PID_velocity.limit = servo_state.pid_velocity.value.count <= 5 ? 1000 : servo_state.pid_velocity.value.elements[5];
-  motor.motion_downsample = servo_state.pid_velocity.value.count <= 6 ? 0 : servo_state.pid_velocity.value.elements[6];
+  motor->PID_velocity.P = servo_state.pid_velocity.value.count <= 0 ? 0 : servo_state.pid_velocity.value.elements[0];
+  motor->PID_velocity.I = servo_state.pid_velocity.value.count <= 1 ? 0 : servo_state.pid_velocity.value.elements[1];
+  motor->PID_velocity.D = servo_state.pid_velocity.value.count <= 2 ? 0 : servo_state.pid_velocity.value.elements[2];
+  motor->LPF_velocity.Tf = servo_state.pid_velocity.value.count <= 3 ? 0.01 : servo_state.pid_velocity.value.elements[3];
+  motor->PID_velocity.output_ramp = servo_state.pid_velocity.value.count <= 4 ? 300 : servo_state.pid_velocity.value.elements[4];
+  motor->PID_velocity.limit = servo_state.pid_velocity.value.count <= 5 ? 1000 : servo_state.pid_velocity.value.elements[5];
+  motor->motion_downsample = servo_state.pid_velocity.value.count <= 6 ? 0 : servo_state.pid_velocity.value.elements[6];
 
-  motor.P_angle.P = servo_state.pid_position.value.count <= 0 ? 0 : servo_state.pid_position.value.elements[0];
-  motor.P_angle.I = servo_state.pid_position.value.count <= 1 ? 0 : servo_state.pid_position.value.elements[1];
-  motor.P_angle.D = servo_state.pid_position.value.count <= 2 ? 0 : servo_state.pid_position.value.elements[2];
-  motor.LPF_angle.Tf = servo_state.pid_position.value.count <= 3 ? 0 : servo_state.pid_position.value.elements[3];
-  motor.P_angle.output_ramp = servo_state.pid_position.value.count <= 4 ? 1e6 : servo_state.pid_position.value.elements[4];
-  motor.P_angle.limit = servo_state.pid_position.value.count <= 5 ? 1000 : servo_state.pid_position.value.elements[5];
-  // motor.P_angle.output_ramp = 10000; // default 1e6 rad/s^2
+  motor->P_angle.P = servo_state.pid_position.value.count <= 0 ? 0 : servo_state.pid_position.value.elements[0];
+  motor->P_angle.I = servo_state.pid_position.value.count <= 1 ? 0 : servo_state.pid_position.value.elements[1];
+  motor->P_angle.D = servo_state.pid_position.value.count <= 2 ? 0 : servo_state.pid_position.value.elements[2];
+  motor->LPF_angle.Tf = servo_state.pid_position.value.count <= 3 ? 0 : servo_state.pid_position.value.elements[3];
+  motor->P_angle.output_ramp = servo_state.pid_position.value.count <= 4 ? 1e6 : servo_state.pid_position.value.elements[4];
+  motor->P_angle.limit = servo_state.pid_position.value.count <= 5 ? 1000 : servo_state.pid_position.value.elements[5];
+
+  // motor->P_angle.output_ramp = 10000; // default 1e6 rad/s^2
   /* angle low pass filtering, use only for very noisy position sensors - try to avoid and keep the values very small */
   // setting the limits
 
   xSemaphoreGive(mutex);
-
-  if (!armed) {
-    if (motor.enabled) {
-      motor.move(0);
-      motor.disable();
-      digitalWrite(INLABC, 0);
+  if (init_motor()) {
+    if (!armed) {
+      if (motor->enabled) {
+        motor->move(0);
+        motor->disable();
+        digitalWrite(INLABC, 0);
+        digitalWrite(LED_PIN, HIGH);
+      }
+    } else {
+      if (!motor->enabled) {
+        digitalWrite(INLABC, 1);
+        motor->enable();
+        digitalWrite(LED_PIN, LOW);
+      }
+      motor->move(target);
     }
+    motor->loopFOC();
   } else {
-    if (!motor.enabled) {
-      digitalWrite(INLABC, 1);
-      motor.enable();
-    }
-    motor.move(target);
+    digitalWrite(LED_PIN, HIGH);
+    delay(10);
+    //if(motor && motor->sensor) {
+    //  motor->sensor->update();
+    //}
   }
-
-  digitalWrite(LED_PIN, armed ? LOW : HIGH);
-
-  motor.loopFOC();
-#else
-  encoder.update();  // optional: Update manually if not using loopfoc()
-#endif
 }
-
 void foc_task(void *parameter) {
   while (true) {
     foc_task_loop();
diff --git a/fw/src/pin_def.h b/fw/src/pin_def_v4.h
similarity index 100%
rename from fw/src/pin_def.h
rename to fw/src/pin_def_v4.h
diff --git a/fw/src/pin_def_v5.h b/fw/src/pin_def_v5.h
new file mode 100644
index 0000000..7edbab7
--- /dev/null
+++ b/fw/src/pin_def_v5.h
@@ -0,0 +1,35 @@
+#pragma once
+#define ENC_A 11
+#define ENC_B 8
+
+#define SPI_ACC_nCS 0
+#define SPI_ENC_nCS 18
+
+#define SPI_CLK 40
+#define SPI_MISO 38
+#define SPI_MOSI 39
+
+#define DRV_nFAULT 15
+#define DRVEN 16
+#define TERMISTOR_PCB 3
+#define TERMISTOR_EXT 9
+#define SOA 1
+#define SOB 2
+#define SOC 10
+
+#define INHA 14
+#define INHB 12
+#define INHC 13
+#define INLABC 46
+
+#define CAN_TX 6
+#define CAN_RX 7
+
+#define SPI_DRV_MISO 45
+#define SPI_DRV_MOSI 48
+#define SPI_DRV_CLK 47
+#define SPI_DRV_SC 21
+
+#define LED_PIN 42
+#define VSENSE_PIN 5
+#define CAL_PIN 17
\ No newline at end of file
diff --git a/fw/src/ringbuf.hpp b/fw/src/ringbuf.hpp
new file mode 100644
index 0000000..39866d1
--- /dev/null
+++ b/fw/src/ringbuf.hpp
@@ -0,0 +1,67 @@
+/*
+ *
+ * Copyright (c) [2022] by InvenSense, Inc.
+ * 
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+#include "ICM42670P.h"
+
+// Instantiate an ICM42670 with SPI interface and CS on pin 8
+ICM42670 IMU(SPI,8);
+
+void setup() {
+  int ret;
+  Serial.begin(115200);
+  while(!Serial) {}
+
+  // Initializing the ICM42670
+  ret = IMU.begin();
+  if (ret != 0) {
+    Serial.print("ICM42670 initialization failed: ");
+    Serial.println(ret);
+    while(1);
+  }
+  // Accel ODR = 100 Hz and Full Scale Range = 16G
+  IMU.startAccel(100,16);
+  // Gyro ODR = 100 Hz and Full Scale Range = 2000 dps
+  IMU.startGyro(100,2000);
+  // Wait IMU to start
+  delay(100);
+}
+
+void loop() {
+
+  inv_imu_sensor_event_t imu_event;
+
+  // Get last event
+  IMU.getDataFromRegisters(imu_event);
+
+  // Format data for Serial Plotter
+  Serial.print("AccelX:");
+  Serial.println(imu_event.accel[0]);
+  Serial.print("AccelY:");
+  Serial.println(imu_event.accel[1]);
+  Serial.print("AccelZ:");
+  Serial.println(imu_event.accel[2]);
+  Serial.print("GyroX:");
+  Serial.println(imu_event.gyro[0]);
+  Serial.print("GyroY:");
+  Serial.println(imu_event.gyro[1]);
+  Serial.print("GyroZ:");
+  Serial.println(imu_event.gyro[2]);
+  Serial.print("Temperature:");
+  Serial.println(imu_event.temperature);
+
+  // Run @ ODR 100Hz
+  delay(10);
+}
\ No newline at end of file
diff --git a/fw/src/udral_servo.hpp b/fw/src/udral_servo.hpp
index 4b7ad0f..1a78eac 100644
--- a/fw/src/udral_servo.hpp
+++ b/fw/src/udral_servo.hpp
@@ -16,7 +16,7 @@
 #define NUNAVUT_ASSERT(x) assert(x)
 #include "canard.h"
 #include "esp32-hal.h"
-#include "pin_def.h"
+#include "pin_def_v5.h"
 
 // #include "socketcan.h"
 #include <assert.h>
@@ -60,6 +60,17 @@ const unsigned int hz = 50;
 /// We keep the state of the application here. Feel free to use static variables
 /// instead if desired.
 
+enum motor_type_t {
+  MOTOR_TYPE_BLDC_OPENLOOP = 0,
+  MOTOR_TYPE_BLDC = 1,
+  MOTOR_TYPE_HYBRID_STEPPER_OPENLOOP = 2,
+  MOTOR_TYPE_HYBRID_STEPPER = 3,
+};
+
+bool is_openloop(const enum motor_type_t t) {
+  return !(((int)t)&0b1);
+}
+
 struct UdralServoState {
   /// Whether the servo is supposed to actuate the load or to stay idle (safe
   /// low-power mode).
@@ -93,6 +104,9 @@ struct UdralServoState {
 
     uavcan_primitive_array_Real32_1_0 pid_position;
     uavcan_primitive_array_Real32_1_0 pid_velocity;
+
+    uavcan_primitive_array_Real32_1_0 max_voltage;
+    enum motor_type_t motor_type;
   };
 };
 
@@ -143,6 +157,8 @@ struct UdralServoInternalState {
   state_sync_f user_state_sync_f;
 };
 
+static struct UdralServoInternalState state;
+
 /// This flag is raised when the node is requested to restart.
 static volatile bool g_restart_required = false;
 
@@ -583,25 +599,58 @@ static uavcan_register_Access_Response_1_0 processRequestRegisterAccess(const ua
   name[req->name.name.count] = '\0';
 
   uavcan_register_Access_Response_1_0 resp = {0};
-  // Serial.println("processRequestRegisterAccess");
-  // Serial.print("name: ");
-  // Serial.println((char *)&req->name.name);
+  Serial.printf("processRequestRegisterAccess name: %s\n\r", (char *)&req->name.name);
+  if (strcmp((char*)&req->name.name.elements[0], "reg.motor_type") == 0) {
+    uavcan_register_Value_1_0_select_string_(&resp.value);
+    if (!uavcan_register_Value_1_0_is_string_(&req->value)) {
+      if (state.servo.motor_type == MOTOR_TYPE_HYBRID_STEPPER) {
+        strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper");
+      } else if (state.servo.motor_type == MOTOR_TYPE_HYBRID_STEPPER_OPENLOOP) {
+        strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper_openloop");
+      } else if (state.servo.motor_type == MOTOR_TYPE_BLDC) {
+        strcpy((char*)&resp.value._string.value.elements[0], "BLDC");
+      } else {
+        strcpy((char*)&resp.value._string.value.elements[0], "BLDC_openloop");
+      }
+      resp.value._string.value.count = strlen((char*)&resp.value._string.value.elements[0]);
 
-  // If we're asked to write a new value, do it now:
-  if (!uavcan_register_Value_1_0_is_empty_(&req->value)) {
+      //Serial.printf("resp reg.motor_type: %s (%d)\n\r",(char*)&resp.value._string.value.elements[0], resp.value._string.value.count);
+      Serial.printf("req  reg.motor_type: %s (%d)\n\r",(char*)&req->value._string.value.elements[0], req->value._string.value.count);
+      registerWrite("reg.motor_type", &resp.value);
+    }
+    if (strcmp((char*)&resp.value._string.value.elements[0], "HybridStepper") == 0) {
+      state.servo.motor_type = MOTOR_TYPE_HYBRID_STEPPER;
+      strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper");
+
+    } else  if (strcmp((char*)&resp.value._string.value.elements[0], "HybridStepper_openloop") == 0) {
+      state.servo.motor_type = MOTOR_TYPE_HYBRID_STEPPER_OPENLOOP;
+      strcpy((char*)&resp.value._string.value.elements[0], "HybridStepper_openloop");
+
+    } else  if (strcmp((char*)&resp.value._string.value.elements[0], "BLDC") == 0) {
+      state.servo.motor_type = MOTOR_TYPE_BLDC;
+      strcpy((char*)&resp.value._string.value.elements[0], "BLDC");
+    } else {
+      state.servo.motor_type = MOTOR_TYPE_BLDC_OPENLOOP;
+      strcpy((char*)&resp.value._string.value.elements[0], "BLDC_openloop");
+    }
+    resp.value._string.value.count = strlen((char*)&resp.value._string.value.elements[0]);
+    registerWrite("reg.motor_type", &resp.value);
+  } else {
+    // If we're asked to write a new value, do it now:
+    if (!uavcan_register_Value_1_0_is_empty_(&req->value)) {
+      uavcan_register_Value_1_0_select_empty_(&resp.value);
+      registerRead(&name[0], &resp.value);
+      // If such register exists and it can be assigned from the request value:
+      if (!uavcan_register_Value_1_0_is_empty_(&resp.value) && registerAssign(&resp.value, &req->value)) {
+        registerWrite(&name[0], &resp.value);
+      }
+    }
+
+    // Regardless of whether we've just wrote a value or not, we need to read the current one and return it.
+    // The client will determine if the write was successful or not by comparing the request value with response.
     uavcan_register_Value_1_0_select_empty_(&resp.value);
     registerRead(&name[0], &resp.value);
-    // If such register exists and it can be assigned from the request value:
-    if (!uavcan_register_Value_1_0_is_empty_(&resp.value) && registerAssign(&resp.value, &req->value)) {
-      registerWrite(&name[0], &resp.value);
-    }
   }
-
-  // Regardless of whether we've just wrote a value or not, we need to read the current one and return it.
-  // The client will determine if the write was successful or not by comparing the request value with response.
-  uavcan_register_Value_1_0_select_empty_(&resp.value);
-  registerRead(&name[0], &resp.value);
-
   // Currently, all registers we implement are mutable and persistent. This is an acceptable simplification,
   // but more advanced implementations will need to differentiate between them to support advanced features like
   // exposing internal states via registers, perfcounters, etc.
@@ -731,9 +780,7 @@ static void canardDeallocate(void *const user_reference, const size_t amount, vo
 
 int udral_loop(state_sync_f servo_state_sync_f) {
   srand(micros());
-  struct UdralServoInternalState state{
-      .user_state_sync_f = servo_state_sync_f,
-  };
+  state.user_state_sync_f = servo_state_sync_f;
   // A simple application like a servo node typically does not require more than 20 KiB of heap and 4 KiB of stack.
   // For the background and related theory refer to the following resources:
   // - https://github.com/OpenCyphal/libcanard/blob/master/README.md
@@ -782,6 +829,12 @@ int udral_loop(state_sync_f servo_state_sync_f) {
   val._string.value.count = strlen((const char *)val._string.value.elements);
   registerWrite("reg.udral.service.actuator.servo", &val);
 
+  uavcan_register_Access_Request_1_0 req;
+  strcpy((char*)&req.name.name.elements[0], "reg.motor_type");
+  req.name.name.count = strlen((char*)&req.name.name.elements[0]);
+  uavcan_register_Value_1_0_select_empty_(&req.value);
+  processRequestRegisterAccess(&req);
+
   // PID
   state.user_state_sync_f(&state.servo, hz);
 
@@ -811,6 +864,22 @@ int udral_loop(state_sync_f servo_state_sync_f) {
   }
   state.servo.pid_velocity = val.real32;
 
+  uavcan_register_Value_1_0_select_real32_(&val);
+  val.real32 = state.servo.max_voltage;
+  registerRead("reg.max_voltage", &val);
+    Serial.printf("reg.max_voltage count %d %d\n\r", state.servo.max_voltage.value.count, val.real32.value.count);
+  if (!uavcan_register_Value_1_0_is_real32_(&val) || val.real32.value.count < state.servo.max_voltage.value.count) {
+    for (int i = 0; i < uavcan_register_Value_1_0_is_real32_(&val) ? val.real32.value.count : 0; i += 1) {
+      state.servo.max_voltage.value.elements[i] = val.real32.value.elements[i];
+    }
+    uavcan_register_Value_1_0_select_real32_(&val);
+    val.real32 = state.servo.max_voltage;
+    registerWrite("reg.max_voltage", &val);
+  }
+  state.servo.max_voltage = val.real32;
+  // The description register is optional but recommended because it helps constructing/maintaining large networks.
+  // It simply keeps a human-readable description of the node that should be empty by default.
+
   // Configure the transport by reading the appropriate standard registers.
   uavcan_register_Value_1_0_select_natural16_(&val);
   val.natural16.value.count = 1;