First

2026-08-08 15:51:33 +03:00
commit 335f2cc9fb
35 changed files with 75208 additions and 0 deletions
@@ -0,0 +1,3 @@
+build
+tmp
+boards
@@ -0,0 +1,24 @@
+{
+	"version": "0.2.0",
+	"configurations": [
+		{
+			"name": "Debug with pyOCD (RP2350)",
+			"type": "cortex-debug",
+			"request": "launch",
+			"servertype": "pyocd",
+		    "serverpath": "${workspaceFolder}/../../../zephyr/venv/bin/pyocd",
+			"cwd": "${workspaceFolder}",
+			"executable": "${workspaceFolder}/build/zephyr/zephyr.elf",
+			"interface": "swd",
+			"runToEntryPoint": "main",
+			// "preLaunchTask": "build",
+			"armToolchainPath": "/usr/bin",
+			"gdbPath": "/usr/bin/arm-none-eabi-gdb",
+			"serverArgs": [
+				"--target=rp2350",
+				"--core=0"
+			],
+			"showDevDebugOutput": "raw"
+		}
+	]
+}
@@ -0,0 +1,10 @@
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.20.0)
+set(BOARD_ROOT ${CMAKE_CURRENT_SOURCE_DIR})
+
+find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
+project(firmware)
+
+FILE(GLOB app_sources src/*.c)
+target_sources(app PRIVATE ${app_sources})
@@ -0,0 +1,60 @@
+# Copyright (c) 2023 Nordic Semiconductor ASA
+# SPDX-License-Identifier: Apache-2.0
+
+source "Kconfig.zephyr"
+
+menu "USB options"
+	depends on USB_DEVICE_STACK_NEXT
+
+config USBD_MANUFACTURER
+	string "USB device manufacturer string"
+	default "Zephyr Project"
+	help
+	  USB device manufacturer string.
+
+config USBD_PRODUCT
+	string "USB device product string"
+	default "USBD"
+	help
+	  USB device product stringa.
+
+config USBD_VID
+	hex "USB device Vendor ID"
+	default 0x2fe3
+	help
+	  USB device Vendor ID. The default id (0x2fe3) is associated to
+	  Zephyr Project, you must use your own VID and applications
+	  outside of Zephyr Project.
+
+config USBD_PID
+	hex "USB device Product ID"
+	default 0x0001
+	help
+	  USB device Product ID. You must use your own PID
+	  and applications outside of Zephyr Project.
+
+config USBD_SELF_POWERED
+	bool "USB device Self-powered attribute"
+	default y
+	help
+	  Set the Self-powered attribute in the configuration.
+
+config USBD_REMOTE_WAKEUP
+	bool "USB device Remote Wakeup attribute"
+	help
+	  Set the Remote Wakeup attribute in the configuration.
+
+config USBD_MAX_POWER
+	int "USB device bMaxPower value"
+	default 125
+	range 0 250
+	help
+	  bMaxPower value in the configuration in 2 mA units.
+
+config USBD_20_EXTENSION_DESC
+	bool "Use default USB 2.0 Extension Descriptor"
+	depends on USBD_BOS_SUPPORT
+	help
+	  Set bcdUSB value to 0201 and use default USB 2.0 Extension Descriptor.
+
+endmenu
@@ -0,0 +1,50 @@
+# Zephyr Sand Table firmware
+
+This repository contains the implementation of Sand Table firmware for the Zephyr RTOS.
+
+The firmware controls 2x stepper motors and RGBW LED strip.
+
+### ACK
+
+Every command returns either an ACK or NACK.
+
+Homing, Step, and Polar commands return separate ACK when the arms are done moving.
+
+### Command Structure
+
+```c
+struct command_message_t {
+    uint8_t prefix;     // 0x69
+    uint8_t length;		// Length of the data
+    uint8_t id;			// 0x00
+    uint8_t command;
+    uint8_t crc;
+    uint8_t data[160];
+} __attribute__((packed));
+```
+
+### Commands
+
+```c
+typedef enum {
+	COMMAND_ACK,
+	COMMAND_NACK,
+	COMMAND_LED,
+	COMMAND_HOME,
+	COMMAND_DISABLE_MOTORS,
+	COMMAND_MOTOR_STEP,
+	COMMAND_MOTOR_SPEED,
+	COMMAND_POLAR,
+	COMMAND_GET_POLAR,
+	COMMAND_SET_OFFSET,
+	COMMAND_RESET_OFFSET,
+} commands_e;
+```
+
+### Python test scripts (AI generated)
+
+* `home.py` = sends homing command
+* `led.py` = sends led command which sends led fade track
+* `position.py` = sends position command and returns the current position
+* `reset_offset.py` = sends reset_offset command
+* `track.py {track name}` = reads a thetarho file and sends polar coordinates line by line
@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2021 Nordic Semiconductor ASA
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+/ {
+	chosen {
+		zephyr,console = &cdc_acm_uart0;
+		zephyr,shell-uart = &cdc_acm_uart0;
+	};
+
+	aliases {
+		led-r = &pwm_led_r;
+		led-g = &pwm_led_g;
+		led-b = &pwm_led_b;
+		led-w = &pwm_led_w;
+		motors-enable = &motors_enable;
+		m1-dir = &m1_dir;
+		m1-step = &m1_step;
+		m1-sensor = &m1_sensor;
+		m2-dir = &m2_dir;
+		m2-step = &m2_step;
+		m2-sensor = &m2_sensor;
+	};
+
+	leds {
+		compatible = "gpio-leds";
+		led_r: led_r {
+			gpios = <&gpio0 25 GPIO_ACTIVE_HIGH>;
+		};
+		led_g: led_g {
+			gpios = <&gpio0 27 GPIO_ACTIVE_HIGH>;
+		};
+		led_b: led_b {
+			gpios = <&gpio0 21 GPIO_ACTIVE_HIGH>;
+		};
+		led_w: led_w {
+			gpios = <&gpio0 17 GPIO_ACTIVE_HIGH>;
+		};
+	};
+
+	steppers {
+		compatible = "gpio-leds";
+		motors_enable: motors_enable {
+			gpios = <&gpio0 20 GPIO_ACTIVE_HIGH>;
+		};
+		m1_dir: m1_dir {
+			gpios = <&gpio0 18 GPIO_ACTIVE_HIGH>;
+		};
+		m1_step: m1_step {
+			gpios = <&gpio0 15 GPIO_ACTIVE_HIGH>;
+		};
+		m2_dir: m2_dir {
+			gpios = <&gpio0 19 GPIO_ACTIVE_HIGH>;
+		};
+		m2_step: m2_step {
+			gpios = <&gpio0 16 GPIO_ACTIVE_HIGH>;
+		};
+	};
+
+	sensors {
+		compatible = "gpio-keys";
+		polling-mode;
+		m1_sensor: m1_sensor {
+			gpios = <&gpio0 6 GPIO_ACTIVE_HIGH>;
+		};
+		m2_sensor: m2_sensor {
+			gpios = <&gpio0 4 GPIO_ACTIVE_HIGH>;
+		};
+	};
+
+	pwmleds {
+		compatible = "pwm-leds";
+		status = "okay";
+		pwm_led_r: pwm_led_r {
+			pwms = <&pwm 9 PWM_MSEC(1) PWM_POLARITY_NORMAL>;
+            label = "PWM LED R";
+		};
+		pwm_led_g: pwm_led_g {
+			pwms = <&pwm 11 PWM_MSEC(1) PWM_POLARITY_NORMAL>;
+            label = "PWM LED G";
+		};
+		pwm_led_b: pwm_led_b {
+			pwms = <&pwm 5 PWM_MSEC(1) PWM_POLARITY_NORMAL>;
+            label = "PWM LED B";
+		};
+		pwm_led_w: pwm_led_w {
+			pwms = <&pwm 1 PWM_MSEC(1) PWM_POLARITY_NORMAL>;
+            label = "PWM LED W";
+		};
+	};
+};
+
+&flash0 {
+	partitions {
+		code_partition: partition@100 {
+			compatible = "zephyr,mapped-partition";
+			reg = <0x100 (DT_SIZE_M(16) - 0x100 - 0x10000 - 0x1000)>;
+			read-only;
+		};
+
+		storage_partition: partition@fef000 {
+			label = "storage";
+			reg = <0xfef000 0x10000>;
+		};
+
+		settings_partition: partition@fff000 {
+			label = "settings";
+			reg = <0xfff000 0x1000>;
+		};
+	};
+};
+
+&zephyr_udc0 {
+	cdc_acm_uart0: cdc_acm_uart0 {
+		compatible = "zephyr,cdc-acm-uart";
+	};
+};
+
+&pinctrl {
+	pwm0_default: pwm0_default {
+		group1 {
+			pinmux = <PWM_4B_P25>, <PWM_5B_P27>, <PWM_2B_P21>, <PWM_0B_P17>;
+		};
+	};
+};
+
+&pwm {
+	status = "okay";
+	pinctrl-0 = <&pwm0_default>;
+	pinctrl-names = "default";
+};
@@ -0,0 +1,38 @@
+CONFIG_GPIO=y
+CONFIG_PWM=y
+CONFIG_INPUT=y
+CONFIG_INPUT_GPIO_KEYS=n
+
+# NVM
+CONFIG_FLASH=y
+CONFIG_NVS=y
+CONFIG_FLASH_MAP=y
+CONFIG_MPU_ALLOW_FLASH_WRITE=y
+
+# Serial
+CONFIG_SERIAL=y
+CONFIG_CONSOLE=y
+CONFIG_UART_CONSOLE=y
+CONFIG_STDOUT_CONSOLE=y
+CONFIG_UART_LINE_CTRL=y
+
+# USB
+CONFIG_USB_DEVICE_STACK_NEXT=y
+CONFIG_CDC_ACM_SERIAL_INITIALIZE_AT_BOOT=n
+CONFIG_USBD_VID=0xffff
+CONFIG_USBD_PID=0x0420
+CONFIG_USBD_MANUFACTURER="Stepper controller"
+CONFIG_USBD_PRODUCT="SandTable"
+CONFIG_USBD_SELF_POWERED=y
+CONFIG_USBD_MAX_POWER=125
+
+# LOG
+CONFIG_LOG=n
+CONFIG_USBD_CDC_ACM_LOG_LEVEL_OFF=y  # This removes a pointless warning
+CONFIG_LOG_DEFAULT_LEVEL=3
+CONFIG_LOG_MODE_IMMEDIATE=y
+
+# DEBUG
+# CONFIG_DEBUG_THREAD_INFO=y
+# CONFIG_DEBUG=y
+# CONFIG_DEBUG_OPTIMIZATIONS=y
@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+
+import struct
+import serial
+
+# -----------------------------------------------------------------------------
+# Configuration
+# -----------------------------------------------------------------------------
+
+PORT = "/dev/ttyACM1"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+COMMAND_ID = 0x00
+
+COMMAND_HOME = 3
+
+# Length
+length = 0
+
+# -----------------------------------------------------------------------------
+# Build packet with placeholder CRC
+# -----------------------------------------------------------------------------
+
+packet = bytearray([
+    COMMAND_PREFIX,
+    length,
+    COMMAND_ID,
+    COMMAND_HOME,
+    0x00,      # Placeholder CRC
+])
+
+# -----------------------------------------------------------------------------
+# Calculate CRC (matches command_calculate_crc())
+#
+# Sum every byte except the CRC byte itself.
+# -----------------------------------------------------------------------------
+
+crc_sum = 0
+
+for i, byte in enumerate(packet):
+    if i == 4:      # Skip CRC field
+        continue
+    crc_sum += byte
+
+packet[4] = 0x100 - (crc_sum & 0xFF)
+# Equivalent to:
+# packet[4] = (0x100 - (crc_sum & 0xFF)) & 0xFF
+
+# -----------------------------------------------------------------------------
+# Send
+# -----------------------------------------------------------------------------
+
+print(f"Packet ({len(packet)} bytes):")
+print(packet.hex(" "))
+
+with serial.Serial(PORT, BAUDRATE, timeout=1) as ser:
+    ser.write(packet)
+    ser.flush()
+
+print("Done.")
@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+
+import struct
+import serial
+
+# -----------------------------------------------------------------------------
+# Configuration
+# -----------------------------------------------------------------------------
+
+PORT = "/dev/ttyACM1"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+COMMAND_ID = 0x00
+
+COMMAND_LED = 2
+
+MAX_BRIGHTNESS = 255
+MIN_TRACK_SPEED = 1
+MAX_TRACK_SPEED = 1000
+
+# -----------------------------------------------------------------------------
+# LED Track
+# -----------------------------------------------------------------------------
+
+track = [
+    (MAX_BRIGHTNESS,   0,   0,   0),   # Red
+    (MAX_BRIGHTNESS, MAX_BRIGHTNESS,   0,   0),   # Yellow
+    (0,   MAX_BRIGHTNESS,   0,   0),   # Green
+    (0,   MAX_BRIGHTNESS, MAX_BRIGHTNESS,   0),   # Cyan
+    (0,     0, MAX_BRIGHTNESS,   0),   # Blue
+]
+
+# -----------------------------------------------------------------------------
+# Build payload
+# speed + length (amount of colors in track) + track (lines of colors in uint8_t)
+# -----------------------------------------------------------------------------
+
+payload = bytearray()
+
+# Track speed
+speed = 10
+payload.append((speed & 0xff))
+payload.append(((speed >> 8) & 0xff))
+
+# Number of colors
+payload.append(len(track))
+
+# Colors
+for color in track:
+    payload += struct.pack("BBBB", *color)
+
+# Length is:
+# payload
+length = len(payload)
+
+# -----------------------------------------------------------------------------
+# Build packet with placeholder CRC
+# -----------------------------------------------------------------------------
+
+packet = bytearray([
+    COMMAND_PREFIX,
+    length,
+    COMMAND_ID,
+    COMMAND_LED,
+    0x00,      # Placeholder CRC
+])
+
+packet += payload
+
+# -----------------------------------------------------------------------------
+# Calculate CRC (matches command_calculate_crc())
+#
+# Sum every byte except the CRC byte itself.
+# -----------------------------------------------------------------------------
+
+crc_sum = 0
+
+for i, byte in enumerate(packet):
+    if i == 4:      # Skip CRC field
+        continue
+    crc_sum += byte
+
+packet[4] = 0x100 - (crc_sum & 0xFF)
+# Equivalent to:
+# packet[4] = (0x100 - (crc_sum & 0xFF)) & 0xFF
+
+# -----------------------------------------------------------------------------
+# Send
+# -----------------------------------------------------------------------------
+
+print(f"Packet ({len(packet)} bytes):")
+print(packet.hex(" "))
+
+with serial.Serial(PORT, BAUDRATE, timeout=1) as ser:
+    ser.write(packet)
+    ser.flush()
+
+print("Done.")
@@ -0,0 +1,108 @@
+#!/usr/bin/env python3
+
+import struct
+import serial
+
+# -----------------------------------------------------------------------------
+# Configuration
+# -----------------------------------------------------------------------------
+
+PORT = "/dev/ttyACM1"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+COMMAND_ID = 0x00
+
+COMMAND_GET_POLAR = 8
+
+# -----------------------------------------------------------------------------
+# Build empty payload
+# -----------------------------------------------------------------------------
+
+payload = bytearray()
+length = len(payload)
+
+# -----------------------------------------------------------------------------
+# Build packet
+# -----------------------------------------------------------------------------
+
+packet = bytearray([
+    COMMAND_PREFIX,
+    length,
+    COMMAND_ID,
+    COMMAND_GET_POLAR,
+    0x00,      # CRC placeholder
+])
+
+packet += payload
+
+# -----------------------------------------------------------------------------
+# Calculate CRC
+# -----------------------------------------------------------------------------
+
+crc_sum = 0
+
+for i, byte in enumerate(packet):
+    if i == 4:
+        continue
+    crc_sum += byte
+
+packet[4] = (0x100 - (crc_sum & 0xFF)) & 0xFF
+
+print("Request:")
+print(packet.hex(" "))
+
+# -----------------------------------------------------------------------------
+# Send request and receive response
+# -----------------------------------------------------------------------------
+
+print("TX bytearray:")
+print(packet)
+
+print("TX hex:")
+print(packet.hex(" "))
+
+with serial.Serial(PORT, BAUDRATE, timeout=1) as ser:
+    ser.write(packet)
+    ser.flush()
+
+    # Read header
+    header = ser.read(5)
+
+    if len(header) != 5:
+        raise RuntimeError("Timeout waiting for response header")
+
+    prefix, length, cmd_id, command, crc = header
+
+    # Read payload
+    payload = ser.read(length)
+
+    if len(payload) != length:
+        raise RuntimeError("Timeout waiting for response payload")
+
+# Combine header + payload into a single bytearray
+response = bytearray(header)
+response.extend(payload)
+
+print("\nRX bytearray:")
+print(response)
+
+print("RX hex:")
+print(response.hex(" "))
+
+print(f"\nPrefix : 0x{prefix:02X}")
+print(f"Length : {length}")
+print(f"Cmd ID : {cmd_id}")
+print(f"Command: {command}")
+print(f"CRC    : 0x{crc:02X}")
+
+if command != COMMAND_GET_POLAR:
+    raise RuntimeError(f"Unexpected response command {command}")
+
+if length != 8:
+    raise RuntimeError(f"Expected 8-byte payload, got {length}")
+
+theta, r = struct.unpack("<ff", payload)
+
+print(f"\nTheta = {theta:.3f}")
+print(f"R     = {r:.3f}")
@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+
+import struct
+import serial
+
+# -----------------------------------------------------------------------------
+# Configuration
+# -----------------------------------------------------------------------------
+
+PORT = "/dev/ttyACM1"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+COMMAND_ID = 0x00
+
+COMMAND_RESET_OFFSET = 10
+
+# Length
+length = 0
+
+# -----------------------------------------------------------------------------
+# Build packet with placeholder CRC
+# -----------------------------------------------------------------------------
+
+packet = bytearray([
+    COMMAND_PREFIX,
+    length,
+    COMMAND_ID,
+    COMMAND_RESET_OFFSET,
+    0x00,      # Placeholder CRC
+])
+
+# -----------------------------------------------------------------------------
+# Calculate CRC (matches command_calculate_crc())
+#
+# Sum every byte except the CRC byte itself.
+# -----------------------------------------------------------------------------
+
+crc_sum = 0
+
+for i, byte in enumerate(packet):
+    if i == 4:      # Skip CRC field
+        continue
+    crc_sum += byte
+
+packet[4] = 0x100 - (crc_sum & 0xFF)
+# Equivalent to:
+# packet[4] = (0x100 - (crc_sum & 0xFF)) & 0xFF
+
+# -----------------------------------------------------------------------------
+# Send
+# -----------------------------------------------------------------------------
+
+print(f"Packet ({len(packet)} bytes):")
+print(packet.hex(" "))
+
+with serial.Serial(PORT, BAUDRATE, timeout=1) as ser:
+    ser.write(packet)
+    ser.flush()
+
+print("Done.")
@@ -0,0 +1,345 @@
+#!/usr/bin/env python3
+"""
+send_thetarho.py
+
+Reads a "thetarho" coordinate file (the format sandify exports, e.g.:
+
+    #
+    # File name: 'sandify'
+    # File type: thetarho
+    #
+
+    # BEGIN LAYER: 0 Polygon
+    0.00000 0.00000
+    1.57080 0.00800
+    ...
+
+) and streams each (theta, rho) point to the sand-table controller over
+serial as a COMMAND_POLAR message, matching the firmware's
+`struct command_message_t` / `struct polar_t` protocol.
+
+Usage examples:
+
+    # Just stream the file, waiting for the completion ACK after every point
+    ./send_thetarho.py pattern.thr
+
+    # Home first, then stream, don't wait for ACKs (fire-and-forget)
+    ./send_thetarho.py pattern.thr --home --no-wait-ack
+
+    # Different port / baud
+    ./send_thetarho.py pattern.thr -p /dev/ttyACM0 -b 115200
+"""
+
+import argparse
+import struct
+import sys
+import time
+
+import serial
+
+# -----------------------------------------------------------------------------
+# Protocol constants (must match firmware)
+# -----------------------------------------------------------------------------
+
+COMMAND_PREFIX = 0x69
+COMMAND_ID = 0x00
+
+COMMAND_DATA_SIZE = 160  # sizeof(msg->data) on the firmware side
+
+# commands_e
+COMMAND_ACK = 0
+COMMAND_NACK = 1
+COMMAND_LED = 2
+COMMAND_HOME = 3
+COMMAND_DISABLE_MOTORS = 4
+COMMAND_MOTOR_STEP = 5
+COMMAND_MOTOR_SPEED = 6
+COMMAND_POLAR = 7
+COMMAND_GET_POLAR = 8
+
+COMMAND_NAMES = {
+    COMMAND_ACK: "ACK",
+    COMMAND_NACK: "NACK",
+    COMMAND_LED: "LED",
+    COMMAND_HOME: "HOME",
+    COMMAND_DISABLE_MOTORS: "DISABLE_MOTORS",
+    COMMAND_MOTOR_STEP: "MOTOR_STEP",
+    COMMAND_MOTOR_SPEED: "MOTOR_SPEED",
+    COMMAND_POLAR: "POLAR",
+    COMMAND_GET_POLAR: "GET_POLAR",
+}
+
+HEADER_LEN = 5  # prefix, length, id, command, crc
+
+# struct polar_t { float theta; float r; };  -> two 32-bit floats
+POLAR_STRUCT = struct.Struct("<ff")
+
+
+# -----------------------------------------------------------------------------
+# Packet building / CRC (mirrors command_calculate_crc() on the firmware)
+# -----------------------------------------------------------------------------
+
+def build_packet(command: int, data: bytes = b"") -> bytearray:
+    """Build a command_message_t packet. `data` is only the bytes actually
+    used (the command does NOT need to fill the full 160-byte buffer)."""
+
+    if len(data) > COMMAND_DATA_SIZE:
+        raise ValueError(
+            f"data length {len(data)} exceeds COMMAND_DATA_SIZE ({COMMAND_DATA_SIZE})"
+        )
+
+    length = len(data)
+
+    packet = bytearray(
+        [
+            COMMAND_PREFIX,
+            length,
+            COMMAND_ID,
+            command,
+            0x00,  # crc placeholder
+        ]
+    )
+    packet += data
+
+    crc_sum = 0
+    for i, byte in enumerate(packet):
+        if i == 4:  # skip the crc field itself
+            continue
+        crc_sum += byte
+
+    packet[4] = (0x100 - (crc_sum & 0xFF)) & 0xFF
+
+    return packet
+
+
+def polar_packet(theta: float, r: float) -> bytearray:
+    return build_packet(COMMAND_POLAR, POLAR_STRUCT.pack(theta, r))
+
+
+# -----------------------------------------------------------------------------
+# Reading responses back from the device
+# -----------------------------------------------------------------------------
+
+def read_message(ser: serial.Serial, timeout: float = 2.0, debug: bool = False):
+    """Read one command_message_t from the serial port, resyncing on the
+    0x69 prefix byte. Returns a dict {prefix, length, id, command, crc, data}
+    or None on timeout."""
+
+    deadline = time.monotonic() + timeout
+    old_timeout = ser.timeout
+    discarded = bytearray()
+    raw_rx = bytearray()  # every byte read during this call, success or not
+
+    def flush_discarded():
+        if debug and discarded:
+            print(f"  [debug] discarded while resyncing: {bytes(discarded).hex(' ')}")
+            discarded.clear()
+
+    try:
+        while time.monotonic() < deadline:
+            ser.timeout = max(0.01, deadline - time.monotonic())
+            b = ser.read(1)
+            if not b:
+                continue
+            raw_rx += b
+            if debug:
+                print(f"  [debug] RX byte: {b.hex()}")
+            if b[0] != COMMAND_PREFIX:
+                discarded += b  # resync: keep looking for the prefix byte
+                continue
+
+            header = ser.read(4)  # length, id, command, crc
+            raw_rx += header
+            if debug and header:
+                print(f"  [debug] RX header: {header.hex(' ')}")
+            if len(header) < 4:
+                discarded += b + header
+                continue
+            length, msg_id, command, crc = header
+
+            data = b""
+            if length:
+                data = ser.read(length)
+                raw_rx += data
+                if debug:
+                    print(f"  [debug] RX data ({len(data)}/{length}): {data.hex(' ')}")
+                if len(data) < length:
+                    discarded += b + header + data
+                    continue  # malformed/short read, keep resyncing
+
+            flush_discarded()
+
+            msg = {
+                "prefix": b[0],
+                "length": length,
+                "id": msg_id,
+                "command": command,
+                "crc": crc,
+                "data": data,
+            }
+
+            if debug:
+                full = bytearray([b[0], length, msg_id, command, crc]) + bytearray(data)
+                name = COMMAND_NAMES.get(command, hex(command))
+                print(f"  [debug] recv {name} <- {bytes(full).hex(' ')}")
+
+            return msg
+    finally:
+        flush_discarded()
+        if debug and not raw_rx:
+            print("  [debug] RX: nothing received before timeout")
+        elif debug:
+            print(f"  [debug] RX total this call: {bytes(raw_rx).hex(' ')}")
+        ser.timeout = old_timeout
+
+    return None
+
+
+def wait_for_ack(ser: serial.Serial, expected_command: int, timeout: float = 10.0, debug: bool = False) -> bool:
+    """Wait for an ACK/NACK message whose data[0] echoes expected_command
+    (this is how the firmware signals a HOME/POLAR move has completed)."""
+
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+        remaining = deadline - time.monotonic()
+        msg = read_message(ser, timeout=remaining, debug=debug)
+        if msg is None:
+            return False
+
+        if msg["command"] == COMMAND_NACK:
+            print("  -> received NACK from device")
+            return False
+
+        if msg["command"] == COMMAND_ACK:
+            acked = msg["data"][0] if msg["data"] else None
+            if acked == expected_command:
+                return True
+            # ACK for something else (e.g. a stray immediate ACK) - keep waiting
+            continue
+
+    return False
+
+
+# -----------------------------------------------------------------------------
+# Thetarho file parsing
+# -----------------------------------------------------------------------------
+
+def parse_thetarho(path: str):
+    """Yield (theta, r) tuples from a sandify-style thetarho file, skipping
+    comments (#...) and blank lines."""
+
+    points = []
+    with open(path, "r") as f:
+        for lineno, raw_line in enumerate(f, start=1):
+            line = raw_line.strip()
+            if not line or line.startswith("#"):
+                continue
+
+            parts = line.split()
+            if len(parts) != 2:
+                print(f"warning: skipping malformed line {lineno}: {raw_line!r}", file=sys.stderr)
+                continue
+
+            try:
+                theta = float(parts[0])
+                r = float(parts[1])
+            except ValueError:
+                print(f"warning: skipping non-numeric line {lineno}: {raw_line!r}", file=sys.stderr)
+                continue
+
+            points.append((theta, r))
+
+    return points
+
+
+# -----------------------------------------------------------------------------
+# Main
+# -----------------------------------------------------------------------------
+
+def main():
+    parser = argparse.ArgumentParser(description="Stream a thetarho file to the sand table over serial.")
+    parser.add_argument("file", help="Path to the thetarho (.thr) file")
+    parser.add_argument("-p", "--port", default="/dev/ttyACM1", help="Serial port (default: %(default)s)")
+    parser.add_argument("-b", "--baud", type=int, default=115200, help="Baud rate (default: %(default)s)")
+    parser.add_argument("--home", action="store_true", help="Send a HOME command before streaming points")
+    parser.add_argument(
+        "--no-wait-ack",
+        dest="wait_ack",
+        action="store_false",
+        default=True,
+        help="Don't wait for the completion ACK between points (fire-and-forget)",
+    )
+    parser.add_argument(
+        "--ack-timeout",
+        type=float,
+        default=15.0,
+        help="Seconds to wait for a completion ACK before giving up on a point (default: %(default)s)",
+    )
+    parser.add_argument(
+        "--delay",
+        type=float,
+        default=0.0,
+        help="Extra delay in seconds between points when not waiting for ACKs",
+    )
+    parser.add_argument("-v", "--verbose", action="store_true", help="Print each packet sent")
+    parser.add_argument(
+        "--debug",
+        action="store_true",
+        help="Print every sent packet's bytearray, every parsed response, and any "
+        "stray bytes discarded while resyncing on the serial line. Implies --verbose.",
+    )
+    args = parser.parse_args()
+    if args.debug:
+        args.verbose = True
+
+    points = parse_thetarho(args.file)
+    if not points:
+        print(f"No valid points found in {args.file}", file=sys.stderr)
+        sys.exit(1)
+
+    print(f"Loaded {len(points)} points from {args.file}")
+
+    with serial.Serial(args.port, args.baud, timeout=1) as ser:
+        # Give the device a moment in case it resets on port open (common on
+        # AVR/USB-CDC boards).
+        time.sleep(2)
+        ser.reset_input_buffer()
+
+        if args.home:
+            print("Homing...")
+            packet = build_packet(COMMAND_HOME)
+            if args.verbose:
+                print("  ->", bytes(packet).hex(" "))
+            ser.write(packet)
+            ser.flush()
+            if args.wait_ack:
+                if not wait_for_ack(ser, COMMAND_HOME, timeout=args.ack_timeout, debug=args.debug):
+                    print("Timed out / failed waiting for HOME to complete", file=sys.stderr)
+                    sys.exit(1)
+            print("Homed.")
+
+        for i, (theta, r) in enumerate(points):
+            packet = polar_packet(theta, r)
+            if args.verbose:
+                print(f"[{i + 1}/{len(points)}] theta={theta:.5f} r={r:.5f} -> {bytes(packet).hex(' ')}")
+            else:
+                print(f"\r[{i + 1}/{len(points)}] theta={theta:.5f} r={r:.5f}", end="", flush=True)
+
+            ser.write(packet)
+            ser.flush()
+
+            if args.wait_ack:
+                if not wait_for_ack(ser, COMMAND_POLAR, timeout=args.ack_timeout, debug=args.debug):
+                    print(f"\nTimed out / failed waiting for point {i + 1} to complete", file=sys.stderr)
+                    sys.exit(1)
+            elif args.delay:
+                time.sleep(args.delay)
+
+        if not args.verbose:
+            print()
+
+    print("Done.")
+
+
+if __name__ == "__main__":
+    main()
@@ -0,0 +1,105 @@
+#include "angles.h"
+
+#include <math.h>
+#include <stdint.h>
+
+
+/*
+	Get angles for arm 2 from polar coordinates
+*/
+float arm_2_angle_from_polar(struct polar_t *coord) {
+	if (coord->r == 0) { return M_PI; }
+
+	float s1 = (float)pow(coord->r, 2) - 0.5f;
+	float q2 = (float)acos(s1 / 0.5f);
+
+	return q2;
+}
+
+/*
+	Get angles for arm 1 from polar coordinates
+*/
+float arm_1_angle_from_polar(float theta2, struct polar_t *coord, bool inverted, float theta1_old) {
+	if (coord->r == 0.0f) { return theta1_old; }
+
+	float s2 = 0.5f + (0.5f * (float)cos(theta2));
+	if (s2 == 0.0f) { s2 = 0.00001f; }
+
+	float s1 = 0.5f * (float)sin(theta2);
+	float alpha = (float)atan(s1 / s2);
+	float q1 = 0.0f;
+
+    if(!inverted) {
+        q1 = coord->theta - alpha;
+    }
+    else {
+        q1 = coord->theta + alpha;
+    }
+
+	return q1;
+}
+
+/*
+	Calculating change of angles
+*/
+float delta_angles(float theta, float theta_old) {
+	float delta_theta = theta - theta_old;
+
+	while (delta_theta >= 5) { delta_theta -= M_PI2; }
+	while (delta_theta <= -5) { delta_theta += M_PI2; }
+
+	return delta_theta;
+}
+
+/*
+	Calculating steps from delta angles
+*/
+int steps(float theta, int microstepping) {
+	if (theta == 0.0f) { return 0; }
+
+	uint32_t steps_per_revolution = 600 * microstepping;
+	float s1 = ((float)steps_per_revolution / (M_PI2));
+	
+	return round(s1 * theta);
+}
+
+/*
+	Calculating new arm angle from steps taken
+*/
+float angle_from_steps(int step, int microstepping) {
+	if (step == 0) { return 0.0f; }
+
+	uint32_t steps_per_revolution = 600 * microstepping;
+	float s1 = (float)steps_per_revolution / M_PI2;
+	float theta = (float)step / s1;
+
+	return theta;
+}
+
+/*
+	Calculating polar coordinates from arm angles
+*/
+void polar_from_arms(struct arm_angles_t *angles, struct polar_t *coord) {
+	if (angles->arm2 == M_PI) {
+		coord->theta = angles->arm1;
+		coord->r = 0;
+		return;
+	}
+
+	float x = (0.5f * (float)cos(angles->arm1)) + 
+						(0.5f * (float)cos(angles->arm1 + angles->arm2));
+	float y = (0.5f * (float)sin(angles->arm1)) + 
+						(0.5f * (float)sin(angles->arm1 + angles->arm2));
+
+	if (x == 0 && y == 0) {
+		coord->theta = angles->arm1;
+		coord->r = 0.0f;
+		return;
+	}
+	// Theta
+	coord->theta = (float)atan2(y, x);
+
+	// R
+	float r2 = (float)pow(x, 2) + (float)pow(y, 2);
+	coord->r = (float)sqrt(r2);
+}
@@ -0,0 +1,36 @@
+#ifndef ANGLES_H
+#define ANGLES_H
+
+
+#include <stdbool.h>
+
+
+#define M_PI (float)3.1415926536
+#define M_PI2 (float)6.2831853072
+
+
+struct polar_t {
+	float theta;
+	float r;
+};
+
+struct cartesian_t {
+	float x;
+	float y;
+};
+
+struct arm_angles_t {
+	float arm1;
+	float arm2;
+};
+
+
+float arm_2_angle_from_polar(struct polar_t *coord);
+float arm_1_angle_from_polar(float theta2, struct polar_t *coord, bool inverted, float theta1_old);
+float delta_angles(float theta, float theta_old);
+int steps(float theta, int microstepping);
+float angle_from_steps(int step, int microstepping);
+void polar_from_arms(struct arm_angles_t *angles, struct polar_t *coord);
+
+
+#endif // ANGLES_H
@@ -0,0 +1,111 @@
+#include "command_handler.h"
+#include "led.h"
+#include "stepper.h"
+#include "angles.h"
+#include "usb.h"
+#include "settings.h"
+
+#include <zephyr/logging/log.h>
+
+#include <string.h>
+
+LOG_MODULE_REGISTER(command_handler, LOG_LEVEL_INF);
+
+int command_handler(struct command_message_t *msg) {
+	if (msg == NULL) {
+		LOG_ERR("Received NULL message pointer");
+		return -EINVAL;
+	}
+
+	LOG_DBG("Processing command: %d, length: %d", msg->command, msg->length);
+
+	switch (msg->command) {
+		case COMMAND_LED: {
+			if (msg->length <= 3) { return -EINVAL; }
+			uint16_t speed = (msg->data[1] << 8) | msg->data[0];
+			uint8_t length = msg->data[2];
+
+			if (msg->length != ((sizeof(struct color_t) * length) + 3)) { return -EINVAL; }
+
+			struct color_t track[16];
+
+			if (length > 16) { length = 16; }
+			memcpy(track, &msg->data[3], (sizeof(struct color_t) * length));
+
+			led_set_current_track(speed, length, track);
+			
+			break;
+		}
+
+		case COMMAND_HOME: {
+			stepper_home_motors();
+			break;
+		}
+
+		case COMMAND_DISABLE_MOTORS: {
+			stepper_disable_motors();
+			break;
+		}
+
+		case COMMAND_MOTOR_STEP: {
+			if (msg->length != 4) { return -EINVAL; }
+			int m1_steps = (msg->data[1] << 8) | msg->data[0];
+			int m2_steps = (msg->data[3] << 8) | msg->data[2];
+
+			stepper_add_steps(m1_steps, m2_steps);
+			break;
+		}
+
+		case COMMAND_MOTOR_SPEED: {
+			if (msg->length != 2) { return -EINVAL; }
+			uint16_t speed = (msg->data[1] << 8) | msg->data[0];
+			stepper_set_speed(speed);
+			break;
+		}
+
+		case COMMAND_POLAR: {
+			if (msg->length != sizeof(struct polar_t)) { return -EINVAL; }
+			struct polar_t position;
+			memcpy(&position, &msg->data[0], sizeof(struct polar_t));
+			stepper_set_position(&position);
+			break;
+		}
+
+		case COMMAND_GET_POLAR: {
+			struct polar_t position;
+			stepper_get_position(&position);
+
+			struct command_message_t response;
+			command_create_message(&response, (uint8_t)sizeof(struct polar_t), COMMAND_GET_POLAR, (uint8_t *)&position);
+
+			// Respond the position
+			usb_send_command(&response);
+
+			return -1;
+		}
+
+		case COMMAND_SET_OFFSET: {
+			if (msg->length != 2) { return -EINVAL; }
+
+			int m1 = (int8_t)msg->data[0];
+			int m2 = (int8_t)msg->data[1];
+
+			stepper_add_offset(m1, m2);
+
+			break;
+		}
+
+		case COMMAND_RESET_OFFSET: {
+			stepper_reset_offset();
+
+			break;
+		}
+
+		default: {
+			LOG_WRN("Unknown command received: %d", msg->command);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
@@ -0,0 +1,16 @@
+#ifndef COMMAND_HANDLER_H
+#define COMMAND_HANDLER_H
+
+
+#include "command_message.h"
+
+/**
+ * @brief Process received command message
+ * 
+ * @param msg Command message to process
+ * @return 0 on success, negative errno on failure
+ */
+int command_handler(struct command_message_t *msg);
+
+
+#endif // COMMAND_HANDLER_H
@@ -0,0 +1,75 @@
+#include "command_message.h"
+
+#include <string.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(command_message, LOG_LEVEL_INF);
+
+void command_message_init(struct command_message_t *msg) {
+	memset(msg, 0, sizeof(struct command_message_t));
+	msg->prefix = COMMAND_PREFIX;
+	msg->id = COMMAND_ID;
+}
+
+void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t *data) {
+	// Ensure length doesn't exceed available space
+	if (length > sizeof(msg->data) - 1) {
+		return;
+	}
+
+	command_message_init(msg);
+	msg->length = length;
+	msg->command = command;
+
+	// Copy the data
+	if (data != NULL) {
+		for (int i = 0; i < msg->length; i++) {
+			msg->data[i] = data[i];
+		}
+	}
+	msg->crc = command_calculate_crc(msg);
+}
+
+uint8_t command_calculate_crc(struct command_message_t *msg) {
+	uint32_t sum = 0;
+	uint8_t crc = 0;
+
+	uint8_t *byte_ptr = (uint8_t *)msg;
+	int loop_length = (sizeof(struct command_message_t) - sizeof(msg->data) + msg->length);
+
+	for (int i = 0; i < loop_length; i++) {
+		if (i == 4) { continue; }
+		sum += byte_ptr[i];
+	}
+
+	crc = 0x100 - (sum & 0xff);
+
+	return crc;
+}
+
+void command_create_ack(struct command_message_t *msg) {
+	command_create_message(msg, 0, COMMAND_ACK, NULL);
+}
+
+void command_create_nack(struct command_message_t *msg) {
+	command_create_message(msg, 0, COMMAND_NACK, NULL);
+}
+
+void command_log(struct command_message_t *msg) {
+	if (msg->length > sizeof(msg->data) - 1) {
+		LOG_ERR("Message length too long: %d", msg->length);
+		return;
+	}
+
+	LOG_INF("Prefix: %d\n\r", msg->prefix);
+	LOG_INF("Length: %d\n\r", msg->length);
+	LOG_INF("COMMAND_ID: %d\n\r", msg->id);
+	LOG_INF("Command: %d\n\r", msg->command);
+	LOG_INF("Data:\n\r");
+
+	for (int i = 0; i < msg->length; i++) {
+		LOG_INF("%d", msg->data[i]);
+	}
+
+	LOG_INF("CRC: %d\n\r", msg->crc);
+}
@@ -0,0 +1,82 @@
+#ifndef COMMAND_MESSAGE_H
+#define COMMAND_MESSAGE_H
+
+
+#include <stdint.h>
+
+
+#define COMMAND_PREFIX 0x69
+#define COMMAND_ID 0x00
+#define COMMAND_DATA_SIZE 160
+
+typedef enum {
+	COMMAND_ACK,
+	COMMAND_NACK,
+	COMMAND_LED,
+	COMMAND_HOME,
+	COMMAND_DISABLE_MOTORS,
+	COMMAND_MOTOR_STEP,
+	COMMAND_MOTOR_SPEED,
+	COMMAND_POLAR,
+	COMMAND_GET_POLAR,
+	COMMAND_SET_OFFSET,
+	COMMAND_RESET_OFFSET,
+} commands_e;
+
+struct command_message_t {
+	uint8_t prefix;
+	uint8_t length;
+	uint8_t id;
+	uint8_t command;
+	uint8_t crc;
+	uint8_t data[COMMAND_DATA_SIZE];
+} __attribute__((packed));
+
+/**
+ * @brief Initialize command message to default state
+ * 
+ * @param msg Message to initialize
+ */
+void command_message_init(struct command_message_t *msg);
+
+/**
+ * @brief Create command message with data and CRC
+ * 
+ * @param msg Message to populate
+ * @param length Data length in bytes
+ * @param command Command type
+ * @param data Data payload
+ */
+void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t *data);
+
+/**
+ * @brief Calculate CRC for command message
+ * 
+ * @param msg Message to calculate CRC for
+ * @return CRC value
+ */
+uint8_t command_calculate_crc(struct command_message_t *msg);
+
+/**
+ * @brief Create ACK command message
+ * 
+ * @param msg Message to populate
+ */
+void command_create_ack(struct command_message_t *msg);
+
+/**
+ * @brief Create NACK command message
+ * 
+ * @param msg Message to populate
+ */
+void command_create_nack(struct command_message_t *msg);
+
+/**
+ * @brief Print the command with LOG
+ * 
+ * @param msg Message to calculate CRC for
+ */
+void command_log(struct command_message_t *msg);
+
+
+#endif // COMMAND_MESSAGE_H
@@ -0,0 +1,185 @@
+#include "led.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/pwm.h>
+#include <zephyr/drivers/gpio.h>
+
+#include <string.h>
+
+
+LOG_MODULE_REGISTER(led, LOG_LEVEL_INF);
+
+// DEVICE
+static const struct pwm_dt_spec ledr = PWM_DT_SPEC_GET(DT_ALIAS(led_r));
+static const struct pwm_dt_spec ledg = PWM_DT_SPEC_GET(DT_ALIAS(led_g));
+static const struct pwm_dt_spec ledb = PWM_DT_SPEC_GET(DT_ALIAS(led_b));
+static const struct pwm_dt_spec ledw = PWM_DT_SPEC_GET(DT_ALIAS(led_w));
+
+// THREAD
+static struct k_thread led_thread_data;
+static k_tid_t led_thread_id = NULL;
+#define LED_THREAD_STACK_SIZE 2048
+K_THREAD_STACK_DEFINE(led_thread_stack, LED_THREAD_STACK_SIZE);
+
+// COLORS
+#define MAX_BRIGHTNESS 255
+#define TRACK_BUFFER_SIZE 16
+struct color_t current_color = {0, 0, 0, 0};
+struct led_track_t current_track;
+struct color_t current_track_buffer[TRACK_BUFFER_SIZE];
+struct color_t default_track[] = {
+	{(MAX_BRIGHTNESS / 2), (MAX_BRIGHTNESS / 4), 0, 0},
+	{(MAX_BRIGHTNESS / 2), (MAX_BRIGHTNESS / 2), 0, 0},
+};
+
+// SPEED
+#define MIN_TRACK_SPEED 1
+#define MAX_TRACK_SPEED 1000
+
+
+static void led_set_pwm(const struct pwm_dt_spec *dev, uint32_t value, uint32_t max_brightness) {
+	if (value <= max_brightness && max_brightness > 0) {
+		uint32_t pulse_width_ns = value * (dev->period / max_brightness);
+		pwm_set_pulse_dt(dev, pulse_width_ns);
+	}
+}
+
+static void led_set_rgbw(uint8_t r, uint8_t g, uint8_t b, uint8_t w) {
+	led_set_pwm(&ledr, r, MAX_BRIGHTNESS);
+	led_set_pwm(&ledg, g, MAX_BRIGHTNESS);
+	led_set_pwm(&ledb, b, MAX_BRIGHTNESS);
+	led_set_pwm(&ledw, w, MAX_BRIGHTNESS);
+}
+
+static void led_calculate_color_change(float color_change[4], struct color_t *last_color, struct color_t *set_color) {
+	float r = ((float)set_color->r - (float)last_color->r) / (float)MAX_BRIGHTNESS;
+	float g = ((float)set_color->g - (float)last_color->g) / (float)MAX_BRIGHTNESS;
+	float b = ((float)set_color->b - (float)last_color->b) / (float)MAX_BRIGHTNESS;
+	float w = ((float)set_color->w - (float)last_color->w) / (float)MAX_BRIGHTNESS;
+
+	color_change[0] = r;
+	color_change[1] = g;
+	color_change[2] = b;
+	color_change[3] = w;
+}
+
+static bool led_color_match(struct color_t *first_color, struct color_t *second_color) {
+	bool check = 	(first_color->r == second_color->r) &&
+					(first_color->g == second_color->g) &&
+					(first_color->b == second_color->b) &&
+					(first_color->w == second_color->w);
+	
+	return check;
+}
+
+void led_set_current_track(uint16_t speed, uint8_t length, struct color_t *colors) {
+	if (speed < MIN_TRACK_SPEED) { speed = MIN_TRACK_SPEED; }
+	else if (speed > MAX_TRACK_SPEED) { speed = MAX_TRACK_SPEED; }
+	current_track.speed = speed;
+	
+	if (length > TRACK_BUFFER_SIZE) { length = TRACK_BUFFER_SIZE; }
+	current_track.length = length;
+
+	memcpy(current_track.colors, colors, (length * sizeof(struct color_t)));
+}
+
+static void led_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+	
+	LOG_INF("LED thread started");
+	
+	struct color_t last_color;
+	struct color_t set_color;
+	memset(&last_color, 0, sizeof(struct color_t));
+	memset(&set_color, 0, sizeof(struct color_t));
+
+	while (1) {
+		if (current_track.length == 0) {
+			k_msleep(500);
+			continue;
+		}
+
+		if (current_track.length == 1 && led_color_match(&current_color, &current_track.colors[0])) {
+			k_msleep(500);
+		}
+		else {
+			for (int i = 0; i < current_track.length; i++) {
+				memcpy(&set_color, &current_track.colors[i], sizeof(struct color_t));
+
+				// Calculate color steps
+				float current_color_float[4] = {(float)last_color.r, (float)last_color.g, (float)last_color.b, (float)last_color.w};
+				float color_change[4];
+				led_calculate_color_change(color_change, &last_color, &set_color);
+
+				for (int j = 0; j < MAX_BRIGHTNESS; j++) {
+					current_color.r = (uint8_t)current_color_float[0];
+					current_color.g = (uint8_t)current_color_float[1];
+					current_color.b = (uint8_t)current_color_float[2];
+					current_color.w = (uint8_t)current_color_float[3];
+					led_set_rgbw(current_color.r, current_color.g, current_color.b, current_color.w);
+
+					current_color_float[0] += color_change[0];
+					current_color_float[1] += color_change[1];
+					current_color_float[2] += color_change[2];
+					current_color_float[3] += color_change[3];
+					k_msleep(current_track.speed);
+				}
+
+				memcpy(&last_color, &set_color, sizeof(struct color_t));
+				led_set_rgbw(set_color.r, set_color.g, set_color.b, set_color.w);
+			}
+		}
+	}
+	
+	LOG_INF("LED thread exiting");
+}
+
+int led_init() {
+	memset(current_track_buffer, 0, sizeof(current_track_buffer));
+	current_track.speed = 10;
+	current_track.length = 0;
+	current_track.colors = current_track_buffer;
+
+	// Set orange/white fade on boot
+	led_set_current_track(20, 2, default_track);
+
+	// LED R
+	if (!pwm_is_ready_dt(&ledr)) {
+		LOG_ERR("PWM device %s is not ready", ledr.dev->name);
+	}
+
+	// LED G
+	if (!pwm_is_ready_dt(&ledg)) {
+		LOG_ERR("PWM device %s is not ready", ledg.dev->name);
+	}
+
+	// LED B
+	if (!pwm_is_ready_dt(&ledb)) {
+		LOG_ERR("PWM device %s is not ready", ledb.dev->name);
+	}
+
+	// LED W
+	if (!pwm_is_ready_dt(&ledw)) {
+		LOG_ERR("PWM device %s is not ready", ledw.dev->name);
+	}
+
+	led_thread_id = k_thread_create(
+		&led_thread_data,
+		led_thread_stack,
+		K_THREAD_STACK_SIZEOF(led_thread_stack),
+		led_thread,
+		NULL, NULL, NULL,
+		5,
+		0,
+		K_NO_WAIT
+	);
+
+	if (led_thread_id == NULL) {
+		LOG_ERR("Failed to create LED thread");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
@@ -0,0 +1,26 @@
+#ifndef LED_H
+#define LED_H
+
+
+#include <stdint.h>
+
+
+struct color_t {
+	uint8_t r;
+	uint8_t g;
+	uint8_t b;
+	uint8_t w;
+};
+
+struct led_track_t {
+	uint16_t speed;
+	uint8_t length;
+	struct color_t *colors;
+};
+
+
+int led_init();
+void led_set_current_track(uint16_t speed, uint8_t length, struct color_t *colors);
+
+
+#endif // LED_H
@@ -0,0 +1,41 @@
+#include "led.h"
+#include "usb.h"
+#include "stepper.h"
+#include "nvs.h"
+#include "settings.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/kernel.h>
+LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
+
+
+int main(void) {
+	int ret;
+
+	ret = settings_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable SETTINGS");
+	}
+
+	ret = nvs_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable NVS");
+	}
+
+	ret = usb_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable USB");
+	}
+
+	ret = led_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable LED");
+	}
+
+	ret = stepper_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable STEPPER");
+	}
+
+	return 0;
+}
@@ -0,0 +1,560 @@
+// Pico-SDK specific libraries
+#include <pico/stdlib.h>
+#include <pico/multicore.h>
+#include <pico/time.h>
+#include "hardware/gpio.h"
+#include "hardware/pwm.h"
+
+// C libraries
+#include <stdio.h>
+#include <cmath>
+
+// Local libraries
+#include "leds.hpp"
+#include "driver.hpp"
+#include "angles.hpp"
+
+
+// Communication
+#define BUFFER_LENGTH 10
+/*
+	Commands:
+	1st bit = command
+	2nd bit = data size
+	rest n bits = data
+*/
+#define COMMAND_STOP 				0x00		// Returns: Nothing
+#define COMMAND_ANGLE 				0x01		// Returns: Nothing
+#define COMMAND_R					0x02		// Returns: Actual angle and r (floats as char*)
+#define COMMAND_X_COORDINATE		0x03		// Returns: Nothing
+#define COMMAND_Y_COORDINATE		0x04		// Returns: ---
+#define COMMAND_HOME				0x05		// Returns: Nothing
+#define COMMAND_UPDATE_ANGLE		0x06		// Returns: Nothing
+#define COMMAND_UPDATE_R			0x07		// Returns: Nothing
+#define COMMAND_M1_STEP				0x08		// Returns: Actual angle and r (floats as char*)
+#define COMMAND_M2_STEP				0x09		// Returns: Actual angle and r (floats as char*)
+#define COMMAND_SET_MOTOR_SPEED		0x0a		// Returns: Nothing
+#define COMMAND_SET_LED_TRACK		0x0b		// Returns: Nothing
+#define COMMAND_SET_LED_SPEED		0x0c		// Returns: Nothing
+#define COMMAND_SET_LED_INTENSITY	0x0d		// Returns: Nothing
+#define COMMAND_SET_LED_SATURATION	0x0e		// Returns: Nothing
+#define COMMAND_SET_LED_R			0x0f		// Returns: Nothing
+#define COMMAND_SET_LED_G			0x10		// Returns: Nothing
+#define COMMAND_SET_LED_B			0x11		// Returns: Nothing
+#define COMMAND_SET_LED_W			0x12		// Returns: Nothing
+#define COMMAND_GET_ANGLE			0x13		// Returns: Angle (float as char*)
+#define COMMAND_GET_R				0x14		// Returns: R (float as char*)
+#define COMMAND_RESET				0x15		// Returns: Nothing
+
+
+// LED PINS
+#define R_PIN 25		// PWM4 B - D2
+#define G_PIN 7			// PWM3 B - D10
+#define B_PIN 21		// PWM2 B - D9
+#define W_PIN 17		// PWM0 B - D5
+
+led leds(R_PIN, G_PIN, B_PIN, W_PIN);
+
+
+// Motors
+#define MOTORS_ENABLE 20	// D8
+#define M1_DIR 18			// D6
+#define M1_STEP 15			// D3
+#define M1_SENSOR 6 		// D13
+#define M2_DIR 19			// D7
+#define M2_STEP 16			// D4
+#define M2_SENSOR 4 		// D12
+
+motor motor1(M1_DIR, M1_STEP, MOTORS_ENABLE, M1_SENSOR);
+motor motor2(M2_DIR, M2_STEP, MOTORS_ENABLE, M2_SENSOR);
+
+
+// LED settings
+int led_speed = 50000;
+absolute_time_t  led_time = get_absolute_time();
+float led_intensity = 0.4;
+float led_saturation = 0.0;
+/*
+	LED tracks:
+	0 = static color
+	1 = red pulse
+	2 = green pulse
+	3 = blue pulse
+	4 = white pulse
+	5 = RGB fade
+*/
+int led_track = 0;
+
+
+// Motor settings
+int spr = 600;
+int microstepping = 16;
+float theta1_old = 0;
+float theta2_old = M_PI;
+float angle_old = 0;
+float r_old = 0;
+bool inverted = false;
+int draw_speed = 1;
+
+
+/*
+	Second core function
+*/
+void second_core() {
+	while (1) {
+		float angle = 0.0;
+		float r = 0.0;
+		int fifo = 0;
+		
+		fifo = multicore_fifo_pop_blocking();
+		motor1.Enable();
+
+		// Home command
+		if (fifo == COMMAND_HOME) {
+			home(motor1, motor2);
+			theta1_old = 0.0;
+			theta2_old = M_PI;
+			angle_old = 0;
+			r_old = 0;
+			multicore_fifo_push_blocking(0);
+			continue;
+		}
+
+		// M1 steps
+		else if (fifo == COMMAND_M1_STEP) {
+			int steps = multicore_fifo_pop_blocking();
+			motor1.setDirection(steps);
+			motor1.Step(abs(steps), 5);
+			multicore_fifo_push_blocking(0);
+			continue;
+		}
+
+		// M2 steps
+		else if (fifo == COMMAND_M2_STEP) {
+			int steps = multicore_fifo_pop_blocking();
+			motor2.setDirection(steps);
+			motor2.Step(abs(steps), 5);
+			multicore_fifo_push_blocking(0);
+			continue;
+		}
+
+		// Theta nd R
+		else {
+			fifo = multicore_fifo_pop_blocking();
+			angle = *reinterpret_cast<float*>(&fifo);
+			fifo = multicore_fifo_pop_blocking();
+			r = *reinterpret_cast<float*>(&fifo);
+		}
+
+		// If r is zero ignore angle
+		if (r == 0) {
+			angle = angle_old;
+		}
+		// If r is too much - ignore
+		if (r > 1.0) {
+			multicore_fifo_push_blocking(1);
+			continue;
+		}
+		// If delta r is too much - ignore
+		else if (abs(r - r_old) > 0.05) {
+			multicore_fifo_push_blocking(1);
+			continue;
+		}
+
+		// Getting angles for the arms
+		float theta2 = polarGetTheta2(angle, r);
+		float theta1 = polarGetTheta1(theta2, angle, r, inverted, theta1_old);
+
+		// Getting the change of angles
+		float delta_theta1 = deltaAngles(theta1, theta1_old);
+		float delta_theta2 = deltaAngles(theta2, theta2_old);
+
+		// Getting the steps needed
+		int step1 = steps(delta_theta1, microstepping);
+		int step2 = steps(delta_theta2, microstepping);
+
+		// Accounting the arm2 spin
+		step2 += step1;
+
+		// Setting the direction of the motors
+		motor1.setDirection(step1);
+		motor2.setDirection(step2);
+
+		// Moving the motors
+		if ((abs(step1) > abs(step2)) && (step2 != 0)) {
+			dualSteps(abs(step1), motor1, abs(step2), motor2, draw_speed);
+		}
+		else if ((abs(step1) < abs(step2)) && (step1 != 0)) {
+			dualSteps(abs(step2), motor2, abs(step1), motor1, draw_speed);
+		}
+		else if (abs(step1) == abs(step2)) {
+			equalSteps(abs(step1), motor2, motor1, draw_speed);
+		}
+		else if (step2 == 0 && step1 != 0) {
+			motor1.Step(abs(step1), draw_speed);
+		}
+		else if (step1 == 0 && step2 != 0) {
+			motor2.Step(abs(step2), draw_speed);
+		}
+
+		// Get arm angles from the steps
+		theta1_old += thetaFromSteps(step1, microstepping);
+		theta2_old += thetaFromSteps(step2 - step1, microstepping);
+
+		// Check if angles are over 2PI
+		if (theta1_old >= (2*M_PI)) {
+			theta1_old -= (2*M_PI);
+		}
+		else if (theta1_old <= -(2*M_PI)) {
+			theta1_old += (2*M_PI);
+		}
+
+
+		angle_old = thetaFromArms(theta1_old, theta2_old);
+		r_old = rFromArms(theta1_old, theta2_old);
+		
+		multicore_fifo_push_blocking(1);
+
+	}
+}
+
+
+/*
+	Read serial input
+*/
+uint16_t read_serial(uint8_t *buffer) {
+	uint16_t buffer_index = 0;
+	while (1) {
+		int c = getchar_timeout_us(100);
+		if (c != PICO_ERROR_TIMEOUT &&  buffer_index < BUFFER_LENGTH) {
+			buffer[buffer_index++] = c;
+		}
+		else {
+			break;
+		}
+	}
+	return buffer_index;
+}
+
+
+/*
+	Combine n number of bytes to float
+*/
+float combine_float_bytes(uint8_t *bytes) {
+	uint32_t value = 0;
+	int n = bytes[0];
+
+	for (int i = 1; i <= n; i++) {
+		value |= bytes[i] << (8 * (i - 1));
+	}
+
+    return *reinterpret_cast<float*>(&value);
+}
+
+
+/*
+	Combine n number of bytes to int
+*/
+int combine_int_bytes(uint8_t *bytes) {
+	int result = 0;
+	int n = bytes[0];
+
+	for (int i = 1; i <= n; i++) {
+		result |= static_cast<int32_t>(bytes[i]) << ((8 * (i - 1)));
+	}
+	return result;
+}
+
+
+/*
+	Splits float into bytes for serial
+*/
+void split_float_to_bytes(float value, unsigned char* bytes) {
+	unsigned char* int_bytes = reinterpret_cast<unsigned char*>(&value);
+
+	for (int i = 0; i < 4; i++) {
+		bytes[i] = *(int_bytes + i);
+	}
+}
+
+
+int main() {
+	// Initialize serial
+    stdio_init_all();
+
+	// Loop while serial not connected
+	while(!stdio_usb_connected()) {
+		leds.OneColorFade(0);
+		sleep_us(led_speed);
+	}
+
+	// Turn off LEDs and reset the counter
+	leds.Off();
+	leds.counter = 0;
+
+	// Launch the second core
+    multicore_launch_core1(second_core);
+
+	// Main function variables
+	float angle = 0.0;
+	float r = 0.0;
+	float x = 0.0;
+	float y = 0.0;
+
+    while (1) {
+
+		// Buffer for serial commands
+		uint8_t buf[BUFFER_LENGTH] = {0xfe};
+		read_serial(&buf[0]);
+
+		// Commands
+		switch (buf[0]) {
+			// Stop
+			case COMMAND_STOP: {
+				motor1.Stop();
+				break;
+			}
+
+			// Angle first
+			case COMMAND_ANGLE: {
+				angle = combine_float_bytes(&buf[1]);
+				break;
+			}
+
+			// R second
+			case COMMAND_R: {
+				r = combine_float_bytes(&buf[1]);
+				// TODO: safeguard to check if error in data
+				multicore_fifo_push_blocking(COMMAND_R);
+				multicore_fifo_push_blocking(*reinterpret_cast<uint32_t*>(&angle));
+				multicore_fifo_push_blocking(*reinterpret_cast<uint32_t*>(&r));
+				break;
+			}
+
+			// X coordnate first
+			case COMMAND_X_COORDINATE: {
+				x = combine_float_bytes(&buf[1]);
+				break;
+			}
+
+			// Y coordnate second
+			case COMMAND_Y_COORDINATE: {
+				y = combine_float_bytes(&buf[1]);
+				// TODO: Convert cartesian to polar...
+				break;
+			}
+
+			// Home
+			case COMMAND_HOME: {
+				multicore_fifo_push_blocking(COMMAND_HOME);
+				break;
+			}
+
+			// Update angle first
+			case COMMAND_UPDATE_ANGLE: {
+				angle_old = combine_float_bytes(&buf[1]);
+				break;
+			}
+
+			// Update r second
+			case COMMAND_UPDATE_R: {
+				r_old = combine_float_bytes(&buf[1]);
+                theta2_old = polarGetTheta2(angle_old, r_old);
+                theta1_old = polarGetTheta1(theta2_old, angle_old, r_old, inverted, angle_old);
+				break;
+			}
+
+			// M1 step
+			case COMMAND_M1_STEP: {
+				multicore_fifo_push_blocking(COMMAND_M1_STEP);
+				int steps = combine_int_bytes(&buf[1]);
+				multicore_fifo_push_blocking(steps);
+				break;
+			}
+
+			// M2 step
+			case COMMAND_M2_STEP: {
+				multicore_fifo_push_blocking(COMMAND_M2_STEP);
+				int steps = combine_int_bytes(&buf[1]);
+				multicore_fifo_push_blocking(steps);
+				break;
+			}
+
+			// Set motor speed
+			case COMMAND_SET_MOTOR_SPEED: {
+				draw_speed = combine_int_bytes(&buf[1]);
+				break;
+			}
+
+			// Set led track
+			case COMMAND_SET_LED_TRACK: {
+				led_track = combine_int_bytes(&buf[1]);
+				break;
+			}
+
+			// Set led speed
+			case COMMAND_SET_LED_SPEED: {
+				led_speed = combine_int_bytes(&buf[1]) * 1000;
+				break;
+			}
+
+			// Set led intensity
+			case COMMAND_SET_LED_INTENSITY: {
+				float value = combine_float_bytes(&buf[1]);
+
+				if (value > 1.0) {
+					value = 1.0;
+				}
+				else if (value <= 0.0) {
+					value = 0.00001;
+				}
+
+				leds.intensity = value;
+				break;
+			}
+
+			// Set led saturation
+			case COMMAND_SET_LED_SATURATION: {
+				float value = combine_float_bytes(&buf[1]);
+
+				if (value > 1.0) {
+					value = 1.0;
+				}
+				else if (value <= 0.0) {
+					value = 0.0;
+				}
+
+				leds.saturation = value;
+				break;
+			}
+
+			// Set LED R
+			case COMMAND_SET_LED_R: {
+				led_track = 0;
+				float value = (float)(combine_int_bytes(&buf[1]));
+
+				if (value > 255.0) {
+					value = 255.0;
+				}
+				else if (value <= 0.0) {
+					value = 0.0;
+				}
+
+				value /= 255.0;
+				value *= leds.cycle_wrap;
+				leds.SetValue(0, (int)(value));
+				break;
+			}
+
+			// Set LED G
+			case COMMAND_SET_LED_G: {
+				led_track = 0;
+				float value = (float)(combine_int_bytes(&buf[1]));
+
+				if (value > 255.0) {
+					value = 255.0;
+				}
+				else if (value <= 0.0) {
+					value = 0.0;
+				}
+
+				value /= 255.0;
+				value *= leds.cycle_wrap;
+				leds.SetValue(1, (int)(value));
+				break;
+			}
+
+			// Set LED B
+			case COMMAND_SET_LED_B: {
+				led_track = 0;
+				float value = (float)(combine_int_bytes(&buf[1]));
+
+				if (value > 255.0) {
+					value = 255.0;
+				}
+				else if (value <= 0.0) {
+					value = 0.0;
+				}
+
+				value /= 255.0;
+				value *= leds.cycle_wrap;
+				leds.SetValue(2, (int)(value));
+				break;
+			}
+
+			// Set LED W
+			case COMMAND_SET_LED_W: {
+				led_track = 0;
+				float value = (float)(combine_int_bytes(&buf[1]));
+
+				if (value > 255.0) {
+					value = 255.0;
+				}
+				else if (value <= 0.0) {
+					value = 0.0;
+				}
+
+				value /= 255.0;
+				value *= leds.cycle_wrap;
+				leds.SetValue(3, (int)(value));
+				break;
+			}
+
+			// Get Angle
+			case COMMAND_GET_ANGLE: {
+				printf("%f\n", angle_old);
+			}
+
+			// Get R
+			case COMMAND_GET_R: {
+				printf("%f\n", r_old);
+			}
+
+			// Reset core 1
+			case COMMAND_RESET: {
+				multicore_reset_core1();
+    			multicore_launch_core1(second_core);
+			}
+		}
+
+		// Listening to core 1
+		uint32_t core1_msg = 0;
+		if (multicore_fifo_rvalid()) {
+			multicore_fifo_pop_timeout_us(100, &core1_msg);
+		}
+
+		// Returning the angle and r when motors are done
+		if (core1_msg == 1) {
+			printf("%f\n", angle_old);
+			fflush(stdout);
+			printf("%f\n", r_old);
+			fflush(stdout);
+		}
+
+		// Trigger the LED track
+		if(absolute_time_diff_us(led_time, get_absolute_time()) > led_speed) {
+			led_time = get_absolute_time();
+
+			if (led_track == 1) {
+				leds.OneColorFade(0);
+			}
+
+			else if (led_track == 2) {
+				leds.OneColorFade(1);
+			}
+
+			else if (led_track == 3) {
+				leds.OneColorFade(2);
+			}
+
+			else if (led_track == 4) {
+				leds.OneColorFade(3);
+			}
+
+			else if (led_track == 5) {
+				leds.ColorFade();
+			}
+		}
+
+    }
+}
@@ -0,0 +1,54 @@
+#include "nvs.h"
+#include "angles.h"
+
+#include <zephyr/drivers/flash.h>
+#include <zephyr/storage/flash_map.h>
+#include <zephyr/kvss/nvs.h>
+
+#include <string.h>
+
+
+static struct nvs_fs fs;
+
+// storage_partition size = 0x10000
+// FLASH sector/block size = 0x1000
+#define NVS_PARTITION			storage_partition
+#define NVS_PARTITION_DEVICE	PARTITION_DEVICE(NVS_PARTITION)
+#define NVS_PARTITION_OFFSET	PARTITION_OFFSET(NVS_PARTITION)
+
+#define ID_MY_DATA 1
+
+
+void nvs_save(struct arm_angles_t *angles) {
+	nvs_write(&fs, ID_MY_DATA, angles, sizeof(struct arm_angles_t));
+}
+
+void nvs_load(struct arm_angles_t *angles) {
+	int ret = nvs_read(&fs, ID_MY_DATA, angles, sizeof(struct arm_angles_t));
+	if (ret < 0) {
+		angles->arm1 = 0.0;
+		angles->arm2 = M_PI;
+	}
+}
+
+int nvs_init() {
+    struct flash_pages_info info;
+    int ret;
+
+    fs.flash_device = NVS_PARTITION_DEVICE;
+    if (!device_is_ready(fs.flash_device)) {
+        return -ENODEV;
+    }
+
+    fs.offset = NVS_PARTITION_OFFSET;
+
+    ret = flash_get_page_info_by_offs(fs.flash_device, fs.offset, &info);
+    if (ret) {
+        return ret;
+    }
+
+    fs.sector_size = info.size;      /* 4096 */
+    fs.sector_count = 16U;           /* 64KB / 4KB */
+
+    return nvs_mount(&fs);
+}
@@ -0,0 +1,13 @@
+#ifndef NVS_H
+#define NVS_H
+
+
+#include "angles.h"
+
+
+int nvs_init();
+void nvs_save(struct arm_angles_t *angles);
+void nvs_load(struct arm_angles_t *angles);
+
+
+#endif // NVS_H
@@ -0,0 +1,54 @@
+#include "settings.h"
+
+
+#include <zephyr/drivers/flash.h>
+#include <zephyr/storage/flash_map.h>
+#include <zephyr/kvss/nvs.h>
+
+#include <string.h>
+
+
+static struct nvs_fs fs;
+
+// settings_partition size = 0x1000
+// FLASH sector/block size = 0x1000
+#define NVS_PARTITION			settings_partition
+#define NVS_PARTITION_DEVICE	PARTITION_DEVICE(NVS_PARTITION)
+#define NVS_PARTITION_OFFSET	PARTITION_OFFSET(NVS_PARTITION)
+
+#define ID_MY_DATA 1
+
+
+void settings_save(struct settings_t *settings) {
+	nvs_write(&fs, ID_MY_DATA, settings, sizeof(struct settings_t));
+}
+
+void settings_load(struct settings_t *settings) {
+	int ret = nvs_read(&fs, ID_MY_DATA, settings, sizeof(struct settings_t));
+	if (ret < 0) {
+		settings->m1_offset = 0;
+		settings->m2_offset = 0;
+	}
+}
+
+int settings_init() {
+    struct flash_pages_info info;
+    int ret;
+
+    fs.flash_device = NVS_PARTITION_DEVICE;
+    if (!device_is_ready(fs.flash_device)) {
+        return -ENODEV;
+    }
+
+    fs.offset = NVS_PARTITION_OFFSET;
+
+    ret = flash_get_page_info_by_offs(fs.flash_device, fs.offset, &info);
+    if (ret) {
+        return ret;
+    }
+
+    fs.sector_size = info.size;      /* 4096 */
+    fs.sector_count = 1U;            /* 4KB / 4KB */
+
+    return nvs_mount(&fs);
+}
@@ -0,0 +1,16 @@
+#ifndef SETTINGS_H
+#define SETTINGS_H
+
+
+struct settings_t {
+	int m1_offset;
+	int m2_offset;
+};
+
+
+int settings_init();
+void settings_save(struct settings_t *settings);
+void settings_load(struct settings_t *settings);
+
+
+#endif // SETTINGS_H
@@ -0,0 +1,428 @@
+#include "stepper.h"
+#include "nvs.h"
+#include "usb.h"
+#include "command_message.h"
+#include "settings.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/kernel.h>
+#include <zephyr/drivers/gpio.h>
+#include <zephyr/input/input.h>
+
+#include <string.h>
+#include <stdlib.h>
+
+
+LOG_MODULE_REGISTER(stepper, LOG_LEVEL_INF);
+
+// DEVICE
+static const struct gpio_dt_spec enable_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(motors_enable), gpios);
+static const struct gpio_dt_spec m1_dir_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(m1_dir), gpios);
+static const struct gpio_dt_spec m1_step_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(m1_step), gpios);
+static const struct gpio_dt_spec m1_sensor_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(m1_sensor), gpios);
+static const struct gpio_dt_spec m2_dir_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(m2_dir), gpios);
+static const struct gpio_dt_spec m2_step_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(m2_step), gpios);
+static const struct gpio_dt_spec m2_sensor_gpio = GPIO_DT_SPEC_GET(DT_ALIAS(m2_sensor), gpios);
+
+// THREAD
+static struct k_thread stepper_thread_data;
+static k_tid_t stepper_thread_id = NULL;
+#define stepper_THREAD_STACK_SIZE 2048
+K_THREAD_STACK_DEFINE(stepper_thread_stack, stepper_THREAD_STACK_SIZE);
+
+// STEPPER
+#define MICROSTEPPING 16
+struct arm_angles_t current_angles;
+struct polar_t current_position;
+struct polar_t set_position;
+int m1_steps = 0;
+int m2_steps = 0;
+struct k_sem stepper_sem;
+bool motors_enabled = 0;
+bool homing = 0;
+bool polaring = 0;
+
+// SETTINGS
+struct settings_t settings;
+
+// SPEED
+#define MOTOR_MIN_SPEED 500
+#define MOTOR_MAX_SPEED 50000
+uint16_t motor_speed = 500;
+
+// TIMER
+struct k_timer motor_disable_timer;
+#define MOTOR_DISABLE_DELAY K_MINUTES(5)
+
+
+static void motor_disable_timer_handler(struct k_timer *timer) {
+    stepper_disable_motors();
+}
+
+K_TIMER_DEFINE(motor_disable_timer, motor_disable_timer_handler, NULL);
+
+void stepper_add_steps(int m1, int m2) {
+	m1_steps += m1;
+	m2_steps += m2;
+	k_sem_give(&stepper_sem);
+}
+
+void stepper_set_speed(uint16_t speed) {
+	if (speed < MOTOR_MIN_SPEED) { speed = MOTOR_MIN_SPEED; }
+	if (speed > MOTOR_MAX_SPEED) { speed = MOTOR_MAX_SPEED; }
+
+	motor_speed = speed;
+}
+
+void stepper_disable_motors() {
+	gpio_pin_set_dt(&enable_gpio, 1);
+	motors_enabled = 0;
+	homing = 0;
+	polaring = 0;
+	m1_steps = 0;
+	m2_steps = 0;
+	k_timer_stop(&motor_disable_timer);
+}
+
+void stepper_enable_motors() {
+	gpio_pin_set_dt(&enable_gpio, 0);
+	motors_enabled = 1;
+	k_timer_start(&motor_disable_timer, MOTOR_DISABLE_DELAY, K_NO_WAIT);
+}
+
+void stepper_home_motors() {
+	k_sem_give(&stepper_sem);
+	homing = 1;
+}
+
+void stepper_set_position(struct polar_t *coords) {
+	if (coords->r > 1.0f) { coords->r = 1.0f; }
+
+	memcpy(&set_position, coords, sizeof(struct polar_t));
+	polaring = 1;
+	k_sem_give(&stepper_sem);
+}
+
+void stepper_get_position(struct polar_t *coords) {
+	memcpy(coords, &current_position, sizeof(struct polar_t));
+}
+
+void stepper_add_offset(int m1, int m2) {
+	settings.m1_offset += m1;
+	settings.m2_offset += m2;
+	settings_save(&settings);
+
+	stepper_add_steps(m1, m2);
+}
+
+void stepper_reset_offset() {
+	settings.m1_offset = 0;
+	settings.m2_offset = 0;
+	settings_save(&settings);
+}
+
+static int stepper_device_init(const struct gpio_dt_spec *dev) {
+	int ret;
+
+	if (!device_is_ready(dev->port)) {
+		LOG_ERR("STEPPER GPIO (%d) device not ready", dev->pin);
+		return -ENODEV;
+	}
+
+	ret = gpio_pin_configure_dt(dev, GPIO_OUTPUT_INACTIVE);
+	if (ret != 0) {
+		LOG_ERR("Failed to configure STEPPER GPIO (%d): %d", dev->pin, ret);
+		return ret;
+	}
+
+	// Turn it off
+	ret = gpio_pin_set_dt(dev, 0);
+	if (ret != 0) {
+		LOG_ERR("Failed to initialize STEPPER (GPIO: %d)", dev->pin);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void stepper_step(const struct gpio_dt_spec *motor, int step, int speed_us) {
+	for (int i = 0; i < step; i++) {
+		gpio_pin_set_dt(motor, 1);
+		k_usleep(speed_us);
+		gpio_pin_set_dt(motor, 0);
+		k_usleep(speed_us);
+	}
+}
+
+static void stepper_set_dir(const struct gpio_dt_spec *motor_dir, bool dir) {
+	gpio_pin_set_dt(motor_dir, dir);
+}
+
+static void stepper_home() {
+	int tries = 0;
+
+motor_1_jump_point:
+	// MOTOR 1
+	int m1_counter = 0;
+	stepper_set_dir(&m1_dir_gpio, 0);
+	while(!(gpio_pin_get_dt(&m1_sensor_gpio) > 0) && homing) {
+		stepper_step(&m1_step_gpio, 1, MOTOR_MIN_SPEED);
+
+		// if motor 1 spins 270 degrees, somethings wrong
+		if (m1_counter > (900 * MICROSTEPPING)) {
+			stepper_disable_motors();
+			return;
+		}
+	}
+
+	// MOTOR 2
+	int m2_counter = 0;
+	stepper_set_dir(&m2_dir_gpio, 0);
+	while(!(gpio_pin_get_dt(&m2_sensor_gpio) > 0) && homing) {
+		stepper_step(&m2_step_gpio, 1, MOTOR_MIN_SPEED);
+		m2_counter++;
+
+		// if motor 2 spins 270 degrees, somethings wrong
+		if ((m2_counter > (900 * MICROSTEPPING)) && tries < 3) {
+			stepper_set_dir(&m1_dir_gpio, 1);
+			stepper_step(&m1_step_gpio, 600, 500);
+			tries++;
+			goto motor_1_jump_point;
+		}
+		else if (tries >= 3) {
+			stepper_disable_motors();
+			return;
+		}
+	}
+
+	// MOTOR 1 STEP BACK
+	stepper_set_dir(&m1_dir_gpio, 1);
+	stepper_step(&m1_step_gpio, 100, 5000);
+	
+	// MOTOR 1 SLOW
+	stepper_set_dir(&m1_dir_gpio, 0);
+	while(!(gpio_pin_get_dt(&m1_sensor_gpio) > 0) && homing) {
+		stepper_step(&m1_step_gpio, 1, 5000);
+	}
+
+	// MOTOR 2 STEP BACK
+	stepper_set_dir(&m2_dir_gpio, 1);
+	stepper_step(&m2_step_gpio, 100, 5000);
+	
+	// MOTOR 2 SLOW
+	stepper_set_dir(&m2_dir_gpio, 0);
+	while(!(gpio_pin_get_dt(&m2_sensor_gpio) > 0) && homing) {
+		stepper_step(&m2_step_gpio, 1, 500);
+	}
+
+	// MOVE THE OFFSET
+	stepper_step(&m1_step_gpio, settings.m1_offset, 5000);
+	stepper_step(&m2_step_gpio, settings.m2_offset, 5000);
+}
+
+// static bool stepper_position_match(struct polar_t *pos1, struct polar_t *pos2) {
+// 	return (pos1->theta == pos2->theta) && (pos1->r == pos2->r);
+// }
+
+static void stepper_dual_steps(int more_step, const struct gpio_dt_spec *more_motor,
+								int less_step, const struct gpio_dt_spec *less_motor) {
+	int ratio = more_step / less_step;
+	float ratio_float = (float)more_step / (float)less_step;
+	ratio_float -= (float)ratio;
+	int counter = 0;
+	int less_step_counter = 0;
+	float extra_counter = 0.0f;
+
+	for (int i = 0; i < more_step; i++) {
+		stepper_step(more_motor, 1, motor_speed);
+		counter++;
+		if ((counter >= ratio) && (less_step_counter < less_step)) {
+			if (extra_counter > 1.0f) {
+				extra_counter -= ratio;
+				continue;
+			}
+			stepper_step(less_motor, 1, motor_speed);
+			less_step_counter++;
+			counter = 0;
+			extra_counter += ratio_float;
+		}
+	}
+
+	while (less_step_counter < less_step) {
+		stepper_step(less_motor, 1, motor_speed);
+		less_step_counter++;
+	}
+}
+
+static void stepper_equal_steps(int step, const struct gpio_dt_spec *motor1, const struct gpio_dt_spec *motor2) {
+	for (int i = 0; i < step; i++) {
+		stepper_step(motor1, 1, motor_speed);
+		stepper_step(motor2, 1, motor_speed);
+	}
+}
+
+static void stepper_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+	
+	LOG_INF("stepper thread started");
+
+	while (1) {
+		k_sem_take(&stepper_sem, K_FOREVER);
+
+		uint8_t command = COMMAND_ACK;
+
+		// HOMING
+		if (homing) {
+			if (!motors_enabled) { stepper_enable_motors(); }
+			stepper_home();
+			homing = 0;
+
+			current_angles.arm1 = 0.0;
+			current_angles.arm2 = M_PI;
+			nvs_save(&current_angles);
+
+			current_position.theta = 0.0f;
+			current_position.r = 0.0f;
+
+			polaring = 0;
+			m1_steps = 0;
+			m2_steps = 0;
+
+			command = COMMAND_HOME;
+		}
+
+		// INVERSE KINEMATICS
+		if (polaring) {
+			// If R is zero, ignore angle
+			if (set_position.r == 0.0f) { set_position.theta = current_position.theta; }
+
+			// Getting angles for the arms
+			float theta2 = arm_2_angle_from_polar(&set_position);
+			float theta1 = arm_1_angle_from_polar(theta2, &set_position, false, current_angles.arm1);
+			// TODO: inverted
+
+			// Getting the change of angles
+			float delta_theta1 = delta_angles(theta1, current_angles.arm1);
+			float delta_theta2 = delta_angles(theta2, current_angles.arm2);
+
+			// Getting the steps
+			m1_steps = steps(delta_theta1, MICROSTEPPING);
+			m2_steps = steps(delta_theta2, MICROSTEPPING);
+
+			// Get arm angles from the steps
+			current_angles.arm1 += angle_from_steps(m1_steps, MICROSTEPPING);
+			current_angles.arm2 += angle_from_steps(m2_steps, MICROSTEPPING);
+
+			// Check if angles are over 2PI
+			if (current_angles.arm1 >= (M_PI2)) {
+				current_angles.arm1 -= (M_PI2);
+			}
+			else if (current_angles.arm1 <= -(M_PI2)) {
+				current_angles.arm1 += (M_PI2);
+			}
+
+			// Saving the angles
+			polar_from_arms(&current_angles, &current_position);
+
+			// Accounting for the arm2 spin
+			m2_steps += m1_steps;
+
+			command = COMMAND_POLAR;
+		}
+
+		// STEPPING
+		if (m1_steps || m2_steps) {
+			if (!motors_enabled) { stepper_enable_motors(); }
+			k_timer_start(&motor_disable_timer, MOTOR_DISABLE_DELAY, K_NO_WAIT);
+
+			int m1 = m1_steps;
+			int m2 = m2_steps;
+			m1_steps = 0;
+			m2_steps = 0;
+
+			// Set direction
+			if (m1 > 0) { stepper_set_dir(&m1_dir_gpio, 1); }
+			else { stepper_set_dir(&m1_dir_gpio, 0); }
+			if (m2 > 0) { stepper_set_dir(&m2_dir_gpio, 1); }
+			else { stepper_set_dir(&m2_dir_gpio, 0); }
+			
+			// Moving the motors
+			if ((abs(m1) > abs(m2)) && (m2 != 0)) {
+				stepper_dual_steps(abs(m1), &m1_step_gpio, abs(m2), &m2_step_gpio);
+			}
+			else if ((abs(m1) < abs(m2)) && (m1 != 0)) {
+				stepper_dual_steps(abs(m2), &m2_step_gpio, abs(m1), &m1_step_gpio);
+			}
+			else if (abs(m1) == abs(m2)) {
+				stepper_equal_steps(abs(m1), &m1_step_gpio, &m2_step_gpio);
+			}
+			else if (m2 == 0 && m1 != 0) {
+				stepper_step(&m1_step_gpio, abs(m1), motor_speed);
+			}
+			else if (m1 == 0 && m2 != 0) {
+				stepper_step(&m2_step_gpio, abs(m2), motor_speed);
+			}
+
+			if (command == COMMAND_ACK) { command = COMMAND_MOTOR_STEP; }
+
+			// TODO: timer to save angles to NVS
+			nvs_save(&current_angles);
+
+		}
+
+		// Send ACK that steppers are done
+		// ACK + command as data
+		struct command_message_t ack;
+		command_create_message(&ack, 1, COMMAND_ACK, &command);
+		usb_send_command(&ack);
+	}
+	
+	LOG_INF("stepper thread exiting");
+}
+
+int stepper_init() {
+	// SEMAPHORE
+	k_sem_init(&stepper_sem, 0, 1);
+
+	// CURRENT POSITION
+	nvs_load(&current_angles);
+	polar_from_arms(&current_angles, &current_position);
+
+	// SETTINGS
+	settings_load(&settings);
+
+	// OUTPUTS
+	stepper_device_init(&enable_gpio);
+	stepper_device_init(&m1_dir_gpio);
+	stepper_device_init(&m1_step_gpio);
+	stepper_device_init(&m2_dir_gpio);
+	stepper_device_init(&m2_step_gpio);
+
+	// INPUTS
+	gpio_pin_configure_dt(&m1_sensor_gpio, GPIO_INPUT);
+	gpio_pin_configure_dt(&m2_sensor_gpio, GPIO_INPUT);
+
+	// Disable steppers at boot
+	stepper_disable_motors();
+
+	// THREAD
+	stepper_thread_id = k_thread_create(
+		&stepper_thread_data,
+		stepper_thread_stack,
+		K_THREAD_STACK_SIZEOF(stepper_thread_stack),
+		stepper_thread,
+		NULL, NULL, NULL,
+		5,
+		0,
+		K_NO_WAIT
+	);
+
+	if (stepper_thread_id == NULL) {
+		LOG_ERR("Failed to create stepper thread");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
@@ -0,0 +1,22 @@
+#ifndef STEPPER_H
+#define STEPPER_H
+
+
+#include "angles.h"
+
+#include <stdint.h>
+
+
+int stepper_init();
+void stepper_add_steps(int m1, int m2);
+void stepper_set_speed(uint16_t speed);
+void stepper_disable_motors();
+void stepper_enable_motors();
+void stepper_home_motors();
+void stepper_set_position(struct polar_t *coords);
+void stepper_get_position(struct polar_t *coords);
+void stepper_add_offset(int m1, int m2);
+void stepper_reset_offset();
+
+
+#endif // STEPPER_H
@@ -0,0 +1,231 @@
+#include "usb.h"
+#include "usb_conf.h"
+#include "command_handler.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/device.h>
+#include <zephyr/drivers/uart.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/ring_buffer.h>
+
+// PICO-SDK
+#include "pico/bootrom.h"
+
+LOG_MODULE_REGISTER(usb, LOG_LEVEL_INF);
+
+// DEVICE
+const struct device *const uart_dev = DEVICE_DT_GET_ONE(zephyr_cdc_acm_uart);
+static struct usbd_context *usb_context;
+
+// THREAD
+static struct k_thread usb_thread_data;
+static k_tid_t usb_thread_id = NULL;
+#define USB_THREAD_STACK_SIZE 2048
+K_THREAD_STACK_DEFINE(usb_thread_stack, USB_THREAD_STACK_SIZE);
+
+// RX BUFFER
+#define RING_BUF_SIZE 255
+static uint8_t ring_buffer[RING_BUF_SIZE];
+static struct ring_buf ringbuf;
+struct k_sem rx_semaphore;
+
+// ACK / NACK messages
+#define RETURN_ACK true
+struct command_message_t ack_msg;
+struct command_message_t nack_msg;
+
+
+static void interrupt_handler(const struct device *dev, void *user_data) {
+	ARG_UNUSED(user_data);
+
+	while (true) {
+		uart_irq_update(dev);
+
+		if (uart_irq_is_pending(dev) <= 0) {
+			break;
+		}
+
+		if (uart_irq_rx_ready(dev)) {
+			int recv_len, rb_len;
+			uint8_t buffer[64];
+			size_t len = MIN(ring_buf_space_get(&ringbuf), sizeof(buffer));
+
+			if (len == 0) {
+				// ring buffer full, drops package(s)
+			    uart_irq_rx_disable(dev);
+				k_sem_give(&rx_semaphore);
+				break;
+			}
+
+			recv_len = uart_fifo_read(dev, buffer, len);
+			if (recv_len < 0) {
+				LOG_ERR("Failed to read UART FIFO");
+				recv_len = 0;
+			};
+
+			rb_len = ring_buf_put(&ringbuf, buffer, recv_len);
+			if (rb_len < recv_len) {
+				LOG_ERR("Drop %u bytes", recv_len - rb_len);
+			}
+
+			k_sem_give(&rx_semaphore);
+		}
+	}
+}
+
+static void usb_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+	struct command_message_t msg;
+	command_message_init(&msg);
+	
+	LOG_INF("USB command processing thread started");
+	
+	while (1) {
+		k_sem_take(&rx_semaphore, K_FOREVER);
+
+		int len;
+
+		// While ring buffer has data
+		do {
+			uint8_t buf_prefix;
+			len = ring_buf_get(&ringbuf, &buf_prefix, 1);
+
+			if (len && (buf_prefix == COMMAND_PREFIX)) {
+				uint8_t buf_header[4];
+				len = ring_buf_get(&ringbuf, buf_header, 4);
+
+				if ((len == 4) && (buf_header[1] == COMMAND_ID) && (buf_header[0] <= COMMAND_DATA_SIZE)) {
+					msg.length = buf_header[0];
+					msg.command = buf_header[2];
+					msg.crc = buf_header[3];
+
+					if (msg.length) {
+						len = ring_buf_get(&ringbuf, msg.data, msg.length);
+					}
+
+					uint8_t calculated_crc = command_calculate_crc(&msg);
+					if (calculated_crc != msg.crc) {
+						if (RETURN_ACK) {
+							// Send NACK
+							usb_send_command(&nack_msg);
+						}
+						continue;
+					}
+
+					int ret = command_handler(&msg);
+					if (ret >= 0) {
+						if (RETURN_ACK) {
+							// Send ACK
+							usb_send_command(&ack_msg);
+						}
+					}
+					else if (ret < -1) {
+						if (RETURN_ACK) {
+							// Send NACK
+							usb_send_command(&nack_msg);
+						}
+					}
+
+				}
+				else {
+					// Command_id did not match, ignore
+					continue;
+				}
+			}
+			else {
+				// Prefix did not match, ignore
+				continue;
+			}
+		} while (len > 0);
+
+		uart_irq_rx_enable(uart_dev);
+	}
+	
+	LOG_INF("USB command processing thread exiting");
+}
+
+static void usb_msg_cb(struct usbd_context *const ctx, const struct usbd_msg *msg) {
+	if (msg->type == USBD_MSG_CDC_ACM_LINE_CODING) {
+		// Jump to BOOTSEL when baudrate changes to 1200
+		uint32_t baudrate;
+
+		if (uart_line_ctrl_get(msg->dev, UART_LINE_CTRL_BAUD_RATE, &baudrate) == 0) {
+			LOG_INF("Baudrate %u", baudrate);
+
+			if (baudrate == 1200) {
+				LOG_INF("Entering BOOTSEL...");
+				reset_usb_boot(0, 0);
+			}
+		}
+	}
+}
+
+int usb_init() {
+	ring_buf_init(&ringbuf, sizeof(ring_buffer), ring_buffer);
+	k_sem_init(&rx_semaphore, 0, 1);
+
+	command_create_ack(&ack_msg);
+	command_create_nack(&nack_msg);
+
+	int ret;
+
+	if (!device_is_ready(uart_dev)) {
+		LOG_ERR("CDC ACM device not ready");
+		return -ENODEV;
+	}
+
+	usb_context = usb_device_init(usb_msg_cb);
+	if (usb_context == NULL) {
+		LOG_ERR("Failed to initialize USB device");
+		return -ENODEV;
+	}
+
+	if (!usbd_can_detect_vbus(usb_context)) {
+		ret = usbd_enable(usb_context);
+		if (ret) {
+			LOG_ERR("Failed to enable device support");
+			return ret;
+		}
+	}
+	k_msleep(100);
+
+	uart_irq_callback_set(uart_dev, interrupt_handler);
+	uart_irq_rx_enable(uart_dev);
+
+	usb_thread_id = k_thread_create(
+		&usb_thread_data,
+		usb_thread_stack,
+		K_THREAD_STACK_SIZEOF(usb_thread_stack),
+		usb_thread,
+		NULL, NULL, NULL,
+		5,
+		0,
+		K_NO_WAIT
+	);
+
+	if (usb_thread_id == NULL) {
+		LOG_ERR("Failed to create USB thread");
+		return -ENOMEM;
+	}
+
+	return ret;
+}
+
+int usb_send_command(struct command_message_t *msg) {
+	if (!device_is_ready(uart_dev)) {
+		return -ENODEV;
+	}
+
+	// Message size: prefix + length + id + command + crc + data
+	size_t msg_size = 5 + msg->length;
+	uint8_t *msg_bytes = (uint8_t *)msg;
+
+	/* uart_poll_out blocks until sent, ensuring data integrity */
+	for (size_t i = 0; i < msg_size; i++) {
+		uart_poll_out(uart_dev, msg_bytes[i]);
+	}
+
+	return 0;
+}
@@ -0,0 +1,12 @@
+#ifndef USB_H
+#define USB_H
+
+
+#include "command_message.h"
+
+
+int usb_init();
+int usb_send_command(struct command_message_t *msg);
+
+
+#endif // USB_H
@@ -0,0 +1,184 @@
+#include "usb_conf.h"
+
+#include <stdint.h>
+#include <zephyr/device.h>
+#include <zephyr/usb/usbd.h>
+#include <zephyr/usb/bos.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(usb_conf, LOG_LEVEL_DBG);
+
+/* By default, do not register the USB DFU class DFU mode instance. */
+static const char *const blocklist[] = {
+	"dfu_dfu",
+	NULL,
+};
+
+/*
+ * Instantiate a context named my_usb_context using the default USB device
+ * controller, the Zephyr project vendor ID, and the sample product ID.
+ * Zephyr project vendor ID must not be used outside of Zephyr samples.
+ */
+USBD_DEVICE_DEFINE(my_usb_context,
+		   DEVICE_DT_GET(DT_NODELABEL(zephyr_udc0)),
+		   CONFIG_USBD_VID, CONFIG_USBD_PID);
+
+USBD_DESC_LANG_DEFINE(my_usb_lang);
+USBD_DESC_MANUFACTURER_DEFINE(my_usb_mfr, CONFIG_USBD_MANUFACTURER);
+USBD_DESC_PRODUCT_DEFINE(my_usb_product, CONFIG_USBD_PRODUCT);
+IF_ENABLED(CONFIG_HWINFO, (USBD_DESC_SERIAL_NUMBER_DEFINE(my_usb_serial)));
+
+USBD_DESC_CONFIG_DEFINE(fs_cfg_desc, "FS Configuration");
+USBD_DESC_CONFIG_DEFINE(hs_cfg_desc, "HS Configuration");
+
+static const uint8_t attributes = (IS_ENABLED(CONFIG_USBD_SELF_POWERED) ?
+				   USB_SCD_SELF_POWERED : 0) |
+				  (IS_ENABLED(CONFIG_USBD_REMOTE_WAKEUP) ?
+				   USB_SCD_REMOTE_WAKEUP : 0);
+
+/* Full speed configuration */
+USBD_CONFIGURATION_DEFINE(sample_fs_config,
+			  attributes,
+			  CONFIG_USBD_MAX_POWER, &fs_cfg_desc);
+
+/* High speed configuration */
+USBD_CONFIGURATION_DEFINE(sample_hs_config,
+			  attributes,
+			  CONFIG_USBD_MAX_POWER, &hs_cfg_desc);
+
+#if CONFIG_SAMPLE_USBD_20_EXTENSION_DESC
+/*
+ * This does not yet provide valuable information, but rather serves as an
+ * example, and will be improved in the future.
+ */
+static const struct usb_bos_capability_lpm bos_cap_lpm = {
+	.bLength = sizeof(struct usb_bos_capability_lpm),
+	.bDescriptorType = USB_DESC_DEVICE_CAPABILITY,
+	.bDevCapabilityType = USB_BOS_CAPABILITY_EXTENSION,
+	.bmAttributes = 0UL,
+};
+
+USBD_DESC_BOS_DEFINE(my_usb_usbext, sizeof(bos_cap_lpm), &bos_cap_lpm);
+#endif
+
+static void usb_fix_code_triple(struct usbd_context *uds_ctx, const enum usbd_speed speed) {
+	/* Always use class code information from Interface Descriptors */
+	if (IS_ENABLED(CONFIG_USBD_CDC_ACM_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_CDC_ECM_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_CDC_NCM_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_MIDI2_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_AUDIO2_CLASS) ||
+	    IS_ENABLED(CONFIG_USBD_VIDEO_CLASS)) {
+		/*
+		 * Class with multiple interfaces have an Interface
+		 * Association Descriptor available, use an appropriate triple
+		 * to indicate it.
+		 */
+		usbd_device_set_code_triple(uds_ctx, speed,
+					    USB_BCC_MISCELLANEOUS, 0x02, 0x01);
+	} else {
+		usbd_device_set_code_triple(uds_ctx, speed, 0, 0, 0);
+	}
+}
+
+struct usbd_context *usb_device_setup(usbd_msg_cb_t msg_cb) {
+	int err;
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_lang);
+	if (err) {
+		LOG_ERR("Failed to initialize language descriptor (%d)", err);
+		return NULL;
+	}
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_mfr);
+	if (err) {
+		LOG_ERR("Failed to initialize manufacturer descriptor (%d)", err);
+		return NULL;
+	}
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_product);
+	if (err) {
+		LOG_ERR("Failed to initialize product descriptor (%d)", err);
+		return NULL;
+	}
+
+	IF_ENABLED(CONFIG_HWINFO, (
+		err = usbd_add_descriptor(&my_usb_context, &my_usb_serial);
+	))
+	if (err) {
+		LOG_ERR("Failed to initialize SN descriptor (%d)", err);
+		return NULL;
+	}
+
+	if (USBD_SUPPORTS_HIGH_SPEED &&
+	    usbd_caps_speed(&my_usb_context) == USBD_SPEED_HS) {
+		err = usbd_add_configuration(&my_usb_context, USBD_SPEED_HS,
+					     &sample_hs_config);
+		if (err) {
+			LOG_ERR("Failed to add High-Speed configuration");
+			return NULL;
+		}
+
+		err = usbd_register_all_classes(&my_usb_context, USBD_SPEED_HS, 1,
+						blocklist);
+		if (err) {
+			LOG_ERR("Failed to add register classes");
+			return NULL;
+		}
+
+		usb_fix_code_triple(&my_usb_context, USBD_SPEED_HS);
+	}
+
+	err = usbd_add_configuration(&my_usb_context, USBD_SPEED_FS,
+				     &sample_fs_config);
+	if (err) {
+		LOG_ERR("Failed to add Full-Speed configuration");
+		return NULL;
+	}
+
+	err = usbd_register_all_classes(&my_usb_context, USBD_SPEED_FS, 1, blocklist);
+	if (err) {
+		LOG_ERR("Failed to add register classes");
+		return NULL;
+	}
+
+	usb_fix_code_triple(&my_usb_context, USBD_SPEED_FS);
+	usbd_self_powered(&my_usb_context, attributes & USB_SCD_SELF_POWERED);
+
+	if (msg_cb != NULL) {
+		err = usbd_msg_register_cb(&my_usb_context, msg_cb);
+		if (err) {
+			LOG_ERR("Failed to register message callback");
+			return NULL;
+		}
+	}
+
+#if CONFIG_SAMPLE_USBD_20_EXTENSION_DESC
+	(void)usbd_device_set_bcd_usb(&my_usb_context, USBD_SPEED_FS, 0x0201);
+	(void)usbd_device_set_bcd_usb(&my_usb_context, USBD_SPEED_HS, 0x0201);
+
+	err = usbd_add_descriptor(&my_usb_context, &my_usb_usbext);
+	if (err) {
+		LOG_ERR("Failed to add USB 2.0 Extension Descriptor");
+		return NULL;
+	}
+#endif
+
+	return &my_usb_context;
+}
+
+struct usbd_context *usb_device_init(usbd_msg_cb_t msg_cb) {
+	int err;
+
+	if (usb_device_setup(msg_cb) == NULL) {
+		return NULL;
+	}
+
+	err = usbd_init(&my_usb_context);
+	if (err) {
+		LOG_ERR("Failed to initialize device support");
+		return NULL;
+	}
+
+	return &my_usb_context;
+}
@@ -0,0 +1,11 @@
+#ifndef USB_CONF_H
+#define USB_CONF_H
+
+
+#include <zephyr/usb/usbd.h>
+
+struct usbd_context *usb_device_setup(usbd_msg_cb_t msg_cb);
+struct usbd_context *usb_device_init(usbd_msg_cb_t msg_cb);
+
+
+#endif // USB_CONF_H