Updated project plan

servo_controller/app.overlay

@@ -0,0 +1,9 @@
+&i2c0 {
+	power_monitor: ina3221@40 {
+		compatible = "ti,ina3221";
+		reg = <0x40>;
+		status = "okay";
+
+		shunt-resistors = <100>, <100>, <100>;
+	};
+};

servo_controller/prj.conf

@@ -1,6 +1,11 @@
 CONFIG_GPIO=y
 CONFIG_PWM=y
 CONFIG_ADC=y
+CONFIG_I2C=y
+
+# INA3221
+CONFIG_SENSOR=y
+CONFIG_INA3221=y
 
 # Mux
 CONFIG_CD74HC4067=y
@@ -33,5 +38,5 @@ CONFIG_LOG_MODE_IMMEDIATE=y
 CONFIG_DEBUG_THREAD_INFO=y
 # CONFIG_DEBUG=y
 # CONFIG_DEBUG_OPTIMIZATIONS=y
-CONFIG_OUTPUT_DISASSEMBLY=n		# Create disassmebly files
+# CONFIG_OUTPUT_DISASSEMBLY=y		# Create disassmebly files
 #CONFIG_OUTPUT_DISASSEMBLE_ALL=n

servo_controller/scripts/plot_channels.py

@@ -1,135 +1,366 @@
 #!/usr/bin/env python3
-"""Plot packed 4-byte-integer channel data from a serial_logger.py CSV, dark mode.
+"""Plot decoded command payloads from a serial_logger.py CSV, dark mode.
 
-Expects rows where `data_hex` is N 4-byte little-endian integers packed back
-to back (default N=18, matching an 18-channel ADC_READ_ALL-style packet).
-X-axis is the device-clock `tick_ms` column by default (or host_time).
+Each command gets its own subplot, stacked vertically in one window, in the
+order given by --commands (default: ADC_READ_ALL on top, POWER_MONITOR_READ
+below it). X-axis is the device-clock `tick_ms` column by default, or
+host_time.
+
+--------------------------------------------------------------------------
+TO ADD OR CHANGE A DATA FORMAT: edit the FORMATS dict below. Each entry is
+a DataFormat(...) describing how to turn that command's raw data_hex bytes
+into a list of (label, value) channels for one subplot. See the two
+existing entries for examples: a plain packed-int array (ADC_READ_ALL) and
+a decoded struct with scaling (POWER_MONITOR_READ, Zephyr sensor_value).
+--------------------------------------------------------------------------
 """
 
 import argparse
 import csv
 import struct
 import sys
+from dataclasses import dataclass, field, replace
 from datetime import datetime
+from typing import Callable, List, Sequence
 
 import matplotlib
 import matplotlib.pyplot as plt
 import matplotlib.dates as mdates
 
 
-def build_parser():
-	p = argparse.ArgumentParser(
-		description="Plot packed 4-byte-integer channels from a serial_logger.py CSV (dark mode).",
-		formatter_class=argparse.RawDescriptionHelpFormatter,
-		epilog="""\
-examples:
-  %(prog)s -i adc_log.csv
-  %(prog)s -i adc_log.csv --command ADC_READ_ALL -o plot.png
-  %(prog)s -i adc_log.csv --channels 18 --unsigned --x host
-""",
+# --------------------------------------------------------------------------
+# Data formats: command name -> how to decode its payload for plotting.
+# This is the "field" to edit when a new command/payload format shows up.
+# --------------------------------------------------------------------------
+
+@dataclass
+class DataFormat:
+	byte_length: int                        # expected payload length in bytes
+	decode: Callable[[bytes], Sequence[float]]  # raw payload -> one float per channel
+	labels: List[str]                       # one label per decoded value, same order as decode()
+	ylabel: str = "value"
+	title: str = ""                         # subplot title; defaults to the command name
+	# Optional: if the labels are `group_count` groups of `group_size` each
+	# (e.g. 3 power channels x 3 metrics, grouped channel-major), the plot
+	# uses one color per group and a different line style per item within
+	# a group, instead of a flat colormap over every label.
+	group_count: int = 0
+	group_size: int = 0
+
+
+def decode_int32_array(n: int, signed: bool = True) -> Callable[[bytes], Sequence[float]]:
+	"""N packed little-endian 4-byte integers, back to back."""
+	fmt = f"<{n}{'i' if signed else 'I'}"
+
+	def _decode(payload: bytes) -> Sequence[float]:
+		return struct.unpack(fmt, payload)
+
+	return _decode
+
+
+def decode_sensor_value_grid(num_channels: int, metrics: List[str]) -> Callable[[bytes], Sequence[float]]:
+	"""Decode `struct sensor_value values[num_channels][len(metrics)]`,
+	memcpy'd channel-major (matches `values[0][0], values[0][1], ...,
+	values[1][0], ...` in the C code). Each sensor_value is
+	`{int32_t val1; int32_t val2;}`; real value = val1 + val2 * 1e-6
+	(val2 is the fractional part, in millionths)."""
+	n = num_channels * len(metrics)
+
+	def _decode(payload: bytes) -> Sequence[float]:
+		raw = struct.unpack(f"<{n * 2}i", payload)
+		return [raw[2 * i] + raw[2 * i + 1] * 1e-6 for i in range(n)]
+
+	return _decode
+
+
+def grid_labels(num_channels: int, metrics: List[str]) -> List[str]:
+	return [f"Ch{ch + 1} {metric}" for ch in range(num_channels) for metric in metrics]
+
+
+POWER_MONITOR_METRICS = ["Voltage (V)", "Current (A)", "Power (W)"]
+POWER_MONITOR_CHANNELS = 3
+
+FORMATS = {
+	"ADC_READ_ALL": DataFormat(
+		byte_length=18 * 4,
+		decode=decode_int32_array(18, signed=True),
+		labels=[f"angle{i}" for i in range(18)],
+		ylabel="value (int32)",
+		title="Angles",
+	),
+	"POWER_MONITOR_READ": DataFormat(
+		# 3 channels x 3 sensor_value structs x 8 bytes each (int32 val1 + int32 val2)
+		byte_length=POWER_MONITOR_CHANNELS * len(POWER_MONITOR_METRICS) * 8,
+		decode=decode_sensor_value_grid(POWER_MONITOR_CHANNELS, POWER_MONITOR_METRICS),
+		labels=grid_labels(POWER_MONITOR_CHANNELS, POWER_MONITOR_METRICS),
+		ylabel="value",
+		title="Power Monitor",
+		group_count=POWER_MONITOR_CHANNELS,
+		group_size=len(POWER_MONITOR_METRICS),
+	),
+}
+
+DEFAULT_COMMANDS = ["ADC_READ_ALL", "POWER_MONITOR_READ"]
+
+
+def fallback_format(byte_length: int, signed: bool = True) -> DataFormat:
+	"""Used for a --commands entry with no FORMATS registry entry: assume a
+	plain packed int32 array and size it from the data actually seen."""
+	n = byte_length // 4
+	return DataFormat(
+		byte_length=byte_length,
+		decode=decode_int32_array(n, signed=signed),
+		labels=[f"ch{i}" for i in range(n)],
+		ylabel="value (int32)" if signed else "value (uint32)",
 	)
 
-	p.add_argument("-i", "--input", required=True, help="input CSV file (from serial_logger.py)")
-	p.add_argument("-o", "--output", help="save the plot to this file instead of showing it interactively")
 
-	p.add_argument("-c", "--command", help="only use rows with this command name (default: use whatever "
-	                                        "PACKET rows have the right byte length)")
-	p.add_argument("-n", "--channels", type=int, default=18,
-	               help="number of 4-byte integer channels packed in data_hex (default: %(default)s)")
-	p.add_argument("--signed", dest="signed", action="store_true", default=True,
-	               help="interpret channels as signed int32 (default: on)")
-	p.add_argument("--unsigned", dest="signed", action="store_false",
-	               help="interpret channels as unsigned uint32")
+# --------------------------------------------------------------------------
+# CSV loading
+# --------------------------------------------------------------------------
 
-	p.add_argument("--x", choices=["tick", "host"], default="tick",
-	               help="x-axis source: device tick_ms, or host_time (default: %(default)s)")
-	p.add_argument("--labels", help="comma-separated channel labels (default: ch0..chN-1)")
-	p.add_argument("--title", default="Channel data", help="plot title (default: %(default)s)")
-
-	return p
-
-
-def load_rows(path, command, channel_bytes):
-	"""Read the CSV and return rows that look like packed-channel PACKET rows."""
+def load_rows(path: str, command: str):
 	rows = []
 	with open(path, newline="", encoding="utf-8") as fh:
 		reader = csv.DictReader(fh)
 		for row in reader:
 			if row.get("type") != "PACKET":
 				continue
-			if command and row.get("command") != command:
-				continue
-			data_hex = row.get("data_hex", "")
-			if len(data_hex) != channel_bytes * 2:  # 2 hex chars per byte
+			if row.get("command") != command:
 				continue
 			rows.append(row)
 	return rows
 
 
+def build_series(rows, fmt: DataFormat, x_source: str):
+	xs = []
+	channels = [[] for _ in fmt.labels]
+
+	for row in rows:
+		data_hex = row.get("data_hex", "")
+		if len(data_hex) != fmt.byte_length * 2:  # 2 hex chars per byte
+			continue  # skip rows that don't match this format's expected size
+
+		if x_source == "tick":
+			xs.append(int(row["tick_ms"]))
+		else:
+			xs.append(datetime.fromisoformat(row["host_time"]))
+
+		values = fmt.decode(bytes.fromhex(data_hex))
+		for i, v in enumerate(values):
+			channels[i].append(v)
+
+	if not xs:
+		return xs, channels
+
+	order = sorted(range(len(xs)), key=lambda i: xs[i])
+	xs = [xs[i] for i in order]
+	channels = [[ch[i] for i in order] for ch in channels]
+	return xs, channels
+
+
+# --------------------------------------------------------------------------
+# Plotting
+# --------------------------------------------------------------------------
+
+LINESTYLES = ["-", "--", ":", "-."]
+
+
+def plot_subplot(ax, xs, channels, fmt: DataFormat, command: str, x_source: str):
+	if fmt.group_count and fmt.group_size and fmt.group_count * fmt.group_size == len(fmt.labels):
+		# One color per group (e.g. per channel), one line style per item
+		# within the group (e.g. per metric), so related lines are easy
+		# to tell apart at a glance instead of 9 near-identical hues.
+		# Index the discrete tab10 swatches directly (not .resampled(),
+		# which interpolates between them and can produce near-duplicate
+		# muddy colors for small group counts).
+		tab10 = matplotlib.colormaps["tab10"].colors
+		for i, label in enumerate(fmt.labels):
+			g, m = divmod(i, fmt.group_size)
+			ax.plot(xs, channels[i], label=label, color=tab10[g % len(tab10)],
+			        linestyle=LINESTYLES[m % len(LINESTYLES)], linewidth=1.3)
+	else:
+		cmap = matplotlib.colormaps["tab20"].resampled(max(len(fmt.labels), 1))
+		for i, label in enumerate(fmt.labels):
+			ax.plot(xs, channels[i], label=label, color=cmap(i), linewidth=1.2)
+
+	ax.set_title(fmt.title or command, fontsize=12, color="white", loc="left")
+	ax.set_ylabel(fmt.ylabel)
+	ax.grid(True, alpha=0.25)
+
+	if x_source == "host":
+		ax.xaxis.set_major_formatter(mdates.DateFormatter("%H:%M:%S"))
+
+	if fmt.group_count and fmt.group_size and fmt.group_count * fmt.group_size == len(fmt.labels):
+		ncol = fmt.group_size  # one column per metric, one row per channel/group
+	else:
+		ncol = min(6, max(len(fmt.labels), 1))
+	ax.legend(ncol=ncol, fontsize=8, loc="upper center",
+	          bbox_to_anchor=(0.5, -0.15), frameon=False)
+
+
+def parse_index_list(text: str) -> List[int]:
+	"""'0,2,5' or '0-3,7' -> [0,2,5] / [0,1,2,3,7]."""
+	indices = []
+	for part in text.split(","):
+		part = part.strip()
+		if not part:
+			continue
+		if "-" in part:
+			a, b = part.split("-", 1)
+			indices.extend(range(int(a), int(b) + 1))
+		else:
+			indices.append(int(part))
+	return indices
+
+
+def select_channels(fmt: DataFormat, channels, keep_indices: List[int]) -> "tuple[DataFormat, list]":
+	"""Return a (fmt, channels) pair restricted to keep_indices, preserving
+	group_count/group_size for the grouped coloring in plot_subplot if the
+	selection still divides evenly (e.g. picking a subset of metrics but
+	keeping every channel)."""
+	labels2 = [fmt.labels[i] for i in keep_indices]
+	channels2 = [channels[i] for i in keep_indices]
+
+	group_count, group_size = fmt.group_count, fmt.group_size
+	if group_count and group_size:
+		# If we kept the same subset of positions-within-group for every
+		# group (e.g. "voltage only" for all 3 channels), grouping still
+		# applies with a smaller group_size. Otherwise drop grouping.
+		per_group = [[] for _ in range(group_count)]
+		ok = True
+		for i in keep_indices:
+			g, m = divmod(i, group_size)
+			if g >= group_count:
+				ok = False
+				break
+			per_group[g].append(m)
+		if ok and len(set(tuple(p) for p in per_group if p)) <= 1 and all(per_group[0] == p for p in per_group if p):
+			group_size = len(per_group[0]) if per_group[0] else 0
+		else:
+			group_count, group_size = 0, 0
+	return replace(fmt, labels=labels2, group_count=group_count, group_size=group_size), channels2
+
+
+def build_parser():
+	p = argparse.ArgumentParser(
+		description="Plot decoded command payloads from a serial_logger.py CSV (dark mode).",
+		formatter_class=argparse.RawDescriptionHelpFormatter,
+		epilog="""\
+To add/change how a command's data is decoded, edit the FORMATS dict at the
+top of this script (see decode_int32_array / decode_power_monitor).
+
+examples:
+  %(prog)s -i log.csv
+  %(prog)s -i log.csv -o plot.png
+  %(prog)s -i log.csv --commands ADC_READ_ALL
+  %(prog)s -i log.csv --commands POWER_MONITOR_READ,ADC_READ_ALL --x host
+""",
+	)
+
+	p.add_argument("-i", "--input", required=True, help="input CSV file (from serial_logger.py)")
+	p.add_argument("-o", "--output", help="save the plot to this file instead of showing it interactively")
+
+	p.add_argument("--commands", default=",".join(DEFAULT_COMMANDS),
+	               help="comma-separated command names to plot, top to bottom "
+	                    "(default: %(default)s)")
+	p.add_argument("--x", choices=["tick", "host"], default="tick",
+	               help="x-axis source: device tick_ms, or host_time (default: %(default)s)")
+	p.add_argument("--title", default="Serial data", help="overall figure title (default: %(default)s)")
+
+	p.add_argument("--unsigned", action="store_true",
+	               help="for a command with no FORMATS entry, decode its ints as unsigned "
+	                    "(default: signed)")
+
+	p.add_argument("--adc-channels",
+	               help="only plot these ADC_READ_ALL channel indices, e.g. '0,2,5' or '0-3,7' "
+	                    "(default: all)")
+
+	power_group = p.add_argument_group(
+		"power monitor metrics",
+		"which POWER_MONITOR_READ metrics to plot, for every channel (default: all three)")
+	power_group.add_argument("--voltage", action="store_true", help="show voltage")
+	power_group.add_argument("--current", action="store_true", help="show current")
+	power_group.add_argument("--power", action="store_true", help="show power")
+
+	return p
+
+
 def main():
 	parser = build_parser()
 	args = parser.parse_args()
 
-	channel_bytes = args.channels * 4
+	commands = [c.strip() for c in args.commands.split(",") if c.strip()]
+	if not commands:
+		parser.error("--commands gave no command names")
 
-	rows = load_rows(args.input, args.command, channel_bytes)
+	panels = []  # (command, fmt, xs, channels)
+	for command in commands:
+		rows = load_rows(args.input, command)
 		if not rows:
-		hint = f" with command={args.command!r}" if args.command else ""
-		print(f"ERROR: no PACKET rows{hint} with {channel_bytes}-byte payloads "
-		      f"({args.channels} x 4-byte ints) found in {args.input}", file=sys.stderr)
+			print(f"WARNING: no PACKET rows for command={command!r} in {args.input}, skipping", file=sys.stderr)
+			continue
+
+		fmt = FORMATS.get(command)
+		if fmt is None:
+			# No registry entry: fall back to a plain packed-int32 array,
+			# sized from whatever payload length actually shows up.
+			byte_length = len(rows[0]["data_hex"]) // 2
+			fmt = fallback_format(byte_length, signed=not args.unsigned)
+			print(f"NOTE: {command!r} has no FORMATS entry, treating its {byte_length}-byte "
+			      f"payload as {byte_length // 4} packed int32 values. Add a FORMATS entry "
+			      f"for proper decoding.", file=sys.stderr)
+
+		xs, channels = build_series(rows, fmt, args.x)
+		if not xs:
+			print(f"WARNING: rows for command={command!r} didn't match the expected "
+			      f"{fmt.byte_length}-byte payload, skipping", file=sys.stderr)
+			continue
+
+		if command == "ADC_READ_ALL" and args.adc_channels:
+			try:
+				keep = parse_index_list(args.adc_channels)
+			except ValueError:
+				parser.error(f"--adc-channels: couldn't parse {args.adc_channels!r}")
+			bad = [i for i in keep if not (0 <= i < len(fmt.labels))]
+			if bad:
+				parser.error(f"--adc-channels: index out of range (0-{len(fmt.labels) - 1}): {bad}")
+			fmt, channels = select_channels(fmt, channels, keep)
+
+		if command == "POWER_MONITOR_READ" and fmt.group_count and fmt.group_size:
+			selected_metrics = [m for flag, m in
+			                     [(args.voltage, "Voltage"), (args.current, "Current"), (args.power, "Power")]
+			                     if flag]
+			if selected_metrics:
+				keep = [i for i, label in enumerate(fmt.labels)
+				        if any(m in label for m in selected_metrics)]
+				fmt, channels = select_channels(fmt, channels, keep)
+
+		panels.append((command, fmt, xs, channels))
+
+	if not panels:
+		print("ERROR: nothing to plot", file=sys.stderr)
 		return 1
 
-	fmt = f"<{args.channels}{'i' if args.signed else 'I'}"
-
-	xs = []
-	channels = [[] for _ in range(args.channels)]
-
-	for row in rows:
-		if args.x == "tick":
-			xs.append(int(row["tick_ms"]))
-		else:
-			xs.append(datetime.fromisoformat(row["host_time"]))
-
-		values = struct.unpack(fmt, bytes.fromhex(row["data_hex"]))
-		for i, v in enumerate(values):
-			channels[i].append(v)
-
-	# sort chronologically, just in case the file wasn't strictly ordered
-	order = sorted(range(len(xs)), key=lambda i: xs[i])
-	xs = [xs[i] for i in order]
-	channels = [[ch[i] for i in order] for ch in channels]
-
-	if args.labels:
-		labels = [s.strip() for s in args.labels.split(",")]
-		if len(labels) != args.channels:
-			parser.error(f"--labels has {len(labels)} entries, expected {args.channels}")
-	else:
-		labels = [f"ch{i}" for i in range(args.channels)]
-
-	# --- dark mode plot ---
 	plt.style.use("dark_background")
-	fig, ax = plt.subplots(figsize=(13, 7))
+	fig, axes = plt.subplots(len(panels), 1, figsize=(13, 5 * len(panels)), sharex=True)
+	if len(panels) == 1:
+		axes = [axes]
 
-	cmap = matplotlib.colormaps["tab20"].resampled(args.channels)
-	for i in range(args.channels):
-		ax.plot(xs, channels[i], label=labels[i], color=cmap(i), linewidth=1.2)
-
-	ax.set_title(args.title, fontsize=14, color="white")
-	ax.set_xlabel("tick (ms)" if args.x == "tick" else "host time")
-	ax.set_ylabel("value" + (" (int32)" if args.signed else " (uint32)"))
-	ax.grid(True, alpha=0.25)
+	for ax, (command, fmt, xs, channels) in zip(axes, panels):
+		plot_subplot(ax, xs, channels, fmt, command, args.x)
 
+	axes[-1].set_xlabel("tick (ms)" if args.x == "tick" else "host time")
 	if args.x == "host":
 		fig.autofmt_xdate()
-		ax.xaxis.set_major_formatter(mdates.DateFormatter("%H:%M:%S"))
 
-	ax.legend(ncol=min(6, args.channels), fontsize=8, loc="upper center",
-	          bbox_to_anchor=(0.5, -0.12), frameon=False)
-	fig.tight_layout()
+	fig.suptitle(args.title, fontsize=15, color="white")
+	fig.tight_layout(rect=(0, 0, 1, 0.97))
 
 	if args.output:
 		fig.savefig(args.output, dpi=150, facecolor=fig.get_facecolor())
-		print(f"Saved plot to {args.output} ({len(xs)} samples, {args.channels} channels)")
+		summary = ", ".join(f"{c} ({len(xs)} samples)" for c, _, xs, _ in panels)
+		print(f"Saved plot to {args.output}: {summary}")
 	else:
 		plt.show()
 

servo_controller/scripts/send_cmd.py

@@ -45,6 +45,9 @@ COMMANDS = {
 	"ADC_READ_ALL": 9,
 	"ADC_SET_READ": 10,
 	"DIGITAL_IN": 11,
+	"DIGITAL_LEG_OUT": 12,
+	"POWER_MONITOR_SET_READ": 13,
+	"POWER_MONITOR_READ": 14,
 }
 COMMAND_NAMES = {v: k for k, v in COMMANDS.items()}
 

servo_controller/scripts/serial_logger.py

@@ -49,6 +49,9 @@ COMMANDS = {
 	"ADC_READ_ALL": 9,
 	"ADC_SET_READ": 10,
 	"DIGITAL_IN": 11,
+	"DIGITAL_LEG_OUT": 12,
+	"POWER_MONITOR_SET_READ": 13,
+	"POWER_MONITOR_READ": 14,
 }
 COMMAND_NAMES = {v: k for k, v in COMMANDS.items()}
 

servo_controller/scripts/set_legs.py

@@ -0,0 +1,137 @@
+#!/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
+DIGITAL_LEG_OUT = 12
+
+DEVICE_ID = 0
+
+# Header layout: prefix, length, id, command, tick(int64 LE), crc
+TICK_FORMAT = "<q"                         # signed 64-bit little-endian, milliseconds
+TICK_SIZE = struct.calcsize(TICK_FORMAT)   # 8
+CRC_OFFSET = 4 + TICK_SIZE                 # index of the crc byte -> 12
+HEADER_SIZE = CRC_OFFSET + 1               # bytes before data -> 13
+
+
+def calculate_crc(msg: bytes) -> int:
+	s = sum(msg) & 0xFF
+	return (-s) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"", tick: int = 0) -> bytes:
+	length = len(data)
+
+	pkt = bytearray()
+	pkt.append(COMMAND_PREFIX)
+	pkt.append(length)
+	pkt.append(DEVICE_ID)
+	pkt.append(command)
+	pkt.extend(struct.pack(TICK_FORMAT, tick))
+	pkt.append(0)			  # CRC placeholder
+	pkt.extend(data)
+
+	pkt[CRC_OFFSET] = calculate_crc(pkt[:CRC_OFFSET] + pkt[CRC_OFFSET + 1:])
+
+	return bytes(pkt)
+
+
+def verify_crc(packet: bytes) -> bool:
+	crc = packet[CRC_OFFSET]
+	calc = calculate_crc(packet[:CRC_OFFSET] + packet[CRC_OFFSET + 1:])
+	return crc == calc
+
+
+def packet_size(buf: bytes):
+	if len(buf) < 2:
+		return None
+	return HEADER_SIZE + buf[1]
+
+
+def unpack_tick(packet: bytes) -> int:
+	return struct.unpack(TICK_FORMAT, packet[4:4 + TICK_SIZE])[0]
+
+
+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]
+				tick = unpack_tick(pkt)
+
+				if cmd == COMMAND_ACK:
+					print(f"<-- ACK (device={dev_id}, tick={tick}ms)")
+
+				elif cmd == COMMAND_NACK:
+					print(f"<-- NACK (device={dev_id}, tick={tick}ms)")
+
+				else:
+					print(f"<-- Command {cmd} (tick={tick}ms) 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()
+
+	for i in range(0x3f):
+		data = struct.pack("<B", i)  # explicitly uint8_t, raises if i is out of 0-255 range
+
+		ser.write(make_packet(DIGITAL_LEG_OUT, data, tick=0))
+		time.sleep(0.1)
+
+	print("Done.")
+	time.sleep(2)
+
+
+if __name__ == "__main__":
+	main()

servo_controller/src/adc.c

@@ -1,6 +1,5 @@
 #include "adc.h"
-#include "command_message.h"
-#include "usb.h"
+#include "command_handler.h"
 #include "mux.h"
 
 #include <zephyr/logging/log.h>
@@ -36,6 +35,11 @@ struct k_msgq adc_mask_msgq;
 // ADC TIMER
 struct k_timer adc_timer;
 
+// ADC BUFFER
+#define ADC_MSG_BUFFER_SIZE 10
+struct command_message_t adc_msg_buffer[ADC_MSG_BUFFER_SIZE];
+struct command_message_t *adc_msg_buffer_ptr;
+
 
 static void adc_timer_handler(struct k_timer *timer) {
     // Timer trigger
@@ -68,9 +72,13 @@ static void adc_thread(void *p1, void *p2, void *p3) {
 		}
 		
 		// Send to USB
-		struct command_message_t *msg = usb_get_next_tx_buf();
-		command_create_message(msg, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
-		usb_send_command(msg);
+		command_create_message(adc_msg_buffer_ptr, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
+		command_handler_tx(adc_msg_buffer_ptr);
+		adc_msg_buffer_ptr++;
+
+		if (adc_msg_buffer_ptr > &adc_msg_buffer[ADC_MSG_BUFFER_SIZE-1]) {
+			adc_msg_buffer_ptr = adc_msg_buffer;
+		}
 	}
 }
 
@@ -83,6 +91,10 @@ int adc_init_all(void) {
 	// TIMER
 	k_timer_init(&adc_timer, adc_timer_handler, NULL);
 
+	// BUFFER
+	memset(adc_msg_buffer, 0, sizeof(adc_msg_buffer));
+	adc_msg_buffer_ptr = adc_msg_buffer;
+
 	// MUX init
 	ret = mux_init();
 	if (ret != 0) {
@@ -138,7 +150,6 @@ int adc_init_all(void) {
     return 0;
 }
 
-
 int adc_read_id(int id) {
 	int ret;
     int val_mv = 0;
@@ -211,3 +222,14 @@ int adc_timer_set(int interval_ms) {
 	
 	return 0;
 }
+
+void* adc_get_buffer() {
+	struct command_message_t *ptr = adc_msg_buffer_ptr;
+	ptr--;
+
+	if (ptr < &adc_msg_buffer[0]) {
+		ptr = &adc_msg_buffer[ADC_MSG_BUFFER_SIZE-1];
+	}
+
+	return (void *)ptr;
+}

servo_controller/src/adc.h

@@ -12,6 +12,7 @@ int adc_init_all();
 int adc_read_id(int id);
 int adc_add_mask(uint32_t mask);
 int adc_timer_set(int interval_ms);
+void* adc_get_buffer();
 
 
 #endif // ADC_H

servo_controller/src/command_handler.c

@@ -4,12 +4,33 @@
 #include "servo.h"
 #include "adc.h"
 #include "usb.h"
+#include "digital_out.h"
+#include "power_monitor.h"
 
 #include <zephyr/logging/log.h>
 
 LOG_MODULE_REGISTER(command_handler, LOG_LEVEL_INF);
 
-int command_handler(struct command_message_t *msg) {
+
+// BUFFER (add ack and nack at the and as static)
+#define CMD_MSG_BUFFER_SIZE 10
+struct command_message_t cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 2];
+struct command_message_t *cmd_msg_buffer_ptr;
+
+
+int command_handler_init() {
+	// BUFFER
+	memset(cmd_msg_buffer, 0, sizeof(cmd_msg_buffer));
+	cmd_msg_buffer_ptr = cmd_msg_buffer;
+
+	// CREATE ACK/NACK
+	command_create_ack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE]);
+	command_create_nack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 1]);
+
+	return 0;
+}
+
+int command_handler_rx(struct command_message_t *msg) {
 	if (msg == NULL) {
 		LOG_ERR("Received NULL message pointer");
 		return -EINVAL;
@@ -69,9 +90,9 @@ int command_handler(struct command_message_t *msg) {
 			int channel = msg->data[0];
 			int value = adc_read_id(channel);
 
-			struct command_message_t *reply = usb_get_next_tx_buf();
+			struct command_message_t *reply = cmd_get_next_tx_buf();
 			command_create_message(reply, sizeof(value), msg->command, (uint8_t *)&value);
-			usb_send_command(reply);
+			command_handler_tx(reply);
 
 			break;
 		}
@@ -81,9 +102,9 @@ int command_handler(struct command_message_t *msg) {
 				values[i] = adc_read_id(i);
 			}
 
-			struct command_message_t *reply = usb_get_next_tx_buf();
+			struct command_message_t *reply = cmd_get_next_tx_buf();
 			command_create_message(reply, sizeof(values), msg->command, (uint8_t *)values);
-			usb_send_command(reply);
+			command_handler_tx(reply);
 			
 			break;
 		}
@@ -98,7 +119,7 @@ int command_handler(struct command_message_t *msg) {
 		}
 
 		case ADC_READ_ALL: {
-			uint32_t mask = 0b1111111111111111;
+			uint32_t mask = 0b111111111111111111;
 
 			adc_add_mask(mask);
 			
@@ -114,6 +135,23 @@ int command_handler(struct command_message_t *msg) {
 			break;
 		}
 
+		case DIGITAL_LEG_OUT: {
+			uint8_t mask = msg->data[0];
+
+			digital_out_set_leg(mask);
+			
+			break;
+		}
+
+		case POWER_MONITOR_SET_READ: {
+			int interval_ms;
+			memcpy(&interval_ms, &msg->data[0], sizeof(int));
+
+			power_monitor_timer_set(interval_ms);
+			
+			break;
+		}
+
 		default: {
 			LOG_WRN("Unknown command received: %d", msg->command);
 			return -EINVAL;
@@ -122,3 +160,26 @@ int command_handler(struct command_message_t *msg) {
 
 	return 0;
 }
+
+struct command_message_t* cmd_get_next_tx_buf() {
+	struct command_message_t *buf = cmd_msg_buffer_ptr;
+
+	// Increment the buffer pointer
+	cmd_msg_buffer_ptr++;
+
+	if (cmd_msg_buffer_ptr > &cmd_msg_buffer[CMD_MSG_BUFFER_SIZE-1]) {
+		cmd_msg_buffer_ptr = cmd_msg_buffer;
+	}
+
+	return buf;
+}
+
+int command_handler_tx(struct command_message_t *msg) {
+
+	// Send over USB if enabled and ready
+	if (usb_is_tx_enabled()) {
+		usb_send_command(msg);
+	}
+
+	return 0;
+}

servo_controller/src/command_handler.h

@@ -4,13 +4,10 @@
 
 #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);
+int command_handler_init();
+int command_handler_rx(struct command_message_t *msg);
+struct command_message_t* cmd_get_next_tx_buf();
+int command_handler_tx(struct command_message_t *msg);
 
 
 #endif // COMMAND_HANDLER_H

servo_controller/src/command_message.h

@@ -23,6 +23,9 @@ typedef enum {
 	ADC_READ_ALL,
 	ADC_SET_READ,
 	DIGITAL_IN,
+	DIGITAL_LEG_OUT,
+	POWER_MONITOR_SET_READ,
+	POWER_MONITOR_READ,
 
 	// Keep last
 	NUM_COMMANDS,
@@ -80,7 +83,7 @@ void command_create_nack(struct command_message_t *msg);
 /**
  * @brief Print the command with LOG
  * 
- * @param msg Message to calculate CRC for
+ * @param msg Message to LOG
  */
 void command_log(struct command_message_t *msg);
 

servo_controller/src/digital_in.c

@@ -1,5 +1,5 @@
 #include "digital_in.h"
-#include "usb.h"
+#include "command_handler.h"
 
 #include <zephyr/kernel.h>
 #include <zephyr/device.h>
@@ -20,9 +20,9 @@ static struct gpio_callback digital_in_cb_data;
 
 void digital_in_irq_handler(const struct device *dev, struct gpio_callback *cb, uint32_t pins) {
 	// TODO: what happens
-	struct command_message_t *msg = usb_get_next_tx_buf();
+	struct command_message_t *msg = cmd_get_next_tx_buf();
 	command_create_message(msg, sizeof(pins), DIGITAL_IN, (uint8_t *)&pins);
-	usb_send_command(msg);
+	command_handler_tx(msg);
 }
 
 int digital_in_pin_init(const struct gpio_dt_spec *gpio, uint32_t *bitmask) {
@@ -30,7 +30,7 @@ int digital_in_pin_init(const struct gpio_dt_spec *gpio, uint32_t *bitmask) {
 
 	if (!gpio_is_ready_dt(gpio)) {
 		LOG_ERR("GPIO device %s is not ready\n", gpio->port->name);
-		return -EBUSY;
+		return -ENODEV;
 	}
 
 	ret = gpio_pin_configure_dt(gpio, GPIO_INPUT);

servo_controller/src/digital_out.c

@@ -0,0 +1,61 @@
+#include "digital_out.h"
+
+#include <zephyr/devicetree.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/gpio.h>
+
+LOG_MODULE_REGISTER(digital_out, LOG_LEVEL_INF);
+
+
+#define DIGITAL_OUT_MACRO(node_id) GPIO_DT_SPEC_GET(node_id, gpios),
+
+static const struct gpio_dt_spec digital_leg_outputs[] = {
+    DT_FOREACH_CHILD(DT_NODELABEL(digital_outputs), DIGITAL_OUT_MACRO)
+};
+
+
+static int digital_out_pin_init(const struct gpio_dt_spec *gpio) {
+	int ret;
+
+	if (!device_is_ready(gpio->port)) {
+		LOG_ERR("GPIO device %s is not ready\n", gpio->port->name);
+		return -ENODEV;
+	}
+
+	ret = gpio_pin_configure_dt(gpio, GPIO_OUTPUT_INACTIVE);
+	if (ret != 0) {
+		LOG_ERR("Failed to configure %s pin %d\n", gpio->port->name, gpio->pin);
+		return ret;
+	}
+
+	// Turn it off
+	ret = gpio_pin_set_dt(gpio, 0);
+	if (ret != 0) {
+		LOG_ERR("Failed to initialize %s pin %d\n", gpio->port->name, gpio->pin);
+		return ret;
+	}
+
+	return 0;
+}
+
+int digital_out_init() {
+	// Initialize the LEG enable pins
+	for (int i = 0; i < DIGITAL_OUT_LEG_CHANNELS; i++) {
+		digital_out_pin_init(&digital_leg_outputs[i]);
+	}
+
+	return 0;
+}
+
+int digital_out_set_leg(uint8_t mask) {
+	for (int i = 0; i < DIGITAL_OUT_LEG_CHANNELS; i++) {
+		if ((mask >> i) & 0x1) {
+			gpio_pin_set_dt(&digital_leg_outputs[i], 1);
+		}
+		else {
+			gpio_pin_set_dt(&digital_leg_outputs[i], 0);
+		}
+	}
+
+	return 0;
+}

servo_controller/src/digital_out.h

@@ -0,0 +1,14 @@
+#ifndef DIGITAL_OUT_H
+#define DIGITAL_OUT_H
+
+
+#include <stdint.h>
+
+
+#define DIGITAL_OUT_LEG_CHANNELS 6
+
+int digital_out_init();
+int digital_out_set_leg(uint8_t mask);
+
+
+#endif // DIGITAL_OUT_H

servo_controller/src/main.c

@@ -3,6 +3,9 @@
 #include "servo.h"
 #include "adc.h"
 #include "digital_in.h"
+#include "digital_out.h"
+#include "power_monitor.h"
+#include "command_handler.h"
 
 #include <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
@@ -11,12 +14,20 @@ LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
 int main(void) {
 	int ret;
 
+	// COMMAND HANDLER init
+	ret = command_handler_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable COMMAND HANDLER");
+		return 0;
+	}
+
 	// USB init
 	ret = usb_init();
 	if (ret != 0) {
 		LOG_ERR("Failed to enable USB");
 		return 0;
 	}
+	usb_set_tx_state(1);
 
 	// LED init
 	ret = led_init();
@@ -46,5 +57,19 @@ int main(void) {
 		return 0;
 	}
 
+	// DIGITAL OUT init
+	ret = digital_out_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable DIGITAL OUT");
+		return 0;
+	}
+
+	// POWER MONITOR init
+	ret = power_monitor_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable POWER MONITOR");
+		return 0;
+	}
+
 	return 0;
 }

servo_controller/src/power_monitor.c

@@ -0,0 +1,133 @@
+#include "power_monitor.h"
+#include "command_handler.h"
+
+#include <zephyr/kernel.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/drivers/sensor.h>
+
+LOG_MODULE_REGISTER(power_monitor, LOG_LEVEL_INF);
+
+// DEVICE
+#define POWER_MONITOR_NODE DT_NODELABEL(power_monitor)
+const struct device *power_monitor_dt = DEVICE_DT_GET(POWER_MONITOR_NODE);
+
+
+// THREAD
+static struct k_thread power_monitor_thread_data;
+static k_tid_t power_monitor_thread_id = NULL;
+#define POWER_MONITOR_THREAD_STACK_SIZE 2048
+#define POWER_MONITOR_THREAD_PRIORITY 5
+K_THREAD_STACK_DEFINE(power_monitor_thread_stack, POWER_MONITOR_THREAD_STACK_SIZE);
+
+// TIMER
+struct k_timer power_monitor_timer;
+struct k_sem power_monitor_semaphore;
+
+// BUFFER
+#define PWR_MSG_BUFFER_SIZE 10
+struct command_message_t pwr_msg_buffer[PWR_MSG_BUFFER_SIZE];
+struct command_message_t *pwr_msg_buffer_ptr;
+
+
+static void power_monitor_timer_handler(struct k_timer *timer) {
+    // Timer trigger
+	k_sem_give(&power_monitor_semaphore);
+}
+
+static void power_monitor_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+
+	while (1) {
+		k_sem_take(&power_monitor_semaphore, K_FOREVER);
+
+        struct sensor_value values[3][3];
+        sensor_sample_fetch(power_monitor_dt);
+
+		// Read channel 1
+		struct sensor_value value = {1, 0};
+		sensor_attr_set(power_monitor_dt, SENSOR_CHAN_ALL, (SENSOR_ATTR_PRIV_START+1), &value);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_VOLTAGE, &values[0][0]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_CURRENT, &values[0][1]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_POWER, &values[0][2]);
+
+		// Read channel 2
+		value.val1 = 2;
+		sensor_attr_set(power_monitor_dt, SENSOR_CHAN_ALL, (SENSOR_ATTR_PRIV_START+1), &value);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_VOLTAGE, &values[1][0]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_CURRENT, &values[1][1]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_POWER, &values[1][2]);
+
+		// Read channel 3
+		value.val1 = 3;
+		sensor_attr_set(power_monitor_dt, SENSOR_CHAN_ALL, (SENSOR_ATTR_PRIV_START+1), &value);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_VOLTAGE, &values[2][0]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_CURRENT, &values[2][1]);
+        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_POWER, &values[2][2]);
+
+		// Send to USB
+		command_create_message(pwr_msg_buffer_ptr, sizeof(values), POWER_MONITOR_READ, (uint8_t *)values);
+		command_handler_tx(pwr_msg_buffer_ptr);
+		pwr_msg_buffer_ptr++;
+
+		if (pwr_msg_buffer_ptr > &pwr_msg_buffer[PWR_MSG_BUFFER_SIZE-1]) {
+			pwr_msg_buffer_ptr = pwr_msg_buffer;
+		}
+	}
+}
+
+int power_monitor_init() {
+
+	k_sem_init(&power_monitor_semaphore, 0, 1);
+	
+	// TIMER
+	k_timer_init(&power_monitor_timer, power_monitor_timer_handler, NULL);
+
+	// BUFFER
+	memset(pwr_msg_buffer, 0, sizeof(pwr_msg_buffer));
+	pwr_msg_buffer_ptr = pwr_msg_buffer;
+
+	// DEVICE
+    if (!device_is_ready(power_monitor_dt)) {
+        LOG_ERR("INA3221 not ready\n");
+        return 0;
+    }
+	
+	// THREAD
+	power_monitor_thread_id = k_thread_create(
+		&power_monitor_thread_data,
+		power_monitor_thread_stack,
+		K_THREAD_STACK_SIZEOF(power_monitor_thread_stack),
+		power_monitor_thread,
+		NULL, NULL, NULL,
+		POWER_MONITOR_THREAD_PRIORITY,
+		0,
+		K_NO_WAIT
+	);
+
+	return 0;
+}
+
+int power_monitor_timer_set(int interval_ms) {
+
+	if (interval_ms > 0) {
+		k_timer_start(&power_monitor_timer, K_MSEC(interval_ms), K_MSEC(interval_ms));
+	}
+	else {
+		k_timer_stop(&power_monitor_timer);
+	}
+	
+	return 0;
+}
+
+void* power_monitor_get_buffer() {
+	struct command_message_t *ptr = pwr_msg_buffer_ptr;
+	ptr--;
+
+	if (ptr < &pwr_msg_buffer[0]) {
+		ptr = &pwr_msg_buffer[PWR_MSG_BUFFER_SIZE-1];
+	}
+
+	return (void *)ptr;
+}

servo_controller/src/power_monitor.h

@@ -0,0 +1,10 @@
+#ifndef POWER_MONITOR_H
+#define POWER_MONITOR_H
+
+
+int power_monitor_init();
+int power_monitor_timer_set(int interval_ms);
+void* power_monitor_get_buffer();
+
+
+#endif // POWER_MONITOR_H

servo_controller/src/usb.c

@@ -16,6 +16,8 @@ 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;
+int usb_device_ready = 0;
+int usb_tx_enabled = 0;
 
 // RX THREAD
 static struct k_thread usb_rx_thread_data;
@@ -35,12 +37,12 @@ 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)
+// TX MSG BUFFER
 #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 *)];
+char usb_tx_ptr_msgq_buffer[(TX_BUFFER_SIZE) * sizeof(struct command_message_t *)];
 struct k_msgq usb_tx_ptr_msgq;
+struct command_message_t ack;
+struct command_message_t nack;
 
 // ACK / NACK messages
 #define RETURN_ACK true
@@ -121,28 +123,28 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) {
 					if (calculated_crc != msg.crc) {
 						if (RETURN_ACK) {
 							// Send NACK
-							usb_tx_buffer[TX_BUFFER_SIZE + 1].tick = k_uptime_get();
-							usb_tx_buffer[TX_BUFFER_SIZE + 1].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
-							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+							nack.tick = k_uptime_get();
+							nack.crc = command_calculate_crc(&nack);
+							usb_send_command(&nack);
 						}
 						continue;
 					}
 
-					int ret = command_handler(&msg);
+					int ret = command_handler_rx(&msg);
 					if (ret == 0) {
 						if (RETURN_ACK) {
 							// Send ACK
-							usb_tx_buffer[TX_BUFFER_SIZE].tick = k_uptime_get();
-							usb_tx_buffer[TX_BUFFER_SIZE].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE]);
-							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE]);
+							ack.tick = k_uptime_get();
+							ack.crc = command_calculate_crc(&ack);
+							usb_send_command(&ack);
 						}
 					}
 					else {
 						if (RETURN_ACK) {
 							// Send NACK
-							usb_tx_buffer[TX_BUFFER_SIZE + 1].tick = k_uptime_get();
-							usb_tx_buffer[TX_BUFFER_SIZE + 1].crc = command_calculate_crc(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
-							usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+							nack.tick = k_uptime_get();
+							nack.crc = command_calculate_crc(&nack);
+							usb_send_command(&nack);
 						}
 					}
 
@@ -205,11 +207,11 @@ static void usb_tx_thread(void *p1, void *p2, void *p3) {
 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(&usb_tx_buffer[TX_BUFFER_SIZE]);
-	command_create_nack(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
+	// CREATE ACK/NACK
+	command_create_ack(&ack);
+	command_create_nack(&nack);
 
 	int ret;
 
@@ -270,20 +272,17 @@ int usb_init() {
 		return -ENOMEM;
 	}
 
+	usb_device_ready = 1;
+
 	return ret;
 }
 
-struct command_message_t* usb_get_next_tx_buf() {
-	struct command_message_t *buf = usb_tx_buf_ptr;
+int usb_is_tx_enabled() {
+	return usb_tx_enabled && usb_device_ready;
+}
 
-	// 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;
+void usb_set_tx_state(int state) {
+	usb_tx_enabled = state ? 1 : 0;
 }
 
 int usb_send_command(struct command_message_t *msg) {

servo_controller/src/usb.h

@@ -6,7 +6,8 @@
 
 
 int usb_init();
-struct command_message_t* usb_get_next_tx_buf();
+int usb_is_tx_enabled();
+void usb_set_tx_state(int state);
 int usb_send_command(struct command_message_t *msg);