diff --git a/cad/standoff.FCStd b/cad/standoff.FCStd
new file mode 100644
index 0000000..9fcd9d7
Binary files /dev/null and b/cad/standoff.FCStd differ
diff --git a/cad/standoff.stl b/cad/standoff.stl
new file mode 100644
index 0000000..affd819
Binary files /dev/null and b/cad/standoff.stl differ
diff --git a/fw/src/main.cpp b/fw/src/main.cpp
index 39cddea..f45a334 100644
--- a/fw/src/main.cpp
+++ b/fw/src/main.cpp
@@ -11,6 +11,10 @@
 #include "SimpleFOCDrivers.h"
 #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
 
+#include "motors/HybridStepperMotor/HybridStepperMotor.h"
+
+#include "drivers/drv8316/drv8316.h"
+
 #include <cmath>
 
 // #include "encoders/sc60228/MagneticSensorSC60228.h"
@@ -49,8 +53,6 @@
 #define SPI_CLK 15
 #define SPI_DRV_SC 16
 
-
-
 #define MOTOR
 #define SSIF_USBSERIAL
 #define FW_NO_WATCHDOG
@@ -65,16 +67,17 @@
 // };
 #endif
 
-const float voltage_factor = 19.31;
 const int voltage_lpf = 50;
-const float anglefactor = 10000.0;
 // BLDC motor & driver instance
 // BLDCMotor motor = BLDCMotor(pole pair number);
-ESP32HWEncoder encoder = ESP32HWEncoder(ENC_A, ENC_B, 2048); // The Index pin can be omitted
+ESP32HWEncoder encoder =
+    ESP32HWEncoder(ENC_A, ENC_B, 2048 / 2); // The Index pin can be omitted
 
 #ifdef MOTOR
 BLDCMotor motor = BLDCMotor(11); // 24N22P
-BLDCDriver3PWM driver = BLDCDriver3PWM(INHA,INHB,INHC);
+// HybridStepperMotor motor = HybridStepperMotor(50, 3.7, 10, 4.5 / 1000);
+DRV8316Driver3PWM driver = DRV8316Driver3PWM(INHA, INHB, INHC, SPI_DRV_SC);
+// BLDCDriver3PWM driver = BLDCDriver3PWM(INHA,INHB,INHC);
 #endif
 // BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
 
@@ -87,50 +90,103 @@ USBCDC usbserial;
 // #define SSYNCIF Serial
 
 void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
-    Wire.beginTransmission(addr);
-    Wire.write(reg);  // MAN:0x2E
-    int error = Wire.endTransmission();
-    if (error != 0) {
-        // Serial.println("read error!");
-        return;
-    }
-    delayMicroseconds(50);
-    int ret = Wire.requestFrom(addr, len);
-    // Serial.print("0x");
-    // Serial.print(reg, HEX);
-    // Serial.print(": ");
-    Wire.readBytes(buf, ret);
-    // for (int i = 0; i < ret; ++i) {
-    // Serial.print("0x");
-    // Serial.print(Wire.read(), HEX);
-    // Serial.print(" ");
-    // }
-    // Serial.println();
+  Wire.beginTransmission(addr);
+  Wire.write(reg); // MAN:0x2E
+  int error = Wire.endTransmission();
+  if (error != 0) {
+    // Serial.println("read error!");
     return;
+  }
+  delayMicroseconds(50);
+  int ret = Wire.requestFrom(addr, len);
+  // Serial.print("0x");
+  // Serial.print(reg, HEX);
+  // Serial.print(": ");
+  Wire.readBytes(buf, ret);
+  // for (int i = 0; i < ret; ++i) {
+  // Serial.print("0x");
+  // Serial.print(Wire.read(), HEX);
+  // Serial.print(" ");
+  // }
+  // Serial.println();
+  return;
 }
 
 void write_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val) {
-    Wire.beginTransmission(addr);
-    uint8_t data[] = {reg_lsb, val};
-    Wire.write(data, sizeof data);
-    int error = Wire.endTransmission();
-    if (error != 0) {
-        // Serial.println("write error!");
-        return;
-    }
+  Wire.beginTransmission(addr);
+  uint8_t data[] = {reg_lsb, val};
+  Wire.write(data, sizeof data);
+  int error = Wire.endTransmission();
+  if (error != 0) {
+    // Serial.println("write error!");
+    return;
+  }
 }
-void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t
-val2) {
-    Wire.beginTransmission(addr);
-    uint8_t data[] = {reg_lsb, val1, val2};
-    Wire.write(data, sizeof data);
-    int error = Wire.endTransmission();
-    if (error != 0) {
-        // Serial.println("write error!");
-        return;
-    }
+void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) {
+  Wire.beginTransmission(addr);
+  uint8_t data[] = {reg_lsb, val1, val2};
+  Wire.write(data, sizeof data);
+  int error = Wire.endTransmission();
+  if (error != 0) {
+    // Serial.println("write error!");
+    return;
+  }
 }
 
+void status() {
+
+  DRV8316Status status = driver.getStatus();
+  SSYNCIF.println("DRV8316 Status:");
+  SSYNCIF.print("Fault: ");
+  SSYNCIF.println(status.isFault());
+  SSYNCIF.print("Buck Error: ");
+  SSYNCIF.print(status.isBuckError());
+  SSYNCIF.print("  Undervoltage: ");
+  SSYNCIF.print(status.isBuckUnderVoltage());
+  SSYNCIF.print("  OverCurrent: ");
+  SSYNCIF.println(status.isBuckOverCurrent());
+  SSYNCIF.print("Charge Pump UnderVoltage: ");
+  SSYNCIF.println(status.isChargePumpUnderVoltage());
+  SSYNCIF.print("OTP Error: ");
+  SSYNCIF.println(status.isOneTimeProgrammingError());
+  SSYNCIF.print("OverCurrent: ");
+  SSYNCIF.print(status.isOverCurrent());
+  SSYNCIF.print("  Ah: ");
+  SSYNCIF.print(status.isOverCurrent_Ah());
+  SSYNCIF.print("  Al: ");
+  SSYNCIF.print(status.isOverCurrent_Al());
+  SSYNCIF.print("  Bh: ");
+  SSYNCIF.print(status.isOverCurrent_Bh());
+  SSYNCIF.print("  Bl: ");
+  SSYNCIF.print(status.isOverCurrent_Bl());
+  SSYNCIF.print("  Ch: ");
+  SSYNCIF.print(status.isOverCurrent_Ch());
+  SSYNCIF.print("  Cl: ");
+  SSYNCIF.println(status.isOverCurrent_Cl());
+  SSYNCIF.print("OverTemperature: ");
+  SSYNCIF.print(status.isOverTemperature());
+  SSYNCIF.print("  Shutdown: ");
+  SSYNCIF.print(status.isOverTemperatureShutdown());
+  SSYNCIF.print("  Warning: ");
+  SSYNCIF.println(status.isOverTemperatureWarning());
+  SSYNCIF.print("OverVoltage: ");
+  SSYNCIF.println(status.isOverVoltage());
+  SSYNCIF.print("PowerOnReset: ");
+  SSYNCIF.println(status.isPowerOnReset());
+  SSYNCIF.print("SPI Error: ");
+  SSYNCIF.print(status.isSPIError());
+  SSYNCIF.print("  Address: ");
+  SSYNCIF.print(status.isSPIAddressError());
+  SSYNCIF.print("  Clock: ");
+  SSYNCIF.print(status.isSPIClockFramingError());
+  SSYNCIF.print("  Parity: ");
+  SSYNCIF.println(status.isSPIParityError());
+  DRV8316_PWMMode val = driver.getPWMMode();
+  SSYNCIF.print("PWM Mode: ");
+  SSYNCIF.println(val);
+  SSYNCIF.print("Recirculation: ");
+  SSYNCIF.println(driver.getRecirculationMode());
+}
 // Preferences prefs;
 
 SemaphoreHandle_t mutex;
@@ -147,9 +203,9 @@ void foc_task(void *parameter) {
 #endif
 
 #ifdef MOTOR
-      // motors.target = s_foc.vt[i] / anglefactor;
-      // motors.loopFOC();
-      // motors.move();
+    // motors.target = s_foc.vt[i] / anglefactor;
+    // motors.loopFOC();
+    // motors.move();
 #else
     // sensors.update();
 #endif
@@ -177,17 +233,38 @@ void setup() {
   // SSYNCIF.print(__cplusplus);
 
   // Wire.setTimeOut(10);
-  // SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_CS1);
   Wire.begin(I2C_SDA, I2C_SCL, 400000);
 
-  // write_i2c(0x36,0x07, 0b00000011);
-  write_i2c(0x36,0x09, 0b1000);
-  
-  encoder.init();
+  pinMode(DRVOFF, OUTPUT);
+  digitalWrite(DRVOFF, 0); // enable
+  pinMode(INLA, OUTPUT);
+  digitalWrite(INLA, 1); // enable
+  pinMode(INLB, OUTPUT);
+  digitalWrite(INLB, 1); // enable
+  pinMode(INLC, OUTPUT);
+  digitalWrite(INLC, 1); // enable
 
+  // write_i2c(0x36,0x07, 0b00000011);
+  write_i2c(0x36, 0x09, 0xFF);
+
+  encoder.init();
+#ifdef MOTOR
+
+  driver.voltage_power_supply = 15;
+  SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
+  driver.init(&SPI);
+  driver.setSlew(DRV8316_Slew::Slew_200Vus);
+  status();
+  motor.linkDriver(&driver);
+  // motor.linkSensor(&encoder);
+  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
+  motor.voltage_sensor_align = 5;
+  motor.voltage_limit = 6;
+  motor.controller = MotionControlType::velocity_openloop;
+  motor.init();
+  // motor.initFOC();
 
   // sensor.init(&SPI);
-#ifdef MOTOR
   // motors[i].linkSensor(&sensors[i]);
 #endif
 
@@ -207,30 +284,30 @@ void setup() {
   // s_exchange.vcc_voltage_mV = GET_VCC;
   // drivers[0].voltage_power_supply = s_exchange.vcc_voltage_mV / 1000.0f;
   // for (int i = 1; i < NUM_MOTORS; i += 1)
-    // drivers[i].voltage_power_supply = drivers[0].voltage_power_supply;
+  // drivers[i].voltage_power_supply = drivers[0].voltage_power_supply;
   // s_foc = s_exchange;
 
   // for (int i = 0; i < NUM_MOTORS; i += 1) {
 #ifdef MOTOR
-    // drivers[i].init();
-    // link driver
-    // motors[i].linkDriver(&drivers[i]);
-    // motors[i].voltage_sensor_align = 5;
+  // drivers[i].init();
+  // link driver
+  // motors[i].linkDriver(&drivers[i]);
+  // motors[i].voltage_sensor_align = 5;
 
-    // set control loop type to be used
-    // motors[i].torque_controller = TorqueControlType::voltage;
-    // motors[i].controller = MotionControlType::velocity;
+  // set control loop type to be used
+  // motors[i].torque_controller = TorqueControlType::voltage;
+  // motors[i].controller = MotionControlType::velocity;
 
-    // contoller configuration based on the control type
-    // motors[i].PID_velocity.P = 0.35;
-    // motors[i].PID_velocity.I = 8;
-    // motors[i].PID_velocity.D = 0;
+  // contoller configuration based on the control type
+  // motors[i].PID_velocity.P = 0.35;
+  // motors[i].PID_velocity.I = 8;
+  // motors[i].PID_velocity.D = 0;
 
-    // default voltage_power_supply
-    // motors[i].voltage_limit = 3.4 * 3;
+  // default voltage_power_supply
+  // motors[i].voltage_limit = 3.4 * 3;
 
-    // velocity low pass filtering time constant
-    // motors[i].LPF_velocity.Tf = 0.01;
+  // velocity low pass filtering time constant
+  // motors[i].LPF_velocity.Tf = 0.01;
 #endif
   // }
 #ifdef MOTOR
@@ -260,15 +337,18 @@ void setup() {
   //   // if (prefs.isKey(pref_zero_key.c_str()) &&
   //   // prefs.isKey(pref_dir_key.c_str())) {
   //   //     motors[i].zero_electric_angle =
-  //   //     prefs.getFloat(pref_zero_key.c_str()); motors[i].sensor_direction =
+  //   //     prefs.getFloat(pref_zero_key.c_str()); motors[i].sensor_direction
+  //   =
   //   //     (Direction)prefs.getInt(pref_dir_key.c_str());
   //   // }
   //   motors[i].initFOC();
   //   // if (!prefs.isKey(pref_zero_key.c_str()) ||
   //   // !prefs.isKey(pref_dir_key.c_str())) {
   //   //     prefs.clear();
-  //   //     prefs.putFloat(pref_zero_key.c_str(), motors[i].zero_electric_angle);
-  //   //     prefs.putInt(pref_dir_key.c_str(), (int)motors[i].sensor_direction);
+  //   //     prefs.putFloat(pref_zero_key.c_str(),
+  //   motors[i].zero_electric_angle);
+  //   //     prefs.putInt(pref_dir_key.c_str(),
+  //   (int)motors[i].sensor_direction);
   //   // }
   // }
 #endif
@@ -280,10 +360,9 @@ void setup() {
 #endif
 
   // xTaskCreatePinnedToCore(&comm_task, // Function name of the task
-  //                         "Comm",     // Name of the task (e.g. for debugging)
-  //                         4096,       // Stack size (bytes)
-  //                         NULL,       // Parameter to pass
-  //                         1,          // Task priority
+  //                         "Comm",     // Name of the task (e.g. for
+  //                         debugging) 4096,       // Stack size (bytes) NULL,
+  //                         // Parameter to pass 1,          // Task priority
   //                         &taskCommHandle, // Assign task handle
   //                         1                // Run on the non-Arduino (1) core
   // );
@@ -299,13 +378,25 @@ void setup() {
   // esp_task_wdt_add(NULL);                  // add current thread to WDT watch
 }
 
-int i = 0;
 void loop() {
+#ifdef MOTOR
+  // motor.loopFOC();
 
-    encoder.update(); // optional: Update manually if not using loopfoc()
-
-     // Access the sensor value
-    SSYNCIF.println(encoder.getAngle());
-    delay(10);
+  motor.move(3);
+#endif
+  // delay(100);
+  // static int i = 0;
+  // if (i++ % 100 == 0)
+  //   status();
 }
+
+// int i = 0;
+// void loop() {
+
+//     //encoder.update(); // optional: Update manually if not using loopfoc()
+//     //
+//      // Access the sensor value
+//     SSYNCIF.println(encoder.getAngle());
+//     delay(10);
+// }
 // esp_task_wdt_reset();