diff --git a/servo_controller/prj.conf b/servo_controller/prj.conf
index b438aa3..aa4d4e3 100644
--- a/servo_controller/prj.conf
+++ b/servo_controller/prj.conf
@@ -1,5 +1,6 @@
 CONFIG_GPIO=y
 CONFIG_PWM=y
+CONFIG_ADC=y
 
 # Serial
 CONFIG_SERIAL=y
@@ -19,7 +20,8 @@ CONFIG_USBD_SELF_POWERED=y
 CONFIG_USBD_MAX_POWER=125
 
 # LOG
-CONFIG_LOG=n
+CONFIG_LOG=y
+CONFIG_CBPRINTF_FP_SUPPORT=y  # For logging float values
 CONFIG_USBD_CDC_ACM_LOG_LEVEL_OFF=y  # This removes a pointless warning
 CONFIG_LOG_DEFAULT_LEVEL=3
 CONFIG_LOG_MODE_IMMEDIATE=y
diff --git a/servo_controller/scripts/led_blink.py b/servo_controller/scripts/led_blink.py
index fd75b1f..77faa82 100644
--- a/servo_controller/scripts/led_blink.py
+++ b/servo_controller/scripts/led_blink.py
@@ -12,7 +12,10 @@ COMMAND_PREFIX = 0x69
 
 COMMAND_ACK = 0
 COMMAND_NACK = 1
-LED = 2
+LED_TOGGLE = 2
+LED_SET = 3
+SERVO_SET = 4
+SERVO_SET_ALL = 5
 
 DEVICE_ID = 0
 
@@ -112,7 +115,7 @@ def main():
 
     for i in range(10):
         print(f"--> Sending LED command {i + 1}")
-        ser.write(make_packet(LED))
+        ser.write(make_packet(LED_TOGGLE))
         time.sleep(0.5)
 
     print("Done.")
diff --git a/servo_controller/scripts/read_adc.py b/servo_controller/scripts/read_adc.py
new file mode 100644
index 0000000..206f27c
--- /dev/null
+++ b/servo_controller/scripts/read_adc.py
@@ -0,0 +1,126 @@
+#!/usr/bin/env python3
+
+import serial
+import struct
+import threading
+import time
+
+PORT = "/dev/ttyACM0"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+
+COMMAND_ACK = 0
+COMMAND_NACK = 1
+LED_TOGGLE = 2
+LED_SET = 3
+SERVO_SET = 4
+SERVO_SET_ALL = 5
+ADC_READ = 6
+ADC_READ_ALL = 7
+
+DEVICE_ID = 0
+
+
+def calculate_crc(msg: bytes) -> int:
+	s = sum(msg) & 0xFF
+	return (-s) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"") -> bytes:
+	length = len(data)
+
+	pkt = bytearray()
+	pkt.append(COMMAND_PREFIX)
+	pkt.append(length)
+	pkt.append(DEVICE_ID)
+	pkt.append(command)
+	pkt.append(0)			  # CRC placeholder
+	pkt.extend(data)
+
+	pkt[4] = calculate_crc(pkt[:4] + pkt[5:])
+
+	return bytes(pkt)
+
+
+def verify_crc(packet: bytes) -> bool:
+	crc = packet[4]
+	calc = calculate_crc(packet[:4] + packet[5:])
+	return crc == calc
+
+
+def packet_size(buf: bytes):
+	if len(buf) < 2:
+		return None
+	return 5 + buf[1]
+
+
+def reader(ser):
+	rx = bytearray()
+
+	while True:
+		data = ser.read(64)
+		if not data:
+			continue
+
+		rx.extend(data)
+
+		while rx:
+			# Binary packet?
+			if rx[0] == COMMAND_PREFIX:
+				size = packet_size(rx)
+				if size is None or len(rx) < size:
+					break
+
+				pkt = bytes(rx[:size])
+				del rx[:size]
+
+				if not verify_crc(pkt):
+					print("RX: Bad CRC:", pkt.hex())
+					continue
+
+				length = pkt[1]
+				dev_id = pkt[2]
+				cmd = pkt[3]
+
+				if cmd == COMMAND_ACK:
+					print(f"<-- ACK (device={dev_id})")
+
+				elif cmd == COMMAND_NACK:
+					print(f"<-- NACK (device={dev_id})")
+
+				else:
+					print(f"<-- Command {cmd} len={length}")
+					print(f"<-- RX: {pkt.hex(' ')} ")
+
+			else:
+				# ASCII log output
+				idx = rx.find(b'\n')
+				if idx == -1:
+					break
+
+				line = rx[:idx + 1]
+				del rx[:idx + 1]
+
+				try:
+					print("[LOG]", line.decode().rstrip())
+				except UnicodeDecodeError:
+					print("[RAW]", line.hex())
+
+
+def main():
+	ser = serial.Serial(PORT, BAUDRATE, timeout=0.05)
+
+	threading.Thread(target=reader, args=(ser,), daemon=True).start()
+
+	data = bytearray()
+	data.append(17)
+
+	ser.write(make_packet(ADC_READ, data))
+
+	print("Done.")
+	time.sleep(2)
+
+
+if __name__ == "__main__":
+	main()
diff --git a/servo_controller/scripts/read_adc_all.py b/servo_controller/scripts/read_adc_all.py
new file mode 100644
index 0000000..1ec855b
--- /dev/null
+++ b/servo_controller/scripts/read_adc_all.py
@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+
+import serial
+import struct
+import threading
+import time
+
+PORT = "/dev/ttyACM0"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+
+COMMAND_ACK = 0
+COMMAND_NACK = 1
+LED_TOGGLE = 2
+LED_SET = 3
+SERVO_SET = 4
+SERVO_SET_ALL = 5
+ADC_READ = 6
+ADC_READ_ALL = 7
+
+DEVICE_ID = 0
+
+
+def calculate_crc(msg: bytes) -> int:
+	s = sum(msg) & 0xFF
+	return (-s) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"") -> bytes:
+	length = len(data)
+
+	pkt = bytearray()
+	pkt.append(COMMAND_PREFIX)
+	pkt.append(length)
+	pkt.append(DEVICE_ID)
+	pkt.append(command)
+	pkt.append(0)			  # CRC placeholder
+	pkt.extend(data)
+
+	pkt[4] = calculate_crc(pkt[:4] + pkt[5:])
+
+	return bytes(pkt)
+
+
+def verify_crc(packet: bytes) -> bool:
+	crc = packet[4]
+	calc = calculate_crc(packet[:4] + packet[5:])
+	return crc == calc
+
+
+def packet_size(buf: bytes):
+	if len(buf) < 2:
+		return None
+	return 5 + buf[1]
+
+
+def reader(ser):
+	rx = bytearray()
+
+	while True:
+		data = ser.read(64)
+		if not data:
+			continue
+
+		rx.extend(data)
+
+		while rx:
+			# Binary packet?
+			if rx[0] == COMMAND_PREFIX:
+				size = packet_size(rx)
+				if size is None or len(rx) < size:
+					break
+
+				pkt = bytes(rx[:size])
+				del rx[:size]
+
+				if not verify_crc(pkt):
+					print("RX: Bad CRC:", pkt.hex())
+					continue
+
+				length = pkt[1]
+				dev_id = pkt[2]
+				cmd = pkt[3]
+
+				if cmd == COMMAND_ACK:
+					print(f"<-- ACK (device={dev_id})")
+
+				elif cmd == COMMAND_NACK:
+					print(f"<-- NACK (device={dev_id})")
+
+				else:
+					print(f"<-- Command {cmd} len={length}")
+					print(f"<-- RX: {pkt.hex(' ')} ")
+
+			else:
+				# ASCII log output
+				idx = rx.find(b'\n')
+				if idx == -1:
+					break
+
+				line = rx[:idx + 1]
+				del rx[:idx + 1]
+
+				try:
+					print("[LOG]", line.decode().rstrip())
+				except UnicodeDecodeError:
+					print("[RAW]", line.hex())
+
+
+def main():
+	ser = serial.Serial(PORT, BAUDRATE, timeout=0.05)
+
+	threading.Thread(target=reader, args=(ser,), daemon=True).start()
+
+	data = bytearray()
+
+	ser.write(make_packet(ADC_READ_ALL, data))
+
+	print("Done.")
+	time.sleep(2)
+
+
+if __name__ == "__main__":
+	main()
diff --git a/servo_controller/scripts/set_all_servo.py b/servo_controller/scripts/set_all_servo.py
new file mode 100644
index 0000000..f610409
--- /dev/null
+++ b/servo_controller/scripts/set_all_servo.py
@@ -0,0 +1,128 @@
+#!/usr/bin/env python3
+
+import serial
+import struct
+import threading
+import time
+
+PORT = "/dev/ttyACM0"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+
+COMMAND_ACK = 0
+COMMAND_NACK = 1
+LED_TOGGLE = 2
+LED_SET = 3
+SERVO_SET = 4
+SERVO_SET_ALL = 5
+
+DEVICE_ID = 0
+
+
+def calculate_crc(msg: bytes) -> int:
+	s = sum(msg) & 0xFF
+	return (-s) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"") -> bytes:
+	length = len(data)
+
+	pkt = bytearray()
+	pkt.append(COMMAND_PREFIX)
+	pkt.append(length)
+	pkt.append(DEVICE_ID)
+	pkt.append(command)
+	pkt.append(0)			  # CRC placeholder
+	pkt.extend(data)
+
+	pkt[4] = calculate_crc(pkt[:4] + pkt[5:])
+
+	return bytes(pkt)
+
+
+def verify_crc(packet: bytes) -> bool:
+	crc = packet[4]
+	calc = calculate_crc(packet[:4] + packet[5:])
+	return crc == calc
+
+
+def packet_size(buf: bytes):
+	if len(buf) < 2:
+		return None
+	return 5 + buf[1]
+
+
+def reader(ser):
+	rx = bytearray()
+
+	while True:
+		data = ser.read(64)
+		if not data:
+			continue
+
+		rx.extend(data)
+
+		while rx:
+			# Binary packet?
+			if rx[0] == COMMAND_PREFIX:
+				size = packet_size(rx)
+				if size is None or len(rx) < size:
+					break
+
+				pkt = bytes(rx[:size])
+				del rx[:size]
+
+				if not verify_crc(pkt):
+					print("RX: Bad CRC:", pkt.hex())
+					continue
+
+				length = pkt[1]
+				dev_id = pkt[2]
+				cmd = pkt[3]
+
+				if cmd == COMMAND_ACK:
+					print(f"<-- ACK (device={dev_id})")
+
+				elif cmd == COMMAND_NACK:
+					print(f"<-- NACK (device={dev_id})")
+
+				elif cmd == COMMAND_ERROR:
+					print(f"<-- ERROR (device={dev_id})")
+
+				else:
+					print(f"<-- Command {cmd} len={length}")
+
+			else:
+				# ASCII log output
+				idx = rx.find(b'\n')
+				if idx == -1:
+					break
+
+				line = rx[:idx + 1]
+				del rx[:idx + 1]
+
+				try:
+					print("[LOG]", line.decode().rstrip())
+				except UnicodeDecodeError:
+					print("[RAW]", line.hex())
+
+
+def main():
+	ser = serial.Serial(PORT, BAUDRATE, timeout=0.05)
+
+	threading.Thread(target=reader, args=(ser,), daemon=True).start()
+
+	angle = 0.123
+	data = bytearray()
+	for i in range(18):
+		data.extend(struct.pack('<f', angle))
+
+	ser.write(make_packet(SERVO_SET_ALL, data))
+
+	print("Done.")
+	time.sleep(2)
+
+
+if __name__ == "__main__":
+	main()
diff --git a/servo_controller/scripts/set_servo.py b/servo_controller/scripts/set_servo.py
new file mode 100644
index 0000000..21957fc
--- /dev/null
+++ b/servo_controller/scripts/set_servo.py
@@ -0,0 +1,128 @@
+#!/usr/bin/env python3
+
+import serial
+import struct
+import threading
+import time
+
+PORT = "/dev/ttyACM0"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+
+COMMAND_ACK = 0
+COMMAND_NACK = 1
+LED_TOGGLE = 2
+LED_SET = 3
+SERVO_SET = 4
+SERVO_SET_ALL = 5
+
+DEVICE_ID = 0
+
+
+def calculate_crc(msg: bytes) -> int:
+	s = sum(msg) & 0xFF
+	return (-s) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"") -> bytes:
+	length = len(data)
+
+	pkt = bytearray()
+	pkt.append(COMMAND_PREFIX)
+	pkt.append(length)
+	pkt.append(DEVICE_ID)
+	pkt.append(command)
+	pkt.append(0)			  # CRC placeholder
+	pkt.extend(data)
+
+	pkt[4] = calculate_crc(pkt[:4] + pkt[5:])
+
+	return bytes(pkt)
+
+
+def verify_crc(packet: bytes) -> bool:
+	crc = packet[4]
+	calc = calculate_crc(packet[:4] + packet[5:])
+	return crc == calc
+
+
+def packet_size(buf: bytes):
+	if len(buf) < 2:
+		return None
+	return 5 + buf[1]
+
+
+def reader(ser):
+	rx = bytearray()
+
+	while True:
+		data = ser.read(64)
+		if not data:
+			continue
+
+		rx.extend(data)
+
+		while rx:
+			# Binary packet?
+			if rx[0] == COMMAND_PREFIX:
+				size = packet_size(rx)
+				if size is None or len(rx) < size:
+					break
+
+				pkt = bytes(rx[:size])
+				del rx[:size]
+
+				if not verify_crc(pkt):
+					print("RX: Bad CRC:", pkt.hex())
+					continue
+
+				length = pkt[1]
+				dev_id = pkt[2]
+				cmd = pkt[3]
+
+				if cmd == COMMAND_ACK:
+					print(f"<-- ACK (device={dev_id})")
+
+				elif cmd == COMMAND_NACK:
+					print(f"<-- NACK (device={dev_id})")
+
+				elif cmd == COMMAND_ERROR:
+					print(f"<-- ERROR (device={dev_id})")
+
+				else:
+					print(f"<-- Command {cmd} len={length}")
+
+			else:
+				# ASCII log output
+				idx = rx.find(b'\n')
+				if idx == -1:
+					break
+
+				line = rx[:idx + 1]
+				del rx[:idx + 1]
+
+				try:
+					print("[LOG]", line.decode().rstrip())
+				except UnicodeDecodeError:
+					print("[RAW]", line.hex())
+
+
+def main():
+	ser = serial.Serial(PORT, BAUDRATE, timeout=0.05)
+
+	threading.Thread(target=reader, args=(ser,), daemon=True).start()
+
+	angle = 0.123
+	data = bytearray()
+	data.append(0)
+	data.extend(struct.pack('<f', angle))
+
+	ser.write(make_packet(SERVO_SET, data))
+
+	print("Done.")
+	time.sleep(2)
+
+
+if __name__ == "__main__":
+	main()
diff --git a/servo_controller/src/adc.c b/servo_controller/src/adc.c
new file mode 100644
index 0000000..de07663
--- /dev/null
+++ b/servo_controller/src/adc.c
@@ -0,0 +1,120 @@
+#include "adc.h"
+
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/adc.h>
+
+LOG_MODULE_REGISTER(adc, LOG_LEVEL_INF);
+
+
+#define ZEPHYR_USER_NODE DT_PATH(zephyr_user)
+static const struct adc_dt_spec adc_mux_spec = ADC_DT_SPEC_GET_BY_IDX(ZEPHYR_USER_NODE, 0);
+static const struct adc_dt_spec adc_ch16_spec = ADC_DT_SPEC_GET_BY_IDX(ZEPHYR_USER_NODE, 1);
+static const struct adc_dt_spec adc_ch17_spec = ADC_DT_SPEC_GET_BY_IDX(ZEPHYR_USER_NODE, 2);
+
+int16_t adc_buffer;
+struct adc_sequence sequence = {
+    .buffer = &adc_buffer,
+    .buffer_size = sizeof(adc_buffer),
+};
+
+int adc_init_all(void) {
+	int ret;
+
+	// ret = mux_init();
+	// if (ret != 0) {
+	// 	LOG_ERR("Failed to setup multiplexer: %d", ret);
+	// 	return ret;
+	// }
+
+	if (!adc_is_ready_dt(&adc_mux_spec)) {
+		LOG_ERR("ADC mux channel not ready");
+		return -1;
+	}
+	if (!adc_is_ready_dt(&adc_ch16_spec)) {
+		LOG_ERR("ADC channel 17 not ready");
+		return -1;
+	}
+	if (!adc_is_ready_dt(&adc_ch17_spec)) {
+		LOG_ERR("ADC channel 18 not ready");
+		return -1;
+	}
+
+	ret = adc_channel_setup_dt(&adc_mux_spec);
+	if (ret != 0) {
+		LOG_ERR("Failed to setup ADC mux: %d", ret);
+		return ret;
+	}
+	ret = adc_channel_setup_dt(&adc_ch16_spec);
+	if (ret != 0) {
+		LOG_ERR("Failed to setup ADC channel 17: %d", ret);
+		return ret;
+	}
+	ret = adc_channel_setup_dt(&adc_ch17_spec);
+	if (ret != 0) {
+		LOG_ERR("Failed to setup ADC channel 18: %d", ret);
+		return ret;
+	}
+
+	adc_sequence_init_dt(&adc_mux_spec, &sequence);
+	adc_sequence_init_dt(&adc_ch16_spec, &sequence);
+	adc_sequence_init_dt(&adc_ch17_spec, &sequence);
+
+    return 0;
+}
+
+
+int adc_read_id(int id) {
+	int ret;
+    int val_mv = 0;
+    adc_buffer = 0;
+
+	if (id < MUX_CHANNELS) {
+		// Read muxed channels
+		(void)adc_sequence_init_dt(&adc_mux_spec, &sequence);
+
+		// Set mux TODO:
+		// ret = mux_select_channel(id);
+		// if (ret < 0) {
+		// 	LOG_ERR("Could not set multiplexer (%d)\n", ret);
+		// 	return 0;
+		// }
+		
+		ret = adc_read_dt(&adc_mux_spec, &sequence);
+		if (ret < 0) {
+			LOG_ERR("Could not read (%d)\n", ret);
+			return 0;
+		}
+
+		val_mv = (int)adc_buffer;
+		// ret = adc_raw_to_millivolts_dt(&adc_mux_spec, &val_mv);
+	}
+	else if (id == MUX_CHANNELS) {
+		// Read unmuxed channel 17
+		(void)adc_sequence_init_dt(&adc_ch16_spec, &sequence);
+
+		ret = adc_read_dt(&adc_ch16_spec, &sequence);
+		if (ret < 0) {
+			LOG_ERR("Could not read (%d)\n", ret);
+			return 0;
+		}
+
+		val_mv = (int)adc_buffer;
+		// ret = adc_raw_to_millivolts_dt(&adc_ch16_spec, &val_mv);
+	}
+	else if (id == (MUX_CHANNELS + 1)) {
+		// Read unmuxed channel 18
+		(void)adc_sequence_init_dt(&adc_ch17_spec, &sequence);
+
+		ret = adc_read_dt(&adc_ch17_spec, &sequence);
+		if (ret < 0) {
+			LOG_ERR("Could not read (%d)\n", ret);
+			return 0;
+		}
+
+		val_mv = (int)adc_buffer;
+		// ret = adc_raw_to_millivolts_dt(&adc_ch17_spec, &val_mv);
+	}
+
+    
+	return val_mv;
+}
\ No newline at end of file
diff --git a/servo_controller/src/adc.h b/servo_controller/src/adc.h
new file mode 100644
index 0000000..aeab1ea
--- /dev/null
+++ b/servo_controller/src/adc.h
@@ -0,0 +1,13 @@
+#ifndef ADC_H
+#define ADC_H
+
+
+#define MUX_CHANNELS 16
+#define NUM_ADC_CHANNELS 18
+
+
+int adc_init_all();
+int adc_read_id(int id);
+
+
+#endif // ADC_H
diff --git a/servo_controller/src/command_handler.c b/servo_controller/src/command_handler.c
index ffb688c..e58d342 100644
--- a/servo_controller/src/command_handler.c
+++ b/servo_controller/src/command_handler.c
@@ -1,5 +1,9 @@
 #include "command_handler.h"
+#include "command_message.h"
 #include "led.h"
+#include "servo.h"
+#include "adc.h"
+#include "usb.h"
 
 #include <zephyr/logging/log.h>
 
@@ -37,6 +41,53 @@ int command_handler(struct command_message_t *msg) {
 			break;
 		}
 
+		case SERVO_SET: {
+			// Set SERVO
+			int servo = msg->data[0];
+			float angle;
+			memcpy(&angle, &msg->data[1], sizeof(float));
+
+			servo_set_angle(servo, angle);
+
+			break;
+		}
+
+		case SERVO_SET_ALL: {
+			// Set ALL SERVO
+			float angles[NUM_SERVO_CHANNELS];
+			for (int i = 0; i < NUM_SERVO_CHANNELS; i++) {
+				int start = i*4;
+				memcpy(&angles[i], &msg->data[start], sizeof(float));
+			}
+
+			servo_set_all_angles(angles);
+
+			break;
+		}
+
+		case ADC_READ: {
+			int channel = msg->data[0];
+			int value = adc_read_id(channel);
+
+			struct command_message_t *reply = usb_get_next_tx_buf();
+			command_create_message(reply, sizeof(value), ADC_READ, (uint8_t *)&value);
+			usb_send_command(reply);
+
+			break;
+		}
+		case ADC_READ_ALL: {
+			int values[NUM_ADC_CHANNELS];
+			for (int i = 0; i < NUM_ADC_CHANNELS; i++) {
+				values[i] = adc_read_id(i);
+			}
+
+			struct command_message_t *reply = usb_get_next_tx_buf();
+			command_create_message(reply, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
+			usb_send_command(reply);
+			
+			break;
+		}
+
 		default: {
 			LOG_WRN("Unknown command received: %d", msg->command);
 			return -EINVAL;
diff --git a/servo_controller/src/command_message.c b/servo_controller/src/command_message.c
index 2e175cc..169a421 100644
--- a/servo_controller/src/command_message.c
+++ b/servo_controller/src/command_message.c
@@ -11,7 +11,7 @@ void command_message_init(struct command_message_t *msg) {
 	msg->id = COMMAND_ID;
 }
 
-void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[160]) {
+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;
diff --git a/servo_controller/src/command_message.h b/servo_controller/src/command_message.h
index 4d6a6ac..c7fa389 100644
--- a/servo_controller/src/command_message.h
+++ b/servo_controller/src/command_message.h
@@ -8,12 +8,20 @@
 #define COMMAND_PREFIX 0x69
 #define COMMAND_ID 0x00
 #define COMMAND_DATA_SIZE 160
+#define COMMAND_HEADER_SIZE 5
 
 typedef enum {
 	COMMAND_ACK,
 	COMMAND_NACK,
 	LED_TOGGLE,
 	LED_SET,
+	SERVO_SET,
+	SERVO_SET_ALL,
+	ADC_READ,
+	ADC_READ_ALL,
+
+	// Keep last
+	NUM_COMMANDS,
 } commands_e;
 
 struct command_message_t {
@@ -40,7 +48,7 @@ void command_message_init(struct command_message_t *msg);
  * @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[160]);
+void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[]);
 
 /**
  * @brief Calculate CRC for command message
diff --git a/servo_controller/src/main.c b/servo_controller/src/main.c
index 0f6fd28..596d41f 100644
--- a/servo_controller/src/main.c
+++ b/servo_controller/src/main.c
@@ -1,5 +1,7 @@
 #include "led.h"
 #include "usb.h"
+#include "servo.h"
+#include "adc.h"
 
 #include <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
@@ -8,17 +10,33 @@ LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
 int main(void) {
 	int ret;
 
+	// USB init
 	ret = usb_init();
 	if (ret != 0) {
 		LOG_ERR("Failed to enable USB");
 		return 0;
 	}
 
+	// LED init
 	ret = led_init();
 	if (ret != 0) {
 		LOG_ERR("Failed to enable LED");
 		return 0;
 	}
 
+	// ADC init before SERVO
+	ret = adc_init_all();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable ADC");
+		return 0;
+	}
+
+	// SERVO init
+	ret = servo_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable SERVO");
+		return 0;
+	}
+
 	return 0;
 }
\ No newline at end of file
diff --git a/servo_controller/src/servo.c b/servo_controller/src/servo.c
index b807ba6..c77a2ef 100644
--- a/servo_controller/src/servo.c
+++ b/servo_controller/src/servo.c
@@ -2,10 +2,14 @@
 
 #include <zephyr/logging/log.h>
 #include <zephyr/drivers/pwm.h>
+#include <zephyr/kernel.h>
+
+#include <math.h>
 
 LOG_MODULE_REGISTER(servo, LOG_LEVEL_INF);
 
-// ----------- SERVOS -----------
+
+// SERVOS
 static const struct pwm_dt_spec servo_pwm_specs[NUM_SERVO_CHANNELS] = {
 	PWM_DT_SPEC_GET(DT_ALIAS(servo1)),
 	PWM_DT_SPEC_GET(DT_ALIAS(servo2)),
@@ -26,12 +30,119 @@ static const struct pwm_dt_spec servo_pwm_specs[NUM_SERVO_CHANNELS] = {
 	PWM_DT_SPEC_GET(DT_ALIAS(servo17)),
 	PWM_DT_SPEC_GET(DT_ALIAS(servo18)),
 };
+static struct servo_t servos[NUM_SERVO_CHANNELS];
+
+// THREAD
+static struct k_thread servo_thread_data;
+static k_tid_t servo_thread_id = NULL;
+#define SERVO_THREAD_STACK_SIZE 2048
+K_THREAD_STACK_DEFINE(servo_thread_stack, SERVO_THREAD_STACK_SIZE);
+struct k_sem servo_semaphore;
 
 
-static int pin_init(const ) {
+static void servo_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+	
+	LOG_INF("Servo control thread started");
 
+	while (1) {
+		k_sem_take(&servo_semaphore, K_FOREVER);
+
+		// TODO: acceleration?
+
+		for (int i = 0; i < NUM_SERVO_CHANNELS; i++) {
+			servo_set(i, servos[i].set_position);
+			servos[i].current_position = servos[i].set_position;
+		}
+	}
 }
 
 int servo_init() {
+	int ret;
 
+	k_sem_init(&servo_semaphore, 0, 1);
+	memset(servos, 0, sizeof(servos));
+
+	for (int ch = 0; ch < NUM_SERVO_CHANNELS; ch++) {
+		if (!pwm_is_ready_dt(&servo_pwm_specs[ch])) {
+			LOG_ERR("PWM device for servo %d is not ready", ch + 1);
+			return -ENODEV;
+		}
+	}
+
+	for (int ch = 0; ch < NUM_SERVO_CHANNELS; ch++) {
+		ret = pwm_set_pulse_dt(&servo_pwm_specs[ch], PWM_USEC(0));
+		if (ret != 0) {
+			LOG_ERR("Failed to initialize PWM for servo %d: %d", ch + 1, ret);
+			return ret;
+		}
+	}
+
+	servo_thread_id = k_thread_create(
+		&servo_thread_data,
+		servo_thread_stack,
+		K_THREAD_STACK_SIZEOF(servo_thread_stack),
+		servo_thread,
+		NULL, NULL, NULL,
+		5,
+		0,
+		K_NO_WAIT
+	);
+
+	return 0;
+}
+
+void servo_set_angle(int channel, float angle) {
+	servos[channel].set_position = angle;
+	LOG_DBG("Settings servo %d to %f", channel, (double)angle);
+	k_sem_give(&servo_semaphore);
+}
+
+void servo_set_all_angles(float angles[NUM_SERVO_CHANNELS]) {
+	for (int i = 0; i < NUM_SERVO_CHANNELS; i++) {
+		servos[i].set_position = angles[i];
+		LOG_DBG("Settings servo %d to %f", i, (double)angles[i]);
+	}
+	k_sem_give(&servo_semaphore);
+}
+
+static uint32_t servo_angle_to_pulse_us(float angle) {
+	// Check limits
+	if (angle > SERVO_MAX_ANGLE) {
+		return SERVO_MAX_PULSE;
+	} else if (angle < SERVO_MIN_ANGLE) {
+		return SERVO_MIN_PULSE;
+	}
+
+	// Angle to pulse
+	float pulse_offset_us = angle * SERVO_ANGLE_PULSE_MULTIPLIER;
+	uint32_t pulse_us = (uint32_t)(SERVO_CENTER_PULSE + pulse_offset_us);
+
+	if (pulse_us < SERVO_MIN_PULSE) {
+		return SERVO_MIN_PULSE;
+	} else if (pulse_us > SERVO_MAX_PULSE) {
+		return SERVO_MAX_PULSE;
+	}
+
+	return pulse_us;
+}
+
+int servo_set(int channel, float angle) {
+	if (channel >= NUM_SERVO_CHANNELS || channel < 0) {
+		LOG_ERR("Servo channel exceeds NUM_SERVO_CHANNELS");
+		return -EINVAL;
+	}
+
+	float effective_angle = angle;
+
+	uint32_t pulse_us = servo_angle_to_pulse_us(effective_angle);
+	int ret = pwm_set_pulse_dt(&servo_pwm_specs[channel], PWM_USEC(pulse_us));
+	if (ret != 0) {
+		LOG_ERR("Failed to set PWM pulse for servo %d: %d", channel + 1, ret);
+		return ret;
+	}
+
+	return 0;
 }
\ No newline at end of file
diff --git a/servo_controller/src/servo.h b/servo_controller/src/servo.h
index da6a805..36538a2 100644
--- a/servo_controller/src/servo.h
+++ b/servo_controller/src/servo.h
@@ -2,12 +2,26 @@
 #define SERVO_H
 
 
-
 #define NUM_SERVO_CHANNELS 18
+#define SERVO_CENTER_PULSE 1500
+#define SERVO_MIN_PULSE 750
+#define SERVO_MAX_PULSE 2500
+#define SERVO_HALF_PULSE_RANGE 875
+#define SERVO_MAX_ANGLE 2.3561944902f
+#define SERVO_MIN_ANGLE -2.3561944902f
+#define SERVO_ANGLE_PULSE_MULTIPLIER 371.3615338798826f
 
 
+struct servo_t {
+	float current_position;
+	float set_position;
+};
+
 
 int servo_init();
+void servo_set_angle(int channel, float angle);
+void servo_set_all_angles(float angles[NUM_SERVO_CHANNELS]);
+int servo_set(int channel, float angle);
 
 
 #endif // SERVO_H
\ No newline at end of file
diff --git a/servo_controller/src/usb.c b/servo_controller/src/usb.c
index f16bba2..3cd84bd 100644
--- a/servo_controller/src/usb.c
+++ b/servo_controller/src/usb.c
@@ -17,11 +17,11 @@ LOG_MODULE_REGISTER(usb, LOG_LEVEL_INF);
 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 THREAD
+static struct k_thread usb_rx_thread_data;
+static k_tid_t usb_rx_thread_id = NULL;
+#define USB_RX_THREAD_STACK_SIZE 2048
+K_THREAD_STACK_DEFINE(usb_rx_thread_stack, USB_RX_THREAD_STACK_SIZE);
 
 // RX BUFFER
 #define RING_BUF_SIZE 255
@@ -29,10 +29,21 @@ static uint8_t ring_buffer[RING_BUF_SIZE];
 static struct ring_buf ringbuf;
 struct k_sem rx_semaphore;
 
+// TX THREAD
+static struct k_thread usb_tx_thread_data;
+static k_tid_t usb_tx_thread_id = NULL;
+#define USB_TX_THREAD_STACK_SIZE 512
+K_THREAD_STACK_DEFINE(usb_tx_thread_stack, USB_TX_THREAD_STACK_SIZE);
+
+// TX BUFFER (add ack and nack at the and as static)
+#define TX_BUFFER_SIZE 10
+struct command_message_t usb_tx_buffer[TX_BUFFER_SIZE + 2];
+struct command_message_t *usb_tx_buf_ptr;
+char usb_tx_ptr_msgq_buffer[(TX_BUFFER_SIZE + 2) * sizeof(struct command_message_t *)];
+struct k_msgq usb_tx_ptr_msgq;
+
 // 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) {
@@ -73,7 +84,7 @@ static void interrupt_handler(const struct device *dev, void *user_data) {
 	}
 }
 
-static void usb_thread(void *p1, void *p2, void *p3) {
+static void usb_rx_thread(void *p1, void *p2, void *p3) {
 	ARG_UNUSED(p1);
 	ARG_UNUSED(p2);
 	ARG_UNUSED(p3);
@@ -109,7 +120,7 @@ static void usb_thread(void *p1, void *p2, void *p3) {
 					if (calculated_crc != msg.crc) {
 						if (RETURN_ACK) {
 							// Send NACK
-							usb_send_command(&nack_msg);
+							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
 						}
 						continue;
 					}
@@ -118,13 +129,13 @@ static void usb_thread(void *p1, void *p2, void *p3) {
 					if (ret == 0) {
 						if (RETURN_ACK) {
 							// Send ACK
-							usb_send_command(&ack_msg);
+							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE]);
 						}
 					}
 					else {
 						if (RETURN_ACK) {
 							// Send NACK
-							usb_send_command(&nack_msg);
+							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
 						}
 					}
 
@@ -162,12 +173,36 @@ static void usb_msg_cb(struct usbd_context *const ctx, const struct usbd_msg *ms
 	}
 }
 
+static void usb_tx_thread(void *p1, void *p2, void *p3) {
+	struct command_message_t *data;
+
+    while (1) {
+        k_msgq_get(&usb_tx_ptr_msgq, &data, K_FOREVER);
+
+		if (!device_is_ready(uart_dev)) {
+			// FIXME: Don't drop packages
+			continue;
+		}
+
+		// Message size: prefix + length + id + command + crc + data
+		size_t msg_size = COMMAND_HEADER_SIZE + data->length;
+		uint8_t *msg_bytes = (uint8_t *)data;
+
+		/* 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]);
+		}
+    }
+}
+
 int usb_init() {
 	ring_buf_init(&ringbuf, sizeof(ring_buffer), ring_buffer);
 	k_sem_init(&rx_semaphore, 0, 1);
+	usb_tx_buf_ptr = usb_tx_buffer;
+	k_msgq_init(&usb_tx_ptr_msgq, usb_tx_ptr_msgq_buffer, sizeof(struct command_message_t *), TX_BUFFER_SIZE);
 
-	command_create_ack(&ack_msg);
-	command_create_nack(&nack_msg);
+	command_create_ack(&usb_tx_buffer[TX_BUFFER_SIZE]);
+	command_create_nack(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
 
 	int ret;
 
@@ -194,38 +229,59 @@ int usb_init() {
 	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,
+	// RX THREAD
+	usb_rx_thread_id = k_thread_create(
+		&usb_rx_thread_data,
+		usb_rx_thread_stack,
+		K_THREAD_STACK_SIZEOF(usb_rx_thread_stack),
+		usb_rx_thread,
 		NULL, NULL, NULL,
 		5,
 		0,
 		K_NO_WAIT
 	);
 
-	if (usb_thread_id == NULL) {
-		LOG_ERR("Failed to create USB thread");
+	if (usb_rx_thread_id == NULL) {
+		LOG_ERR("Failed to create USB RX thread");
+		return -ENOMEM;
+	}
+
+	// TX THREAD
+	usb_tx_thread_id = k_thread_create(
+		&usb_tx_thread_data,
+		usb_tx_thread_stack,
+		K_THREAD_STACK_SIZEOF(usb_tx_thread_stack),
+		usb_tx_thread,
+		NULL, NULL, NULL,
+		5,
+		0,
+		K_NO_WAIT
+	);
+
+	if (usb_tx_thread_id == NULL) {
+		LOG_ERR("Failed to create USB TX thread");
 		return -ENOMEM;
 	}
 
 	return ret;
 }
 
+struct command_message_t* usb_get_next_tx_buf() {
+	struct command_message_t *buf = usb_tx_buf_ptr;
+
+	// Increment the buffer pointer
+	usb_tx_buf_ptr++;
+
+	if (usb_tx_buf_ptr > &usb_tx_buffer[TX_BUFFER_SIZE-1]) {
+		usb_tx_buf_ptr = usb_tx_buffer;
+	}
+
+	return buf;
+}
+
 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]);
-	}
+	
+	k_msgq_put(&usb_tx_ptr_msgq, &msg, K_NO_WAIT);
 
 	return 0;
 }
\ No newline at end of file
diff --git a/servo_controller/src/usb.h b/servo_controller/src/usb.h
index 6ca1e6d..08b9a96 100644
--- a/servo_controller/src/usb.h
+++ b/servo_controller/src/usb.h
@@ -6,6 +6,7 @@
 
 
 int usb_init();
+struct command_message_t* usb_get_next_tx_buf();
 int usb_send_command(struct command_message_t *msg);