diff --git a/servo_controller/scripts/send_cmd.py b/servo_controller/scripts/send_cmd.py index d824622..8c58c4f 100644 --- a/servo_controller/scripts/send_cmd.py +++ b/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()} diff --git a/servo_controller/scripts/serial_logger.py b/servo_controller/scripts/serial_logger.py index aa6f041..6afe9a0 100644 --- a/servo_controller/scripts/serial_logger.py +++ b/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()} diff --git a/servo_controller/scripts/set_legs.py b/servo_controller/scripts/set_legs.py new file mode 100644 index 0000000..327c102 --- /dev/null +++ b/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 = " 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(" @@ -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; diff --git a/servo_controller/src/command_message.h b/servo_controller/src/command_message.h index 6f07e3a..a65fe24 100644 --- a/servo_controller/src/command_message.h +++ b/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, diff --git a/servo_controller/src/digital_in.c b/servo_controller/src/digital_in.c index fdd677e..1f047d0 100644 --- a/servo_controller/src/digital_in.c +++ b/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); diff --git a/servo_controller/src/digital_out.c b/servo_controller/src/digital_out.c new file mode 100644 index 0000000..2a679ad --- /dev/null +++ b/servo_controller/src/digital_out.c @@ -0,0 +1,61 @@ +#include "digital_out.h" + +#include +#include +#include + +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; +} \ No newline at end of file diff --git a/servo_controller/src/digital_out.h b/servo_controller/src/digital_out.h new file mode 100644 index 0000000..7594de3 --- /dev/null +++ b/servo_controller/src/digital_out.h @@ -0,0 +1,14 @@ +#ifndef DIGITAL_OUT_H +#define DIGITAL_OUT_H + + +#include + + +#define DIGITAL_OUT_LEG_CHANNELS 6 + +int digital_out_init(); +int digital_out_set_leg(uint8_t mask); + + +#endif // DIGITAL_OUT_H \ No newline at end of file diff --git a/servo_controller/src/main.c b/servo_controller/src/main.c index 4052444..7030421 100644 --- a/servo_controller/src/main.c +++ b/servo_controller/src/main.c @@ -3,6 +3,7 @@ #include "servo.h" #include "adc.h" #include "digital_in.h" +#include "digital_out.h" #include 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; } \ No newline at end of file