First implementation of digital out for leg enable

servo_controller/scripts/send_cmd.py

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

servo_controller/scripts/serial_logger.py

@@ -49,6 +49,7 @@ COMMANDS = {
 	"ADC_READ_ALL": 9,
 	"ADC_SET_READ": 10,
 	"DIGITAL_IN": 11,
+	"DIGITAL_LEG_OUT": 12,
 }
 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/command_handler.c

@@ -4,6 +4,7 @@
 #include "servo.h"
 #include "adc.h"
 #include "usb.h"
+#include "digital_out.h"
 
 #include <zephyr/logging/log.h>
 
@@ -114,6 +115,14 @@ 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;
+		}
+
 		default: {
 			LOG_WRN("Unknown command received: %d", msg->command);
 			return -EINVAL;

servo_controller/src/command_message.h

@@ -23,6 +23,7 @@ typedef enum {
 	ADC_READ_ALL,
 	ADC_SET_READ,
 	DIGITAL_IN,
+	DIGITAL_LEG_OUT,
 
 	// Keep last
 	NUM_COMMANDS,

servo_controller/src/digital_in.c

@@ -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,7 @@
 #include "servo.h"
 #include "adc.h"
 #include "digital_in.h"
+#include "digital_out.h"
 
 #include <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
@@ -46,5 +47,12 @@ int main(void) {
 		return 0;
 	}
 
+	// DIGITAL OUT init
+	ret = digital_out_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to enable DIGITAL OUT");
+		return 0;
+	}
+
 	return 0;
 }