diff --git a/CMakeLists.txt b/CMakeLists.txt
deleted file mode 100644
index e69de29..0000000
diff --git a/README.md b/README.md
index 268e95c..26504a5 100644
--- a/README.md
+++ b/README.md
@@ -3,27 +3,43 @@
 ![Build](https://github.com/uLipe/espFoC/workflows/Build/badge.svg)
 [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT)
 
-espFoC is a simple implementation of voltage mode, vector controller intended to be used with permanent-magnet synchronous motors (PMSM), and general brushless motors. This component was developed to be used with Zephyr RTOS for example and is aimed to transform
-your esp32 chip into a dual axis PMSM motor controller chip
+espFoC is a simple implementation of voltage mode, vector controller intended to be used with permanent-magnet synchronous motors (PMSM), and general brushless motors for ESP32S3 Espressif SoC, other SoC from espressif might be supported but the primary requirement is to be a dual
+core variant. espFoC started as a library for motor control, but it suffered a goal change, and now it is intended to create a programmable
+BLDC (FoC based) motor control-IC, the dual core capabillity splits the controller into application side and motor control core side
+the former is intended for user to write it own application or exchange packets with external controller via CAN-Bus, the later is
+100% focused on perform motor control algorithm achieving better control bandwidth. Up two axis are supported in this new variant, refer the
+simplified architecture below:
+![espFoC Simplified Architecture](/doc/images/arch.png)
 
 ## Features:
 
 * Voltage mode control, control a PMSM like a DC motor!;
 * Position and Speed closed-loop control;
 * Single-precision Floating point implementation;
-* Sample inverter driver based on esp32 LEDC PWM (easy to wire!);
-* Sample rotor position driver based on as5600 encoder (very popular!);
-* Uses openAMP to offload motor control tasks to one of the core, and leave the other for communication;
-* support UART and CAN communication;
+* Easy to wire motor using common drivers and I2C encoders out there!
+* Uses ESP32 AMP solution for Zephyr RTOS to split execution;
+* App CPU runs the motor control firmware;
+* Pro CPU runs communication and exposes a thread for user application;
+* Interaction between the firmwares are doing by a custom IPC protocol;
+* Planned support for UART and CAN communication;
 
 ## Limitations:
 
-* Support for esp32 and esp32s3 only;
-* Requires and rotor position sensor, for example, incremental encoder.
+* Support only for espressif SoC that are dual-core and have FPU;
+* Once sensored support, requires an I2C magnetic encoder sensor.
 
 ## Getting started:
 
-* Just clone this project on most convenient folder;
+* Just clone this project on most convenient folder then:
+
+```
+ $ west build  -palways -besp32s3_devkitm/esp32s3/procpu --sysbuild /path/to/espFoC/app
+```
+
+* To flash:
+```
+ $ west flash
+```
 
 ## Typical wiring:
 
@@ -35,5 +51,4 @@ your esp32 chip into a dual axis PMSM motor controller chip
 ## Support:
 
 If you find some trouble, open an issue, and if you are enjoying the project
-give it a star or submir a PR. Also, you can try reaching me at:
-ryukokki.felipe@gmail.com
\ No newline at end of file
+give it a star or submir a PR. Also, you can try reaching me at:
\ No newline at end of file
diff --git a/app/CMakeLists.txt b/app/CMakeLists.txt
new file mode 100644
index 0000000..9e34685
--- /dev/null
+++ b/app/CMakeLists.txt
@@ -0,0 +1,20 @@
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.20.0)
+
+set(REMOTE_ZEPHYR_DIR ${CMAKE_CURRENT_BINARY_DIR}/../espfoc_motor_remote/zephyr)
+
+find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
+project(espfoc_app)
+
+set(MOTOR_CONTROL_SRC ${APPLICATION_SOURCE_DIR}/../common)
+set(MOTOR_CONTROL_INC ${APPLICATION_SOURCE_DIR}/../common/include)
+
+FILE(GLOB ipc ${MOTOR_CONTROL_SRC}/ipc/*.c)
+target_sources(app PRIVATE espfoc_app/esp_foc_app_shell.c
+                           espfoc_app/esp_foc_main.c
+                           espfoc_app/esp_foc_user.c
+                           ${ipc})
+
+target_include_directories(app PRIVATE  ${MOTOR_CONTROL_INC})
+
diff --git a/app/Kconfig.sysbuild b/app/Kconfig.sysbuild
new file mode 100644
index 0000000..31c7e63
--- /dev/null
+++ b/app/Kconfig.sysbuild
@@ -0,0 +1,7 @@
+source "share/sysbuild/Kconfig"
+
+config ESP_FOC_REMOTE_BOARD
+	string
+	default "esp32_devkitc_wrover/esp32/appcpu" if $(BOARD) = "esp32_devkitc_wroom"
+	default "esp32_devkitc_wroom/esp32/appcpu" if $(BOARD) = "esp32_devkitc_wroom"
+	default "esp32s3_devkitm/esp32s3/appcpu" if $(BOARD) = "esp32s3_devkitm"
diff --git a/app/boards/esp32s3_devkitm_procpu.overlay b/app/boards/esp32s3_devkitm_procpu.overlay
new file mode 100644
index 0000000..40014ca
--- /dev/null
+++ b/app/boards/esp32s3_devkitm_procpu.overlay
@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2023 Felipe Neves <ryukokki.felipe@gmail.com>
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+/ {
+	chosen {
+		zephyr,console = &uart0;
+		zephyr,shell-uart = &uart0;
+		/*
+		 * shared memory reserved for the inter-processor communication
+		 */
+		zephyr,ipc_shm = &shm0;
+		zephyr,ipc = &ipm0;
+	};
+};
+
+&ipm0 {
+	status = "okay";
+};
+
+&i2c0 {
+	status = "disabled";
+};
+
+&i2c1 {
+	status = "disabled";
+};
diff --git a/app/espfoc_app/esp_foc_app_shell.c b/app/espfoc_app/esp_foc_app_shell.c
new file mode 100644
index 0000000..86f6cbf
--- /dev/null
+++ b/app/espfoc_app/esp_foc_app_shell.c
@@ -0,0 +1,147 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2021 Felipe Neves
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+#include <stdlib.h>
+#include <zephyr/kernel.h>
+#include <zephyr/device.h>
+#include <zephyr/shell/shell.h>
+#include <espFoC/ipc/esp_foc_ipc.h>
+
+static int cmd_set_position(const struct shell *shell, size_t argc, char **argv)
+{
+    struct motor_command command;
+
+	if (argc != 2) {
+		return -EINVAL;
+	}
+
+	command.position_mdeg = (int32_t)(strtof(argv[1], NULL) * 1000.0f);
+	command.command_mask = MOTOR_CMD_POSITION_MASK;
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+static int cmd_set_speed(const struct shell *shell, size_t argc, char **argv)
+{
+	struct motor_command command;
+
+	if (argc != 2) {
+		return -EINVAL;
+	}
+
+	command.speed_mdps = (int32_t)(strtof(argv[1], NULL) * 1000.0f);
+	command.command_mask = MOTOR_CMD_SPEED_MASK;
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+static int cmd_enable(const struct shell *shell, size_t argc, char **argv)
+{
+	struct motor_command command;
+
+	if (argc != 1) {
+		return -EINVAL;
+	}
+
+	command.command_mask = MOTOR_CMD_SHUTDOWN_MASK | MOTOR_CMD_GLOBAL_ENABLE;
+	command.power_state = 1;
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+static int cmd_disable(const struct shell *shell, size_t argc, char **argv)
+{
+	struct motor_command command;
+
+	if (argc != 1) {
+		return -EINVAL;
+	}
+
+	command.command_mask = MOTOR_CMD_SHUTDOWN_MASK | MOTOR_CMD_GLOBAL_DISABLE;
+	command.power_state = 0;
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+static int cmd_set_gains_pid_vel(const struct shell *shell, size_t argc, char **argv)
+{
+    struct motor_command command;
+
+	if (argc != 4) {
+		return -EINVAL;
+	}
+
+	command.command_mask = MOTOR_CMD_SPD_PID_MASK;
+	command.pid_gains[0] = (int32_t)(strtof(argv[1], NULL) * 1e+6f);
+	command.pid_gains[1] = (int32_t)(strtof(argv[2], NULL) * 1e+6f);
+	command.pid_gains[2] = (int32_t)(strtof(argv[3], NULL) * 1e+6f);
+
+	return esp_foc_ipc_send_command(&command);
+
+}
+
+static int cmd_set_gains_pid_pos(const struct shell *shell, size_t argc, char **argv)
+{
+    struct motor_command command;
+
+	if (argc != 4) {
+		return -EINVAL;
+	}
+
+	command.command_mask = MOTOR_CMD_POS_PID_MASK;
+	command.pid_gains[0] = (int32_t)(strtof(argv[1], NULL) * 1e+6f);
+	command.pid_gains[1] = (int32_t)(strtof(argv[2], NULL) * 1e+6f);
+	command.pid_gains[2] = (int32_t)(strtof(argv[3], NULL) * 1e+6f);
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+static int cmd_set_maintenance_pwm(const struct shell *shell, size_t argc, char **argv)
+{
+    struct motor_command command;
+
+	if (argc != 4) {
+		return -EINVAL;
+	}
+
+	command.command_mask = (MOTOR_CMD_MAITENANCE_MODE | MOTOR_CMD_MAITENANCE_PWMS);
+	command.pwms[0] = (int32_t)(strtof(argv[1], NULL) * 1e+6f);
+	command.pwms[1] = (int32_t)(strtof(argv[2], NULL) * 1e+6f);
+	command.pwms[2] = (int32_t)(strtof(argv[3], NULL) * 1e+6f);
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+SHELL_STATIC_SUBCMD_SET_CREATE(
+	esp_foc,
+	SHELL_CMD(set_position, NULL, "sets position in degrees", cmd_set_position),
+	SHELL_CMD(set_speed, NULL, "sets speed in degrees per second", cmd_set_speed),
+	SHELL_CMD(enable, NULL, "enable motor driver", cmd_enable),
+	SHELL_CMD(disable, NULL, "disable motor driver", cmd_disable),
+	SHELL_CMD(set_gains_pid_vel, NULL, "set pids velocity", cmd_set_gains_pid_vel),
+	SHELL_CMD(set_gains_pid_pos, NULL, "set pids speed", cmd_set_gains_pid_pos),
+	SHELL_CMD(set_maintenance_pwm, NULL, "set pwms maitenance mode", cmd_set_maintenance_pwm),
+	SHELL_SUBCMD_SET_END
+	);
+
+SHELL_CMD_REGISTER(esp_foc, &esp_foc, "espFoC Basic shell commands", NULL);
diff --git a/app/espfoc_app/esp_foc_main.c b/app/espfoc_app/esp_foc_main.c
new file mode 100644
index 0000000..3b7a73a
--- /dev/null
+++ b/app/espfoc_app/esp_foc_main.c
@@ -0,0 +1,65 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2021 Felipe Neves
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <stdio.h>
+#include <zephyr/kernel.h>
+#include <espFoC/ipc/esp_foc_ipc.h>
+
+static void esp_foc_motor_on_state (const struct motor_state *state);
+ESP_FOC_DEFINE_IPC_CALLBACK(state_callback, NULL, esp_foc_motor_on_state);
+
+static void esp_foc_motor_on_state (const struct motor_state *state)
+{
+    printk("espFoc Motor firmware uptime: %lu [ms] \n", state->motor_system_uptime);
+    printk("espFoc Motor firmware timestep: %lu [us] \n", state->timestamp_us);
+
+    if(!state->system_enabled) {
+        printk("espFoC Motor is alive \n");
+    } else {
+        printk("espFoC Motor sent report \n");
+    }
+}
+
+int esp_foc_early_init(void)
+{
+    esp_foc_ipc_init();
+    esp_foc_ipc_register_callback(&state_callback);
+    k_msleep(50);
+    return 0;
+}
+SYS_INIT(esp_foc_early_init, POST_KERNEL, CONFIG_APPLICATION_INIT_PRIORITY);
+
+int main (void)
+{
+    /**TODO use the main application to implement espfoc commander
+     * Through UART, SPI, I2C and CAN, last one is the priority
+     */
+    printk("espFoC is running in the %s\n", CONFIG_BOARD);
+}
+
+__weak void esp_foc_user_entry(void)
+{
+    printk("Weak user entry of the espFoC \n");
+}
+K_THREAD_DEFINE(foc_tid, 4096, esp_foc_user_entry, NULL, NULL, NULL, 1, 0, 0);
\ No newline at end of file
diff --git a/app/espfoc_app/esp_foc_user.c b/app/espfoc_app/esp_foc_user.c
new file mode 100644
index 0000000..c077eb1
--- /dev/null
+++ b/app/espfoc_app/esp_foc_user.c
@@ -0,0 +1,35 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2021 Felipe Neves
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <stdio.h>
+#include <zephyr/kernel.h>
+#include <espFoC/ipc/esp_foc_ipc.h>
+
+void esp_foc_user_entry(void)
+{
+    /* Add your user code here! */
+    /* Use esp foc ipc to interact with the motor control firmware
+     * That runs on the other core.
+     */
+}
diff --git a/app/espfoc_motor/CMakeLists.txt b/app/espfoc_motor/CMakeLists.txt
new file mode 100644
index 0000000..1a9b6ab
--- /dev/null
+++ b/app/espfoc_motor/CMakeLists.txt
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.20.0)
+
+find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
+project(espfoc_motor)
+
+set(MOTOR_CONTROL_SRC ${APPLICATION_SOURCE_DIR}/../../common)
+set(MOTOR_CONTROL_INC ${APPLICATION_SOURCE_DIR}/../../common/include)
+
+FILE(GLOB ipc ${MOTOR_CONTROL_SRC}/ipc/*.c)
+FILE(GLOB motor_control ${MOTOR_CONTROL_SRC}/motor_control/*.c)
+
+target_sources(app PRIVATE  esp_foc_motor.c
+                            esp_foc_hardware_if.c
+                            ${ipc}
+                            ${motor_control})
+
+target_include_directories(app PRIVATE  ${MOTOR_CONTROL_INC})
diff --git a/app/espfoc_motor/boards/esp32s3_devkitm_appcpu.overlay b/app/espfoc_motor/boards/esp32s3_devkitm_appcpu.overlay
new file mode 100644
index 0000000..20bc15a
--- /dev/null
+++ b/app/espfoc_motor/boards/esp32s3_devkitm_appcpu.overlay
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2023 Felipe Neves <ryukokki.felipe@gmail.com>
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+ #include <zephyr/dt-bindings/i2c/i2c.h>
+ #include <zephyr/dt-bindings/pwm/pwm.h>
+
+ #include <zephyr/dt-bindings/pinctrl/esp-pinctrl-common.h>
+ #include <dt-bindings/pinctrl/esp32s3-pinctrl.h>
+ #include <zephyr/dt-bindings/pinctrl/esp32s3-gpio-sigmap.h>
+
+ / {
+	chosen {
+		/*
+		 * shared memory reserved for the inter-processor communication
+		 */
+		zephyr,ipc_shm = &shm0;
+		zephyr,ipc = &ipm0;
+	};
+
+	buttons {
+		compatible = "gpio-keys";
+		inverter_enable0: inverter_enable0 {
+			gpios = <&gpio0 9 GPIO_ACTIVE_HIGH>;
+		};
+
+		inverter_enable1: inverter_enable1 {
+			gpios = <&gpio0 10 GPIO_ACTIVE_HIGH>;
+		};
+	};
+};
+
+&pinctrl {
+	ledc0_custom: ledc0_custom {
+		group1 {
+			pinmux = <LEDC_CH0_GPIO2>,
+					 <LEDC_CH1_GPIO3>,
+					 <LEDC_CH2_GPIO4>;
+			output-enable;
+		};
+	};
+
+	i2c0_custom: i2c0_custom {
+		group1 {
+			pinmux = <I2C0_SDA_GPIO17>,
+				 <I2C0_SCL_GPIO18>;
+			bias-pull-up;
+			drive-open-drain;
+			output-high;
+		};
+	};
+
+	ledc1_custom: ledc1_custom {
+		group1 {
+			pinmux = <LEDC_CH0_GPIO5>,
+					 <LEDC_CH1_GPIO6>,
+					 <LEDC_CH2_GPIO7>;
+			output-enable;
+		};
+	};
+
+	i2c1_custom: i2c1_custom {
+		group1 {
+			pinmux = <I2C1_SDA_GPIO19>,
+				 <I2C1_SCL_GPIO20>;
+			bias-pull-up;
+			drive-open-drain;
+			output-high;
+		};
+	};
+
+};
+
+&ipm0 {
+	status = "okay";
+};
+
+// &ledc0 {
+// 	pinctrl-0 = <&ledc0_custom>;
+// 	pinctrl-names = "default";
+// 	status = "okay";
+// 	#address-cells = <1>;
+// 	#size-cells = <0>;
+// 	channel0@0 {
+// 		reg = <0x0>;
+// 		timer = <0>;
+// 	};
+// 	channel1@1 {
+// 		reg = <0x1>;
+// 		timer = <0>;
+// 	};
+
+// 	channel2@2 {
+// 		reg = <0x2>;
+// 		timer = <0>;
+// 	};
+// };
+
+&i2c0 {
+	pinctrl-0 = <&i2c0_custom>;
+	pinctrl-names = "default";
+	status = "okay";
+	clock-frequency = <I2C_BITRATE_FAST>;
+
+	angle_sensor_0: as5600@36 {
+		compatible = "ams,as5600";
+		status = "okay";
+		reg = <0x36>;
+	};
+};
+
+// &ledc1 {
+// 	pinctrl-0 = <&ledc1_custom>;
+// 	pinctrl-names = "default";
+// 	status = "okay";
+// 	#address-cells = <1>;
+// 	#size-cells = <0>;
+// 	channel0@0 {
+// 		reg = <0x0>;
+// 		timer = <0>;
+// 	};
+
+// 	channel1@0 {
+// 		reg = <0x0>;
+// 		timer = <0>;
+// 	};
+
+// 	channel2@0 {
+// 		reg = <0x0>;
+// 		timer = <0>;
+// 	};
+// };
+
+// &i2c1 {
+// 	pinctrl-0 = <&i2c1_custom>;
+// 	pinctrl-names = "default";
+// 	status = "okay";
+// 	clock-frequency = <I2C_BITRATE_FAST>;
+
+// 	angle_sensor_1: as5600@36 {
+// 		compatible = "ams,as5600";
+// 		status = "okay";
+// 		reg = <0x36>;
+// 	};
+// };
+
diff --git a/app/espfoc_motor/esp_foc_hardware_if.c b/app/espfoc_motor/esp_foc_hardware_if.c
new file mode 100644
index 0000000..b8d2f40
--- /dev/null
+++ b/app/espfoc_motor/esp_foc_hardware_if.c
@@ -0,0 +1,296 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2021 Felipe Neves
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <stdbool.h>
+#include <zephyr/kernel.h>
+#include <zephyr/device.h>
+#include <zephyr/drivers/gpio.h>
+#include <zephyr/drivers/i2c.h>
+#include <zephyr/drivers/pwm.h>
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/sys/util.h>
+#include <espFoC/motor_control/esp_foc.h>
+#include "esp_foc_hardware_if.h"
+
+#define PWM_PERIOD_NSEC                 31250
+#define DC_BUS_VOLTAGE                  24.0f
+
+struct z_esp_foc_motor_if {
+    const struct device *pwm_device;
+    const struct device *angle_sensor;
+    struct gpio_dt_spec enable_pins;
+    float accumulated_angle_deg;
+    float current_rotor_deg;
+    float prev_rotor_deg;
+    float current_rotor_speed_dps;
+    float pole_pairs;
+    struct esp_foc_motor_interface interface;
+};
+
+static int reset(struct esp_foc_motor_interface *self)
+{
+    int ret;
+
+    ret = esp_foc_motor_enable(self);
+    if (ret < 0) {
+        return ret;
+    }
+
+    return esp_foc_motor_align_rotor(self);
+}
+
+static int enable(struct esp_foc_motor_interface *self)
+{
+    struct z_esp_foc_motor_if *itf = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface);
+
+    int ret = esp_foc_motor_set_duty_cycles(self, 0.0f, 0.0f, 0.0f);
+    if (ret < 0) {
+        return ret;
+    }
+
+    k_msleep(100);
+
+    gpio_pin_configure_dt(&itf->enable_pins, GPIO_OUTPUT);
+    gpio_pin_set_dt(&itf->enable_pins, 1);
+
+    k_msleep(100);
+
+    return 0;
+}
+
+static int disable(struct esp_foc_motor_interface *self)
+{
+    struct z_esp_foc_motor_if *itf = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface);
+
+    gpio_pin_configure_dt(&itf->enable_pins, GPIO_OUTPUT);
+    gpio_pin_set_dt(&itf->enable_pins, 1);
+    esp_foc_motor_set_duty_cycles(self, 0.0f, 0.0f, 0.0f);
+    k_msleep(100);
+
+    return 0;
+}
+
+static int align_rotor(struct esp_foc_motor_interface *self)
+{
+    int ret;
+
+    ret = esp_foc_motor_set_duty_cycles(self, 0.0f, 0.0f, 0.0f);
+    if (ret < 0) {
+        return ret;
+    }
+
+    k_msleep(500);
+
+    ret = esp_foc_motor_set_duty_cycles(self, 0.2f, 0.0f, 0.0f);
+    if (ret < 0) {
+        return ret;
+    }
+
+    k_msleep(500);
+
+    return esp_foc_motor_fetch_sensors(self);
+}
+
+static int set_duty_cycles(struct esp_foc_motor_interface *self, float dc_a, float dc_b, float dc_c)
+{
+    struct z_esp_foc_motor_if *itf = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface);
+
+    if(dc_a > 1.0f) {
+		dc_a = 1.0f;
+	} else if (dc_a < 0.0f) {
+		dc_a = 0.0f;
+	}
+
+	if(dc_b > 1.0f) {
+		dc_b = 1.0f;
+	} else if (dc_b < 0.0f) {
+		dc_b = 0.0f;
+	}
+
+	if(dc_c > 1.0f) {
+		dc_c = 1.0f;
+	} else if (dc_c < 0.0f) {
+		dc_c = 0.0f;
+	}
+
+    dc_a *= PWM_PERIOD_NSEC - 1;
+    dc_b *= PWM_PERIOD_NSEC - 1;
+    dc_c *= PWM_PERIOD_NSEC - 1;
+
+	// pwm_set(itf->pwm_device, 1 , PWM_PERIOD_NSEC, dc_a, 0);
+	// pwm_set(itf->pwm_device, 2 , PWM_PERIOD_NSEC, dc_b, 0);
+	// pwm_set(itf->pwm_device, 3 , PWM_PERIOD_NSEC, dc_c, 0);
+
+    return 0;
+}
+
+static int get_rotor_angle(struct esp_foc_motor_interface *self, float *angle)
+{
+    struct z_esp_foc_motor_if *itf = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface);
+
+    if(!angle)
+        return -EINVAL;
+
+    *angle = itf->current_rotor_deg;
+
+    return 0;
+}
+static int get_electrical_angle(struct esp_foc_motor_interface *self, float *angle)
+{
+    struct z_esp_foc_motor_if *itf = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface);
+
+    if(!angle)
+        return -EINVAL;
+
+    *angle = itf->current_rotor_deg * itf->pole_pairs;
+
+    return 0;
+}
+
+static int get_rotor_speed_dps(struct esp_foc_motor_interface *self, float *speed_dps)
+{
+    struct z_esp_foc_motor_if *itf = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface);
+
+    if(!speed_dps)
+        return -EINVAL;
+
+    *speed_dps = itf->current_rotor_speed_dps;
+
+    return 0;
+}
+
+static int get_acumulated_angle(struct esp_foc_motor_interface *self, float *angle)
+{
+    struct z_esp_foc_motor_if *itf = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface);
+
+    if(!angle)
+        return -EINVAL;
+
+    *angle = itf->accumulated_angle_deg;
+
+    return 0;
+}
+
+static int get_dc_bus_voltage(struct esp_foc_motor_interface *self, float *vdc)
+{
+    if(!vdc)
+        return -EINVAL;
+
+    *vdc = DC_BUS_VOLTAGE;
+
+    return 0;
+}
+
+static int fetch_sensors(struct esp_foc_motor_interface *self)
+{
+    struct z_esp_foc_motor_if *itf = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface);
+    struct sensor_value raw;
+    int ret;
+
+    itf->prev_rotor_deg = itf->current_rotor_deg;
+
+    // ret = sensor_sample_fetch(itf->angle_sensor);
+    // if(ret) {
+    //     return ret;
+    // }
+
+    // ret = sensor_channel_get(itf->angle_sensor, SENSOR_CHAN_ROTATION, &raw);
+    // if(ret) {
+    //     return ret;
+    // }
+
+    // itf->current_rotor_deg = sensor_value_to_float(&raw);
+    // itf->current_rotor_speed_dps = (itf->current_rotor_deg - itf->prev_rotor_deg);
+    // itf->accumulated_angle_deg += itf->current_rotor_speed_dps;
+
+    return ret;
+}
+
+static int set_pole_pairs(struct esp_foc_motor_interface *self, int32_t pole_pairs)
+{
+    struct z_esp_foc_motor_if *itf = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface);
+
+    itf->pole_pairs = (float)pole_pairs;
+
+    return 0;
+}
+
+static struct z_esp_foc_motor_if z_esp_hw_if[Z_ESP_FOC_MAX] = {
+    {
+        // .pwm_device = DEVICE_DT_GET(DT_NODELABEL(ledc0)),
+        // .angle_sensor = DEVICE_DT_GET(DT_NODELABEL(angle_sensor_0)),
+        .enable_pins = GPIO_DT_SPEC_GET_BY_IDX(DT_NODELABEL(inverter_enable0),gpios,0),
+        .current_rotor_deg = 0.0f,
+        .current_rotor_speed_dps = 0.0f,
+        .accumulated_angle_deg = 0.0f,
+        .pole_pairs = 2.0f,
+        .interface.reset = reset,
+        .interface.enable = enable,
+        .interface.disable = disable,
+        .interface.set_duty_cycles = set_duty_cycles,
+        .interface.get_rotor_angle = get_rotor_angle,
+        .interface.get_electrical_angle = get_electrical_angle,
+        .interface.get_rotor_speed_dps = get_rotor_speed_dps,
+        .interface.align_rotor = align_rotor,
+        .interface.get_acumulated_angle = get_acumulated_angle,
+        .interface.get_encoder_ppr = NULL,
+        .interface.get_dc_bus_voltage = get_dc_bus_voltage,
+        .interface.get_currents = NULL,
+        .interface.number_of_shunts = NULL,
+        .interface.fetch_sensors = fetch_sensors,
+        .interface.set_pole_pairs = set_pole_pairs,
+    },
+    {
+        // .pwm_device = DEVICE_DT_GET(DT_NODELABEL(ledc0)),
+        // .angle_sensor = DEVICE_DT_GET(DT_NODELABEL(angle_sensor_0)),
+        .enable_pins = GPIO_DT_SPEC_GET_BY_IDX(DT_NODELABEL(inverter_enable1),gpios,0),
+        .current_rotor_deg = 0.0f,
+        .current_rotor_speed_dps = 0.0f,
+        .accumulated_angle_deg = 0.0f,
+        .pole_pairs = 2.0f,
+        .interface.reset = reset,
+        .interface.enable = enable,
+        .interface.disable = disable,
+        .interface.set_duty_cycles = set_duty_cycles,
+        .interface.get_rotor_angle = get_rotor_angle,
+        .interface.get_electrical_angle = get_electrical_angle,
+        .interface.get_rotor_speed_dps = get_rotor_speed_dps,
+        .interface.align_rotor = align_rotor,
+        .interface.get_acumulated_angle = get_acumulated_angle,
+        .interface.get_encoder_ppr = NULL,
+        .interface.get_dc_bus_voltage = get_dc_bus_voltage,
+        .interface.get_currents = NULL,
+        .interface.number_of_shunts = NULL,
+        .interface.fetch_sensors = fetch_sensors,
+        .interface.set_pole_pairs = set_pole_pairs,
+    }
+};
+
+struct esp_foc_motor_interface* z_esp_foc_get_interface(enum zephyr_esp_foc_instances instance)
+{
+    if(instance >= Z_ESP_FOC_MAX)
+        return NULL;
+
+    return &z_esp_hw_if[instance].interface;
+}
diff --git a/examples/stubs/motor_hardware_stub.h b/app/espfoc_motor/esp_foc_hardware_if.h
similarity index 83%
rename from examples/stubs/motor_hardware_stub.h
rename to app/espfoc_motor/esp_foc_hardware_if.h
index cc544d8..c47e020 100644
--- a/examples/stubs/motor_hardware_stub.h
+++ b/app/espfoc_motor/esp_foc_hardware_if.h
@@ -23,11 +23,12 @@
  */
 #pragma once
 
-#include <espFoc/esp_foc.h>
+#include <espFoC/motor_control/esp_foc.h>
 
-struct motor_hardware_stub {
-    struct esp_foc_motor_interface self;
+enum zephyr_esp_foc_instances {
+    Z_ESP_FOC_1 = 0,
+    Z_ESP_FOC_2,
+    Z_ESP_FOC_MAX,
 };
 
-int motor_hardware_stub_init(struct motor_hardware_stub *stub,
-                            struct esp_foc_motor_interface **itf);
+struct esp_foc_motor_interface* z_esp_foc_get_interface(enum zephyr_esp_foc_instances instance);
\ No newline at end of file
diff --git a/app/espfoc_motor/esp_foc_motor.c b/app/espfoc_motor/esp_foc_motor.c
new file mode 100644
index 0000000..2c03ea0
--- /dev/null
+++ b/app/espfoc_motor/esp_foc_motor.c
@@ -0,0 +1,202 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2021 Felipe Neves
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <zephyr/kernel.h>
+#include <espFoC/motor_control/esp_foc.h>
+#include <espFoC/ipc/esp_foc_ipc.h>
+#include <zephyr/drivers/counter.h>
+#include "esp_foc_hardware_if.h"
+
+#define ESP_FOC_INNER_CONTROL_US_PERIOD     (100)
+
+static struct counter_alarm_cfg alarm_cfg;
+static struct esp_foc_motor_control axis;
+static struct motor_state outgoing_state;
+static struct esp_foc_pid pid_vel_1;
+static struct esp_foc_pid pid_pos_1;
+static uint32_t timestamp_us = 0;
+static struct k_work_q espfoc_work_q;
+static struct k_work esp_foc_work;
+static bool global_enable = false;
+
+K_THREAD_STACK_DEFINE(espfoc_work_q_stack, 4096);
+
+static void esp_foc_motor_on_cmd (const struct motor_command *cmd)
+{
+    float position_deg = cmd->position_mdeg * 0.001f;
+    float speed_dps = cmd->speed_mdps * 0.001f;
+    float pid_gains[3] = {
+        (float)cmd->pid_gains[0] * 1e-6f,
+        (float)cmd->pid_gains[1] * 1e-6f,
+        (float)cmd->pid_gains[2] * 1e-6f,
+    };
+    float pwms[3] = {
+        (float)cmd->pwms[0] * 1e-6f,
+        (float)cmd->pwms[1] * 1e-6f,
+        (float)cmd->pwms[2] * 1e-6f,
+    };
+
+    int pole_pairs = (int)cmd->pole_pairs;
+
+    if(cmd->command_mask & MOTOR_CMD_GLOBAL_ENABLE) {
+        global_enable = true;
+    }
+
+    if(cmd->command_mask & MOTOR_CMD_GLOBAL_DISABLE) {
+        global_enable = false;
+    }
+
+    if(cmd->command_mask & MOTOR_CMD_SPEED_MASK) {
+        esp_foc_controller_set_speed(&axis, speed_dps);
+    }
+
+    if(cmd->command_mask & MOTOR_CMD_POSITION_MASK) {
+        esp_foc_controller_set_position(&axis, position_deg);
+    }
+
+    if(cmd->command_mask & MOTOR_CMD_SHUTDOWN_MASK) {
+        if(cmd->power_state) {
+            esp_foc_enable_axis(&axis);
+        } else {
+            esp_foc_disable_axis(&axis);
+        }
+    }
+
+    if(cmd->command_mask & MOTOR_CMD_RESET_MASK) {
+        esp_foc_motor_reset(z_esp_foc_get_interface(Z_ESP_FOC_1));
+    }
+
+    if(cmd->command_mask & MOTOR_CMD_SPD_PID_MASK) {
+        if(cmd->command_mask & MOTOR_CMD_KP_MASK) {
+            esp_foc_pid_set_kp(&pid_vel_1, pid_gains[0]);
+        }
+
+        if(cmd->command_mask & MOTOR_CMD_KI_MASK) {
+            esp_foc_pid_set_ki(&pid_vel_1, pid_gains[1]);
+        }
+
+        if(cmd->command_mask & MOTOR_CMD_KD_MASK) {
+            esp_foc_pid_set_kd(&pid_vel_1, pid_gains[2]);
+        }
+    }
+
+    if(cmd->command_mask & MOTOR_CMD_POS_PID_MASK) {
+        if(cmd->command_mask & MOTOR_CMD_KP_MASK) {
+            esp_foc_pid_set_kp(&pid_pos_1, pid_gains[0]);
+        }
+
+        if(cmd->command_mask & MOTOR_CMD_KI_MASK) {
+            esp_foc_pid_set_ki(&pid_pos_1, pid_gains[1]);
+        }
+
+        if(cmd->command_mask & MOTOR_CMD_KD_MASK) {
+            esp_foc_pid_set_kd(&pid_pos_1, pid_gains[2]);
+        }
+    }
+
+    if(cmd->command_mask & MOTOR_CMD_SET_POLE_PAIRS) {
+        esp_foc_motor_set_pole_pairs(z_esp_foc_get_interface(Z_ESP_FOC_1), pole_pairs);
+    }
+
+    if(cmd->command_mask & MOTOR_CMD_MAITENANCE_MODE) {
+        if(cmd->command_mask & MOTOR_CMD_MAITENANCE_PWMS) {
+            esp_foc_motor_set_duty_cycles(z_esp_foc_get_interface(Z_ESP_FOC_1), pwms[0], pwms[1], pwms[2]);
+        }
+    }
+}
+ESP_FOC_DEFINE_IPC_CALLBACK(cmd_callback,esp_foc_motor_on_cmd, NULL);
+
+static void esp_foc_control_work_handler(struct k_work *work)
+{
+    if(global_enable) {
+        esp_foc_controller_run(&axis);
+    }
+}
+
+static void esp_foc_timer_isr(struct k_timer *timer_id)
+{
+    k_work_submit_to_queue(&espfoc_work_q, &esp_foc_work);
+    timestamp_us += 500;
+}
+K_TIMER_DEFINE(espfoc_control_timer, esp_foc_timer_isr, esp_foc_timer_isr);
+
+
+/* We don't actually need a main function*/
+static int esp_foc_early_init(void)
+{
+    float sample_time_seconds = (float)ESP_FOC_INNER_CONTROL_US_PERIOD * 0.000001f;
+    k_work_queue_start(&espfoc_work_q, espfoc_work_q_stack,
+                       K_THREAD_STACK_SIZEOF(espfoc_work_q_stack), -3, NULL);
+    k_work_init(&esp_foc_work, esp_foc_control_work_handler);
+
+    esp_foc_pid_init(&pid_vel_1,
+                    sample_time_seconds * ESP_FOC_SPEED_CONTROL_RATIO,
+                    10000.0f);
+
+    esp_foc_pid_init(&pid_pos_1,
+                    sample_time_seconds * ESP_FOC_POSITION_CONTROL_RATIO,
+                    10000.0f);
+
+    esp_foc_init_controller(&axis, z_esp_foc_get_interface(Z_ESP_FOC_1));
+    esp_foc_add_position_control(&axis, &pid_pos_1);
+    esp_foc_add_speed_control(&axis, &pid_vel_1);
+
+    // k_timer_start(&espfoc_control_timer, K_MSEC(ESP_FOC_INNER_CONTROL_US_PERIOD),
+    //               K_MSEC(ESP_FOC_INNER_CONTROL_US_PERIOD));
+
+    esp_foc_ipc_init();
+    esp_foc_ipc_register_callback(&cmd_callback);
+
+    return 0;
+}
+SYS_INIT(esp_foc_early_init, POST_KERNEL, CONFIG_APPLICATION_INIT_PRIORITY);
+
+int main(void)
+{
+    int keep_alive_cntr = 500;
+    while(1) {
+        k_sleep(K_USEC(1000));
+
+        if(global_enable) {
+            /* Report motor state each 1 milisecond asynchronously */
+            outgoing_state.timestamp_us = timestamp_us;
+            outgoing_state.position_mdeg = (int32_t)(axis.current_position_degrees * 1000.0f);
+            outgoing_state.speed_mdps = (int32_t)(axis.current_speed_dps * 1000.0f);
+            outgoing_state.power_state = (uint8_t)axis.is_enabled;
+            outgoing_state.system_enabled = 1;
+            outgoing_state.motor_system_uptime = k_uptime_get_32();
+            esp_foc_ipc_send_state(&outgoing_state);
+        } else {
+            /* Report a keep alive telling procpu the system is alive each 2s */
+            keep_alive_cntr--;
+            if(!keep_alive_cntr) {
+                keep_alive_cntr = 500;
+                outgoing_state.timestamp_us = timestamp_us;
+                outgoing_state.system_enabled = 0;
+                outgoing_state.motor_system_uptime = k_uptime_get_32();
+                esp_foc_ipc_send_state(&outgoing_state);
+            }
+        }
+    }
+}
diff --git a/app/espfoc_motor/prj.conf b/app/espfoc_motor/prj.conf
new file mode 100644
index 0000000..0af1acb
--- /dev/null
+++ b/app/espfoc_motor/prj.conf
@@ -0,0 +1,12 @@
+CONFIG_HEAP_MEM_POOL_SIZE=4096
+CONFIG_ISR_STACK_SIZE=4096
+
+CONFIG_IPM=y
+CONFIG_I2C=y
+CONFIG_SENSOR=y
+CONFIG_SENSOR=y
+CONFIG_AMS_AS5600=y
+
+CONFIG_PINCTRL=y
+CONFIG_GPIO=y
+CONFIG_CLOCK_CONTROL=y
diff --git a/app/prj.conf b/app/prj.conf
new file mode 100644
index 0000000..54aa845
--- /dev/null
+++ b/app/prj.conf
@@ -0,0 +1,11 @@
+CONFIG_PRINTK=y
+CONFIG_SHELL=y
+CONFIG_IPM=y
+CONFIG_LOG=y
+
+CONFIG_MAIN_STACK_SIZE=8192
+CONFIG_HEAP_MEM_POOL_SIZE=4096
+CONFIG_ISR_STACK_SIZE=4096
+
+CONFIG_PWM=n
+CONFIG_I2C=n
diff --git a/app/sysbuild.cmake b/app/sysbuild.cmake
new file mode 100644
index 0000000..54a75d3
--- /dev/null
+++ b/app/sysbuild.cmake
@@ -0,0 +1,14 @@
+# Add external project
+ExternalZephyrProject_Add(
+    APPLICATION espfoc_motor_remote
+    SOURCE_DIR ${APP_DIR}/espfoc_motor
+    BOARD ${SB_CONFIG_ESP_FOC_REMOTE_BOARD}
+  )
+
+add_dependencies(app espfoc_motor_remote)
+sysbuild_add_dependencies(CONFIGURE app espfoc_motor_remote)
+
+if(SB_CONFIG_BOOTLOADER_MCUBOOT)
+  # Make sure MCUboot is flashed first
+  sysbuild_add_dependencies(FLASH espfoc_motor_remote mcuboot)
+endif()
diff --git a/common/include/espFoC/ipc/esp_foc_ipc.h b/common/include/espFoC/ipc/esp_foc_ipc.h
new file mode 100644
index 0000000..9f04283
--- /dev/null
+++ b/common/include/espFoC/ipc/esp_foc_ipc.h
@@ -0,0 +1,95 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2024 Felipe Neves
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <zephyr/kernel.h>
+
+#define MOTOR_CMD_TORQUE_MASK       0x00
+#define MOTOR_CMD_SPEED_MASK        0x02
+#define MOTOR_CMD_POSITION_MASK     0x04
+#define MOTOR_CMD_SHUTDOWN_MASK     0x08
+#define MOTOR_CMD_RESET_MASK        0x10
+#define MOTOR_CMD_KP_MASK           0x20
+#define MOTOR_CMD_KI_MASK           0x40
+#define MOTOR_CMD_KD_MASK           0x80
+#define MOTOR_CMD_POS_PID_MASK      0x100
+#define MOTOR_CMD_SPD_PID_MASK      0x200
+#define MOTOR_CMD_SET_POLE_PAIRS    0x400
+#define MOTOR_CMD_MAITENANCE_MODE   0x8000
+#define MOTOR_CMD_MAITENANCE_PWMS   0x10000
+#define MOTOR_CMD_GLOBAL_ENABLE     0x20000
+#define MOTOR_CMD_GLOBAL_DISABLE    0x40000
+
+struct motor_state {
+    uint32_t timestamp_us;
+    int32_t position_mdeg;
+    int32_t speed_mdps;
+    int32_t qvoltage_mvolts;
+    int32_t dvoltage_mvolts;
+    uint32_t encoder;
+    uint8_t power_state;
+    uint8_t last_cmd_result;
+    uint8_t motor_number;
+    uint8_t direction;
+    uint8_t system_enabled;
+    uint32_t motor_system_uptime;
+};
+
+struct motor_command {
+    uint32_t command_mask;
+    int32_t speed_mdps;
+    int32_t position_mdeg;
+    int32_t qvoltage_mvolts;
+    int32_t dvoltage_mvolts;
+    uint32_t pid_gains[3];
+    uint32_t pwms[3];
+    uint8_t power_state;
+    uint8_t motor_number;
+    uint8_t direction;
+    uint8_t pole_pairs;
+};
+
+typedef void (*esp_foc_ipc_motor_state_callback_t) (const struct motor_state *state);
+typedef void (*esp_foc_ipc_motor_cmd_callback_t) (const struct motor_command *cmd);
+
+struct motor_report_callback {
+    esp_foc_ipc_motor_state_callback_t state_cb;
+    esp_foc_ipc_motor_cmd_callback_t cmd_cb;
+    sys_dnode_t node;
+};
+
+#define ESP_FOC_DEFINE_IPC_CALLBACK(name,cmd_callback, state_callback) \
+struct motor_report_callback name = {       \
+    .state_cb = state_callback,             \
+    .cmd_cb = cmd_callback,                 \
+}
+
+int esp_foc_ipc_init(void);
+int esp_foc_ipc_send_command(const struct motor_command *cmd);
+int esp_foc_ipc_send_state(const struct motor_state *state);
+int esp_foc_ipc_register_callback(const struct motor_report_callback *cb);
+int esp_foc_ipc_remove_callback(const struct motor_report_callback *cb);
\ No newline at end of file
diff --git a/include/espFoC/esp_foc.h b/common/include/espFoC/motor_control/esp_foc.h
similarity index 83%
rename from include/espFoC/esp_foc.h
rename to common/include/espFoC/motor_control/esp_foc.h
index 0105b2a..7ab62b7 100644
--- a/include/espFoC/esp_foc.h
+++ b/common/include/espFoC/motor_control/esp_foc.h
@@ -24,16 +24,17 @@
 
 #pragma once
 
+#include <stdbool.h>
 #include <math.h>
 #include <errno.h>
-#include <espFoC/esp_foc_motor_interface.h>
-#include <espFoC/esp_foc_svm.h>
-#include <espFoC/esp_foc_pid.h>
+#include "esp_foc_motor_interface.h"
+#include "esp_foc_svm.h"
+#include "esp_foc_pid.h"
 
 struct esp_foc_motor_control {
     struct esp_foc_motor_interface *motor_hw;
-    struct esp_foc_pid pid_position;
-    struct esp_foc_pid pid_velocity;
+    struct esp_foc_pid *pid_position;
+    struct esp_foc_pid *pid_velocity;
 
     int position_control_cntr;
     int speed_control_cntr;
@@ -42,8 +43,13 @@ struct esp_foc_motor_control {
     float target_position_degrees;
     float current_speed_dps;
     float current_position_degrees;
+
+    bool is_enabled;
 };
 
+extern const int ESP_FOC_SPEED_CONTROL_RATIO;
+extern const int ESP_FOC_POSITION_CONTROL_RATIO;
+
 int esp_foc_init_controller(struct esp_foc_motor_control *ctl,
                             const struct esp_foc_motor_interface *hw);
 
@@ -59,4 +65,8 @@ int esp_foc_add_position_control(struct esp_foc_motor_control *ctl,
 int esp_foc_add_speed_control(struct esp_foc_motor_control *ctl,
                                 const struct esp_foc_pid *speed_pid);
 
+int esp_foc_enable_axis(struct esp_foc_motor_control *ctl);
+
+int esp_foc_disable_axis(struct esp_foc_motor_control *ctl);
+
 int esp_foc_controller_run(struct esp_foc_motor_control *ctl);
diff --git a/include/espFoC/esp_foc_motor_interface.h b/common/include/espFoC/motor_control/esp_foc_motor_interface.h
similarity index 63%
rename from include/espFoC/esp_foc_motor_interface.h
rename to common/include/espFoC/motor_control/esp_foc_motor_interface.h
index 3322acd..e8b4711 100644
--- a/include/espFoC/esp_foc_motor_interface.h
+++ b/common/include/espFoC/motor_control/esp_foc_motor_interface.h
@@ -27,8 +27,10 @@
 struct esp_foc_motor_interface {
     int (*reset)(struct esp_foc_motor_interface *self);
     int (*enable)(struct esp_foc_motor_interface *self);
+    int (*disable)(struct esp_foc_motor_interface *self);
     int (*set_duty_cycles)(struct esp_foc_motor_interface *self, float a, float b, float c);
     int (*get_rotor_angle)(struct esp_foc_motor_interface *self, float *angle);
+    int (*get_electrical_angle)(struct esp_foc_motor_interface *self, float *angle);
     int (*get_rotor_speed_dps)(struct esp_foc_motor_interface *self, float *speed_dps);
     int (*align_rotor)(struct esp_foc_motor_interface *self);
     int (*get_acumulated_angle)(struct esp_foc_motor_interface *self, float *angle);
@@ -37,9 +39,10 @@ struct esp_foc_motor_interface {
     int (*get_currents)(struct esp_foc_motor_interface *self, float *ia, float *ib, float *ic);
     int (*number_of_shunts)(struct esp_foc_motor_interface *self, int *shunts);
     int (*fetch_sensors)(struct esp_foc_motor_interface *self);
+    int (*set_pole_pairs)(struct esp_foc_motor_interface *self, int32_t pole_pairs);
 };
 
-int esp_foc_motor_reset(struct esp_foc_motor_interface *self)
+static inline int esp_foc_motor_reset(struct esp_foc_motor_interface *self)
 {
     if(!self)
         return -EINVAL;
@@ -51,7 +54,7 @@ int esp_foc_motor_reset(struct esp_foc_motor_interface *self)
 
 }
 
-int esp_foc_motor_enable(struct esp_foc_motor_interface *self)
+static inline int esp_foc_motor_enable(struct esp_foc_motor_interface *self)
 {
     if(!self)
         return -EINVAL;
@@ -62,7 +65,18 @@ int esp_foc_motor_enable(struct esp_foc_motor_interface *self)
     return self->enable(self);
 }
 
-int esp_foc_motor_set_duty_cycles(struct esp_foc_motor_interface *self, float a, float b, float c)
+static inline int esp_foc_motor_disable(struct esp_foc_motor_interface *self)
+{
+    if(!self)
+        return -EINVAL;
+
+    if(!self->disable)
+        return -ENOTSUP;
+
+    return self->disable(self);
+}
+
+static inline int esp_foc_motor_set_duty_cycles(struct esp_foc_motor_interface *self, float a, float b, float c)
 {
     if(!self)
         return -EINVAL;
@@ -70,10 +84,10 @@ int esp_foc_motor_set_duty_cycles(struct esp_foc_motor_interface *self, float a,
     if(!self->set_duty_cycles)
         return -ENOTSUP;
 
-    return self->set_duty_cycles(Self, a, b, c);
+    return self->set_duty_cycles(self, a, b, c);
 }
 
-int esp_foc_motor_get_rotor_angle(struct esp_foc_motor_interface *self, float *angle)
+static inline int esp_foc_motor_get_rotor_angle(struct esp_foc_motor_interface *self, float *angle)
 {
     if(!self)
         return -EINVAL;
@@ -84,7 +98,18 @@ int esp_foc_motor_get_rotor_angle(struct esp_foc_motor_interface *self, float *a
     return self->get_rotor_angle(self, angle);
 }
 
-int esp_foc_motor_get_rotor_speed_dps(struct esp_foc_motor_interface *self, float *speed_dps)
+static inline int esp_foc_motor_get_electrical_angle(struct esp_foc_motor_interface *self, float *angle)
+{
+    if(!self)
+        return -EINVAL;
+
+    if(!self->get_electrical_angle)
+        return -ENOTSUP;
+
+    return self->get_electrical_angle(self, angle);
+}
+
+static inline int esp_foc_motor_get_rotor_speed_dps(struct esp_foc_motor_interface *self, float *speed_dps)
 {
     if(!self)
         return -EINVAL;
@@ -95,7 +120,7 @@ int esp_foc_motor_get_rotor_speed_dps(struct esp_foc_motor_interface *self, floa
     return self->get_rotor_speed_dps(self, speed_dps);
 }
 
-int esp_foc_motor_align_rotor(struct esp_foc_motor_interface *self)
+static inline int esp_foc_motor_align_rotor(struct esp_foc_motor_interface *self)
 {
     if(!self)
         return -EINVAL;
@@ -106,18 +131,18 @@ int esp_foc_motor_align_rotor(struct esp_foc_motor_interface *self)
     return self->align_rotor(self);
 }
 
-int esp_foc_motor_get_acumulated_angle(struct esp_foc_motor_interface *self, float *angle)
+static inline int esp_foc_motor_get_acumulated_angle(struct esp_foc_motor_interface *self, float *angle)
 {
     if(!self)
         return -EINVAL;
 
-    if(!self->get_acumulated_encoder)
+    if(!self->get_acumulated_angle)
         return -ENOTSUP;
 
     return self->get_acumulated_angle(self, angle);
 }
 
-int esp_foc_motor_get_encoder_ppr(struct esp_foc_motor_interface *self, float *ppr)
+static inline int esp_foc_motor_get_encoder_ppr(struct esp_foc_motor_interface *self, float *ppr)
 {
     if(!self)
         return -EINVAL;
@@ -128,7 +153,7 @@ int esp_foc_motor_get_encoder_ppr(struct esp_foc_motor_interface *self, float *p
     return self->get_encoder_ppr(self, ppr);
 }
 
-int esp_foc_motor_get_dc_bus_voltage(struct esp_foc_motor_interface *self, float *vdc)
+static inline int esp_foc_motor_get_dc_bus_voltage(struct esp_foc_motor_interface *self, float *vdc)
 {
     if(!self)
         return -EINVAL;
@@ -139,7 +164,7 @@ int esp_foc_motor_get_dc_bus_voltage(struct esp_foc_motor_interface *self, float
     return self->get_dc_bus_voltage(self, vdc);
 }
 
-int esp_foc_motor_get_currents(struct esp_foc_motor_interface *self, float *ia, float *ib, float *ic)
+static inline int esp_foc_motor_get_currents(struct esp_foc_motor_interface *self, float *ia, float *ib, float *ic)
 {
     if(!self)
         return -EINVAL;
@@ -150,7 +175,7 @@ int esp_foc_motor_get_currents(struct esp_foc_motor_interface *self, float *ia,
     return self->get_currents(self, ia, ib, ic);
 }
 
-int esp_foc_motor_number_of_shunts(struct esp_foc_motor_interface *self, int *shunts)
+static inline int esp_foc_motor_number_of_shunts(struct esp_foc_motor_interface *self, int *shunts)
 {
     if(!self)
         return -EINVAL;
@@ -161,7 +186,7 @@ int esp_foc_motor_number_of_shunts(struct esp_foc_motor_interface *self, int *sh
     return self->number_of_shunts(self, shunts);
 }
 
-int esp_foc_motor_fetch_sensors(struct esp_foc_motor_interface *self)
+static inline int esp_foc_motor_fetch_sensors(struct esp_foc_motor_interface *self)
 {
     if(!self)
         return -EINVAL;
@@ -170,4 +195,15 @@ int esp_foc_motor_fetch_sensors(struct esp_foc_motor_interface *self)
         return -ENOTSUP;
 
     return self->fetch_sensors(self);
-}
\ No newline at end of file
+}
+
+static inline int esp_foc_motor_set_pole_pairs(struct esp_foc_motor_interface *self, int32_t pole_pairs)
+{
+    if(!self)
+        return -EINVAL;
+
+    if(!self->set_pole_pairs)
+        return -ENOTSUP;
+
+    return self->set_pole_pairs(self, pole_pairs);
+}
diff --git a/include/espFoC/esp_foc_pid.h b/common/include/espFoC/motor_control/esp_foc_pid.h
similarity index 100%
rename from include/espFoC/esp_foc_pid.h
rename to common/include/espFoC/motor_control/esp_foc_pid.h
diff --git a/include/espFoC/esp_foc_svm.h b/common/include/espFoC/motor_control/esp_foc_svm.h
similarity index 100%
rename from include/espFoC/esp_foc_svm.h
rename to common/include/espFoC/motor_control/esp_foc_svm.h
diff --git a/common/ipc/esp_foc_ipc.c b/common/ipc/esp_foc_ipc.c
new file mode 100644
index 0000000..575cd8e
--- /dev/null
+++ b/common/ipc/esp_foc_ipc.c
@@ -0,0 +1,197 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2024 Felipe Neves
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <string.h>
+#include <zephyr/device.h>
+#include <zephyr/drivers/ipm.h>
+#include <espFoC/ipc/esp_foc_ipc.h>
+#include <soc.h>
+
+#define LOCK_FREE_VAL 0xA5A5A5A5
+#define DIRECTION_TX 0x01
+#define DIRECTION_RX 0x81
+
+#define ESP_FOC_IPC_MAGIC       0x45466f43
+#define ESP_FOC_IPC_CMD_MAGIC   0x434d4d44
+#define ESP_FOC_IPC_STATE_MAGIC 0x45466f43
+#define ESP_FOC_TX_MAGIC        0x4d454d54
+#define ESP_FOC_RX_MAGIC        0x4d454d52
+
+static const struct device *ipc_dev = DEVICE_DT_GET(DT_NODELABEL(ipm0));
+static uint8_t *ipc_shm = (uint8_t *)(DT_REG_ADDR(DT_CHOSEN(zephyr_ipc_shm)));
+
+static atomic_t *shm_lock;
+static struct motor_command *tx_motor_cmd;
+static struct motor_command *rx_motor_cmd;
+static struct motor_state *tx_motor_state;
+static struct motor_state *rx_motor_state;
+
+#define MAGIC_OFFSET    0
+#define LOCK_OFFSET     12
+#define MEMT_OFFSET     (LOCK_OFFSET + sizeof(atomic_t))
+#define TX_CMD_OFFSET   (MEMT_OFFSET + sizeof(uint32_t))
+#define RX_CMD_OFFSET   (TX_CMD_OFFSET + sizeof(struct motor_command))
+#define MEMR_OFFSET     (RX_CMD_OFFSET + sizeof(struct motor_command))
+#define TX_STATE_OFFSET  (MEMR_OFFSET + sizeof(uint32_t))
+#define RX_STATE_OFFSET (TX_STATE_OFFSET + sizeof(struct motor_state))
+
+static sys_dlist_t ipc_callbacks;
+static uint32_t dummy_word;
+
+static struct k_work_q ipc_work_q;
+static struct k_work ipc_work;
+K_THREAD_STACK_DEFINE(ipc_work_q_stack, 4096);
+
+static void ipc_work_handler(struct k_work *work)
+{
+    struct motor_command rx_cmd;
+    struct motor_state rx_state;
+
+    sys_dnode_t *link;
+
+    while (!atomic_cas(shm_lock, LOCK_FREE_VAL,
+            DIRECTION_RX)) {
+        ;
+    }
+
+    memcpy(&rx_cmd, rx_motor_cmd, sizeof(rx_cmd));
+    memcpy(&rx_state, rx_motor_state, sizeof(rx_state));
+    rx_motor_cmd->command_mask = 0;
+    atomic_set(shm_lock, LOCK_FREE_VAL);
+
+
+    SYS_DLIST_FOR_EACH_NODE(&ipc_callbacks, link) {
+        struct motor_report_callback * report = CONTAINER_OF(link, struct motor_report_callback, node);
+        if(report) {
+            if(report->cmd_cb) {
+                report->cmd_cb((const struct motor_command *)&rx_cmd);
+            }
+
+            if(report->state_cb) {
+                report->state_cb((const struct motor_state *)&rx_state);
+            }
+        }
+    }
+}
+
+static void ipc_dev_callback(const struct device *dev, void *context,
+				  uint32_t id, volatile void *data)
+{
+    k_work_submit_to_queue(&ipc_work_q, &ipc_work);
+}
+
+int esp_foc_ipc_init(void)
+{
+    k_work_queue_start(&ipc_work_q, ipc_work_q_stack,
+                       K_THREAD_STACK_SIZEOF(ipc_work_q_stack), -1, NULL);
+    k_work_init(&ipc_work, ipc_work_handler);
+
+    shm_lock = (atomic_t *)&ipc_shm[LOCK_OFFSET];
+
+    if(esp_core_id() == 0) {
+        /* Only pro cpu initializes the shared memory*/
+        atomic_set(shm_lock, 0);
+        memset(ipc_shm, 0xAA, DT_REG_SIZE(DT_CHOSEN(zephyr_ipc_shm)));
+
+        uint32_t *magic = (uint32_t *)&ipc_shm[0];
+        uint32_t *memr = (uint32_t *)&ipc_shm[MEMR_OFFSET];
+        uint32_t *memt = (uint32_t *)&ipc_shm[MEMT_OFFSET];
+
+        magic[0] = ESP_FOC_IPC_MAGIC;
+        magic[1] = ESP_FOC_IPC_CMD_MAGIC;
+        magic[2] = ESP_FOC_IPC_STATE_MAGIC;
+        memt[0] = ESP_FOC_TX_MAGIC;
+        memr[0] = ESP_FOC_RX_MAGIC;
+        atomic_set(shm_lock, LOCK_FREE_VAL);
+    }
+
+    tx_motor_cmd = (struct motor_command *)&ipc_shm[TX_CMD_OFFSET];
+    rx_motor_cmd = (struct motor_command *)&ipc_shm[RX_CMD_OFFSET];
+    tx_motor_state = (struct motor_state *)&ipc_shm[TX_STATE_OFFSET];
+    rx_motor_state = (struct motor_state *)&ipc_shm[RX_STATE_OFFSET];
+
+    sys_dlist_init(&ipc_callbacks);
+    ipm_register_callback(ipc_dev, ipc_dev_callback, NULL);
+
+    return 0;
+}
+int esp_foc_ipc_send_command(const struct motor_command *cmd)
+{
+    if(!cmd)
+        return -EINVAL;
+
+    uint32_t key = irq_lock();
+    while (!atomic_cas(shm_lock, LOCK_FREE_VAL,
+            DIRECTION_TX)) {
+        ;
+    }
+
+    memcpy(rx_motor_cmd, cmd, sizeof(*rx_motor_cmd));
+    atomic_set(shm_lock, LOCK_FREE_VAL);
+    irq_unlock(key);
+
+    return ipm_send(ipc_dev, -1, sizeof(dummy_word), &dummy_word, sizeof(dummy_word));
+}
+
+int esp_foc_ipc_send_state(const struct motor_state *state)
+{
+    if(!state)
+        return -EINVAL;
+
+    uint32_t key = irq_lock();
+    while (!atomic_cas(shm_lock, LOCK_FREE_VAL,
+            DIRECTION_TX)) {
+        ;
+    }
+
+    memcpy(rx_motor_state, state, sizeof(*rx_motor_cmd));
+    atomic_set(shm_lock, LOCK_FREE_VAL);
+    irq_unlock(key);
+
+    return ipm_send(ipc_dev, -1, sizeof(dummy_word), &dummy_word, sizeof(dummy_word));
+}
+
+int esp_foc_ipc_register_callback(const struct motor_report_callback *cb)
+{
+    if(!cb)
+        return -EINVAL;
+
+    uint32_t key = irq_lock();
+    sys_dlist_append(&ipc_callbacks, (sys_dnode_t *)&cb->node);
+    irq_unlock(key);
+
+    return 0;
+}
+
+int esp_foc_ipc_remove_callback(const struct motor_report_callback *cb)
+{
+    if(!cb)
+        return -EINVAL;
+
+    uint32_t key = irq_lock();
+    sys_dlist_remove((sys_dnode_t *)&cb->node);
+    irq_unlock(key);
+
+    return 0;
+}
diff --git a/src/esp_foc.c b/common/motor_control/esp_foc.c
similarity index 72%
rename from src/esp_foc.c
rename to common/motor_control/esp_foc.c
index dc33bfb..cb3be48 100644
--- a/src/esp_foc.c
+++ b/common/motor_control/esp_foc.c
@@ -22,8 +22,9 @@
  * SOFTWARE.
  */
 
+#include <errno.h>
 #include <math.h>
-#include <espFoC/esp_foc.h>
+#include <espFoC/motor_control/esp_foc.h>
 
 extern const float ESP_FOC_M_PI;
 extern const float ESP_FOC_2_M_PI;
@@ -38,7 +39,7 @@ extern void ours_arm_sin_cos_f32(float theta,float *pSinVal,float *pCosVal);
 
 static int do_foc_core_operations(struct esp_foc_motor_control *ctl,
                                 const float *vq,
-                                const float *vd);
+                                const float *vd)
 {
     float alpha;
     float beta;
@@ -53,16 +54,16 @@ static int do_foc_core_operations(struct esp_foc_motor_control *ctl,
     if(!ctl || !vq || !vd)
         return -EINVAL;
 
-    err = esp_foc_motor_get_rotor_angle(ctl->hw, &e_angle);
+    err = esp_foc_motor_get_rotor_angle(ctl->motor_hw, &e_angle);
     if(err)
         return err;
 
-    ours_arm_sin_cos_f32(eangle, &sine, &cosine);
+    ours_arm_sin_cos_f32(e_angle, &sine, &cosine);
     alpha = i_vd * cosine - i_vq * sine;
     beta = i_vd * sine + i_vq * cosine;
     err = esp_foc_do_space_vector_pwm(alpha, beta, &u, &v, &w);
 
-    return esp_foc_motor_set_duty_cycles(ctl->hw, u, v, w);
+    return esp_foc_motor_set_duty_cycles(ctl->motor_hw, u, v, w);
 }
 
 int esp_foc_init_controller(struct esp_foc_motor_control *ctl,
@@ -73,16 +74,18 @@ int esp_foc_init_controller(struct esp_foc_motor_control *ctl,
     }
 
     ctl->pid_position = NULL;
-    ctl->speed_pid_position = NULL;
-    ctl->target_speed = 0.0f;
-    ctl->target_position = 0.0f;
-    ctl->motor_hw = hw;
+    ctl->pid_velocity = NULL;
+    ctl->target_speed_dps = 0.0f;
+    ctl->target_position_degrees = 0.0f;
+    ctl->motor_hw = (struct esp_foc_motor_interface *)hw;
 
-    if(esp_foc_motor_enable(ctl->motor_hw)) {
+    if(esp_foc_motor_disable(ctl->motor_hw)) {
         return -ENODEV;
     }
 
-    return esp_foc_motor_reset(ctl->motor_hw);
+    ctl->is_enabled = false;
+
+    return 0;
 }
 
 int esp_foc_controller_set_speed(struct esp_foc_motor_control *ctl,
@@ -112,7 +115,7 @@ int esp_foc_add_position_control(struct esp_foc_motor_control *ctl,
     if(!ctl || !pos_pid)
         return -EINVAL;
 
-    ctl->pid_position = pos_pid;
+    ctl->pid_position = (struct esp_foc_pid *)pos_pid;
     ctl->position_control_cntr = ESP_FOC_POSITION_CONTROL_RATIO;
 
     return 0;
@@ -121,15 +124,57 @@ int esp_foc_add_position_control(struct esp_foc_motor_control *ctl,
 int esp_foc_add_speed_control(struct esp_foc_motor_control *ctl,
                                 const struct esp_foc_pid *speed_pid)
 {
-    if(!ctl || !pos_pid)
+    if(!ctl || !speed_pid)
         return -EINVAL;
 
-    ctl->speed_pid = speed_pid;
+    ctl->pid_velocity = (struct esp_foc_pid *)speed_pid;
     ctl->speed_control_cntr = ESP_FOC_SPEED_CONTROL_RATIO;
 
     return 0;
 }
 
+int esp_foc_enable_axis(struct esp_foc_motor_control *ctl)
+{
+    int r;
+
+    if(!ctl)
+        return -EINVAL;
+
+    if(ctl->is_enabled)
+        return EALREADY;
+
+    r = esp_foc_motor_enable(ctl->motor_hw);
+    if(r)
+        return r;
+
+    r = esp_foc_motor_reset(ctl->motor_hw);
+    if(r)
+        return r;
+
+    ctl->is_enabled = true;
+
+    return 0;
+}
+
+int esp_foc_disable_axis(struct esp_foc_motor_control *ctl)
+{
+    if(!ctl)
+        return -EINVAL;
+
+    if(!ctl->is_enabled)
+        return EALREADY;
+
+    int r = esp_foc_motor_disable(ctl->motor_hw);
+    if(r)
+        return r;
+
+    ctl->target_speed_dps = 0.0f;
+    ctl->target_position_degrees = 0.0f;
+    ctl->is_enabled = false;
+
+    return 0;
+}
+
 int esp_foc_controller_run(struct esp_foc_motor_control *ctl)
 {
     float vd = 0.0f;
@@ -141,11 +186,14 @@ int esp_foc_controller_run(struct esp_foc_motor_control *ctl)
     if(!ctl)
         return -EINVAL;
 
-    esp_foc_motor_fetch_sensors(ctl->hw);
+    if(!ctl->is_enabled)
+        return -EIO;
+
+    esp_foc_motor_fetch_sensors(ctl->motor_hw);
 
     if(ctl->pid_position) {
         float current_angle;
-        int err = esp_foc_motor_get_acumulated_angle(ctl->hw, &current_angle);
+        int err = esp_foc_motor_get_acumulated_angle(ctl->motor_hw, &current_angle);
 
         if(pos_cntr && !err) {
             pos_cntr--;
@@ -158,9 +206,9 @@ int esp_foc_controller_run(struct esp_foc_motor_control *ctl)
         }
     }
 
-    if(ctl->speed_pid) {
+    if(ctl->pid_velocity) {
         float current_speed;
-        int err = esp_foc_motor_get_rotor_speed_dps(ctl->hw, &current_speed);
+        esp_foc_motor_get_rotor_speed_dps(ctl->motor_hw, &current_speed);
 
         if(spd_cntr) {
             spd_cntr--;
diff --git a/src/esp_foc_consts.c b/common/motor_control/esp_foc_consts.c
similarity index 95%
rename from src/esp_foc_consts.c
rename to common/motor_control/esp_foc_consts.c
index 7bd3d7e..29b0c2f 100644
--- a/src/esp_foc_consts.c
+++ b/common/motor_control/esp_foc_consts.c
@@ -24,8 +24,8 @@
 
 const float ESP_FOC_M_PI = 3.14159f;
 const float ESP_FOC_2_M_PI = (2.0f * ESP_FOC_M_PI);
-const float ESP_FOC_2_3 = (2.0/3.0f);
-const float ESP_FOC_1_3 = (1.0/3.0f);
+const float ESP_FOC_2_3 = (2.0f/3.0f);
+const float ESP_FOC_1_3 = (1.0f/3.0f);
 const float ESP_FOC_SQRT3 = 1.73205080757f;
 const float ESP_FOC_1_SQRT3 = (1.0f / ESP_FOC_SQRT3);
 const float ESP_FOC_2_SQRT3 = (2.0f / ESP_FOC_SQRT3);
diff --git a/src/esp_foc_our_sin_cos.c b/common/motor_control/esp_foc_our_sin_cos.c
similarity index 100%
rename from src/esp_foc_our_sin_cos.c
rename to common/motor_control/esp_foc_our_sin_cos.c
diff --git a/src/esp_foc_pid.c b/common/motor_control/esp_foc_pid.c
similarity index 98%
rename from src/esp_foc_pid.c
rename to common/motor_control/esp_foc_pid.c
index 83085e6..4a825ca 100644
--- a/src/esp_foc_pid.c
+++ b/common/motor_control/esp_foc_pid.c
@@ -22,7 +22,8 @@
  * SOFTWARE.
  */
 
-#include <espFoc/esp_foc_pid.h>
+#include <errno.h>
+#include <espFoC/motor_control/esp_foc_pid.h>
 
 int esp_foc_pid_init(struct esp_foc_pid *p, float sample_time_seconds, float integral_limit)
 {
diff --git a/src/esp_foc_svm.c b/common/motor_control/esp_foc_svm.c
similarity index 88%
rename from src/esp_foc_svm.c
rename to common/motor_control/esp_foc_svm.c
index 47861b7..8d90853 100644
--- a/src/esp_foc_svm.c
+++ b/common/motor_control/esp_foc_svm.c
@@ -2,7 +2,7 @@
  * Copyright (c) 2021 Teslabs Engineering S.L.
  * SPDX-License-Identifier: Apache-2.0
  */
-#include <espFoC/esp_foc.h>
+#include <espFoC/motor_control/esp_foc.h>
 
 extern const float ESP_FOC_SQRT3;
 extern const float ESP_FOC_1_SQRT3;
@@ -45,25 +45,22 @@ int esp_foc_do_space_vector_pwm(const float v_alpha,
                         float *w)
 {
 	float a, b, c, mod;
+	float alpha = v_alpha, beta = v_beta;
 	float x, y, z;
 
-    if(!u || !v || !w)
-        return -EINVAL;
-
 	/* normalize and limit alpha-beta vector */
 	mod = sqrtf(v_alpha * v_alpha + v_beta * v_beta);
 	if (mod > ESP_FOC_SQRT3_2) {
-		v_alpha = v_alpha / mod * ESP_FOC_SQRT3_2;
-		v_beta = v_beta / mod * ESP_FOC_SQRT3_2;
+		alpha = v_alpha / mod * ESP_FOC_SQRT3_2;
+		beta = v_beta / mod * ESP_FOC_SQRT3_2;
 	}
 
 	/* do a modified inverse clarke transform to get an auxiliary frame
 	 * to compute the sector we are
 	 */
 
-
-	a = v_alpha - ESP_FOC_1_SQRT3 * v_beta;
-	b = ESP_FOC_2_SQRT3 * v_beta;
+	a = alpha - ESP_FOC_1_SQRT3 * beta;
+	b = ESP_FOC_2_SQRT3 * beta;
 	c = -(a + b);
 
 	switch (get_sector(a, b, c)) {
diff --git a/doc/images/arch.png b/doc/images/arch.png
new file mode 100644
index 0000000..9ddd309
Binary files /dev/null and b/doc/images/arch.png differ
diff --git a/examples/stubs/main.cpp b/examples/stubs/main.cpp
deleted file mode 100644
index 0a7a080..0000000
--- a/examples/stubs/main.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-#include <unistd.h>
-#include <signal.h>
-#include <stdio.h>
-#include <espFoC/esp_foc.h>
-
-struct motor_hardware_stub stub;
-struct esp_foc_motor_interface *hw;
-struct esp_foc_motor_control control;
-int should_stop = 0;
-
-using namespace std;
-
-int main(int argc, char **argv)
-{
-    int err;
-
-    err = motor_hardware_stub_init(&stub, &hw);
-    if(err)
-        cout << "failed to initialize the motor hardware err:" << err << endl;
-
-    err = esp_foc_init_controller(&control, hw);
-    if(err)
-        cout << "failed to initialize esp foc controller err:" << err << endl;
-
-    err = esp_foc_controller_set_speed(&control, 360.0f);
-    if(err)
-        cout << "failed to set the initial speed err" << err << endl;
-
-    while(!should_stop) {
-        err = esp_foc_controller_run(&ctl);
-        if(err)
-            cout << "error when running controller err:" << err << endl;
-        usleep(1000);
-    }
-}
diff --git a/examples/stubs/motor_hardware_stub.c b/examples/stubs/motor_hardware_stub.c
deleted file mode 100644
index 44a46bc..0000000
--- a/examples/stubs/motor_hardware_stub.c
+++ /dev/null
@@ -1,133 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 2021 Felipe Neves
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-#include <math.h>
-#include "motor_hardware_stub.h"
-
-static const float pole_pairs = 12.0f;
-static float erotor_angle = 0.0f;
-static const float angle_step_per_ms = 0.36f;
-
-static int normalize_angle(float *angle)
-{
-    if(!angle)
-        return -EINVAL;
-
-    float result =  fmod(*angle, (2.0 * 3.141569f));
-    if(result > 3.141569f) {
-        result -= 3.141569f;
-    }
-
-    *angle = result;
-
-    return 0;
-}
-
-static int reset(struct esp_foc_motor_interface *self)
-{
-    if(!self)
-        return -EINVAL;
-
-    return 0;
-}
-
-static int enable(struct esp_foc_motor_interface *self)
-{
-    if(!self)
-        return -EINVAL;
-
-    return 0;
-}
-
-static int set_duty_cycles(struct esp_foc_motor_interface *self, float a, float b, float c)
-{
-    if(!self)
-        return -EINVAL;
-
-    (void)a;
-    (void)b;
-    (void)c;
-
-    return 0;
-}
-
-static int get_rotor_angle(struct esp_foc_motor_interface *self, float *angle)
-{
-    if(!self || !angle)
-        return -EINVAL;
-
-    *angle = erotor_angle * pole_pairs;
-
-    return normalize_angle(angle);
-}
-
-static int get_rotor_speed_dps(struct esp_foc_motor_interface *self, float *speed_dps)
-{
-    if(!self || speed_dps)
-        return -EINVAL;
-
-
-    //360 dps -> 1 RPS -> 1 RPM;
-    *speed_dps = 360.0f;
-
-    return 0;
-}
-
-static int align_rotor(struct esp_foc_motor_interface *self)
-{
-    if(!self)
-        return -EINVAL;
-
-    return 0;
-}
-
-static int fetch_sensors(struct esp_foc_motor_interface *self)
-{
-    if(!self)
-        return -EINVAL;
-
-    erotor_angle += angle_step_per_ms;
-    if(e_angle > 360.0f) {
-        erotor_angle -= 360.0f;
-    }
-
-    return 0;
-}
-
-int motor_hardware_stub_init(struct motor_hardware_stub *stub,
-                            struct esp_foc_motor_interface **itf)
-{
-    if(!stub)
-        return -EINVAL;
-
-    stub->self->reset = reset;
-    stub->self->enable = enable;
-    stub->self->set_duty_cycles = set_duty_cycles;
-    stub->self->get_rotor_angle = get_rotor_angle;
-    stub->self->get_rotor_speed_dps = get_rotor_speed_dps;
-    stub->self->align_rotor = align_rotor;
-    stub->self->fetch_sensors = fetch_sensors;
-
-    itf = stub->self;
-    return 0;
-}
\ No newline at end of file