diff --git a/README.md b/README.md index b68fbbd..7cf8687 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,41 @@ # servo2350 -Custom PCB with RP2350. Mainly used for hexapods and driving servo motors. \ No newline at end of file +Custom PCB with RP2350. Mainly used for hexapods and driving 18x servo motors with 18x ADC feedback through multiplexer. + +![image](datasheets/board.jpg) + +## Specs + +* **CPU:** RP2350B with debug pins +* **FLASH:** W25Q16JVUXIQ_TR 16Mbit NOR quad SPI memory +* **OUTPUT:** 18x PWM, 6x digital output (3 pin header), 2x LEDs, extra pins in header +* **INPUT:** 18x ADC though a CD74HC4067 multiplexer +* **SENSORS:** ICM-45686 IMU +* **SERIAL:** SPI, I2C, USB, UART + +## Test code commands + +> [!WARNING] +> The zephyr sdk has bug in the [rp2350 pinctrl](https://github.com/zephyrproject-rtos/zephyr/blob/main/include/zephyr/dt-bindings/pinctrl/rpi-pico-rp2350-pinctrl-common.h) file. The `RP2_PINCTRL_GPIO_FUNC_UART_AUX` macro is defind, but `RP2_PINCTRL_GPIO_FUNC_UART_ALT` macro is used. + +> [!NOTE] +> While shell is over USB, the `adc scan_for` command uses uart. The UART uses spi1 pins, so the spi1 needs to be disabled and `extra_uart.overlay` needs to be used. + + +| Command | Parameters | Description | +| --------------------------- | ---------------------------- | --------------------------------------------------- | +| `led set ` | `id`: `0-1` | Set LED state | +| `led allon` | - | Turn all LEDs ON | +| `led alloff` | - | Turn all LEDs OFF | +| `servo set ` | Servo ID, angle in degrees | Set servo angle | +| `adc read ` | ADC channel ID | Read a single ADC channel | +| `adc read_for ` | ADC channel ID, sample count | Read one ADC channel repeatedly | +| `adc scan` | - | Read all ADC channels once | +| `adc scan_for ` | `count ≤ 1024` | Read all ADC channels repeatedly and log timestamps | + +### UART Output Formats + +| Command | Output Format | +| -------------- | --------------------------------------------------- | +| `adc scan` | `ADC,,` | +| `adc scan_for` | `,ADC,,,` | diff --git a/datasheets/RP-008373-DS-2-rp2350-datasheet.pdf b/datasheets/RP-008373-DS-2-rp2350-datasheet.pdf new file mode 100644 index 0000000..7a11127 Binary files /dev/null and b/datasheets/RP-008373-DS-2-rp2350-datasheet.pdf differ diff --git a/datasheets/board.jpg b/datasheets/board.jpg new file mode 100644 index 0000000..04aed46 Binary files /dev/null and b/datasheets/board.jpg differ diff --git a/datasheets/cd74hc4067.pdf b/datasheets/cd74hc4067.pdf new file mode 100644 index 0000000..0ad6ed1 Binary files /dev/null and b/datasheets/cd74hc4067.pdf differ diff --git a/datasheets/ds-000577-icm-45686-datasheet.pdf b/datasheets/ds-000577-icm-45686-datasheet.pdf new file mode 100644 index 0000000..2968646 Binary files /dev/null and b/datasheets/ds-000577-icm-45686-datasheet.pdf differ diff --git a/schematic/Servo2350.pdf b/schematic/Servo2350.pdf old mode 100755 new mode 100644 index a692e95..20e19d8 Binary files a/schematic/Servo2350.pdf and b/schematic/Servo2350.pdf differ diff --git a/test_code/CMakeLists.txt b/test_code/CMakeLists.txt index da489f1..8626e8d 100644 --- a/test_code/CMakeLists.txt +++ b/test_code/CMakeLists.txt @@ -3,8 +3,14 @@ cmake_minimum_required(VERSION 3.20.0) set(BOARD_ROOT ${CMAKE_CURRENT_SOURCE_DIR}) +# set(EXTRA_DTC_OVERLAY_FILE extra_adc.overlay) +set(EXTRA_DTC_OVERLAY_FILE extra_uart.overlay) + find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) project(servo2350) + +add_subdirectory(drivers) + FILE(GLOB app_sources src/*.c) target_sources(app PRIVATE ${app_sources}) diff --git a/test_code/Kconfig b/test_code/Kconfig new file mode 100644 index 0000000..e014bf5 --- /dev/null +++ b/test_code/Kconfig @@ -0,0 +1,3 @@ +source "Kconfig.zephyr" + +rsource "drivers/Kconfig" \ No newline at end of file diff --git a/test_code/README.rst b/test_code/README.rst deleted file mode 100644 index a67a4b7..0000000 --- a/test_code/README.rst +++ /dev/null @@ -1,97 +0,0 @@ -.. zephyr:code-sample:: blinky - :name: Blinky - :relevant-api: gpio_interface - - Blink an LED forever using the GPIO API. - -Overview -******** - -The Blinky sample blinks an LED forever using the :ref:`GPIO API `. - -The source code shows how to: - -#. Get a pin specification from the :ref:`devicetree ` as a - :c:struct:`gpio_dt_spec` -#. Configure the GPIO pin as an output -#. Toggle the pin forever - -See :zephyr:code-sample:`pwm-blinky` for a similar sample that uses the PWM API instead. - -.. _blinky-sample-requirements: - -Requirements -************ - -Your board must: - -#. Have an LED connected via a GPIO pin (these are called "User LEDs" on many of - Zephyr's :ref:`boards`). -#. Have the LED configured using the ``led0`` devicetree alias. - -Building and Running -******************** - -Build and flash Blinky as follows, changing ``reel_board`` for your board: - -.. zephyr-app-commands:: - :zephyr-app: samples/basic/blinky - :board: reel_board - :goals: build flash - :compact: - -After flashing, the LED starts to blink and messages with the current LED state -are printed on the console. If a runtime error occurs, the sample exits without -printing to the console. - -Build errors -************ - -You will see a build error at the source code line defining the ``struct -gpio_dt_spec led`` variable if you try to build Blinky for an unsupported -board. - -On GCC-based toolchains, the error looks like this: - -.. code-block:: none - - error: '__device_dts_ord_DT_N_ALIAS_led_P_gpios_IDX_0_PH_ORD' undeclared here (not in a function) - -Adding board support -******************** - -To add support for your board, add something like this to your devicetree: - -.. code-block:: DTS - - / { - aliases { - led0 = &myled0; - }; - - leds { - compatible = "gpio-leds"; - myled0: led_0 { - gpios = <&gpio0 13 GPIO_ACTIVE_LOW>; - }; - }; - }; - -The above sets your board's ``led0`` alias to use pin 13 on GPIO controller -``gpio0``. The pin flags :c:macro:`GPIO_ACTIVE_LOW` mean the LED is on when -the pin is set to its low state, and off when the pin is in its high state. - -Tips: - -- See :dtcompatible:`gpio-leds` for more information on defining GPIO-based LEDs - in devicetree. - -- If you're not sure what to do, check the devicetrees for supported boards which - use the same SoC as your target. See :ref:`get-devicetree-outputs` for details. - -- See :zephyr_file:`include/zephyr/dt-bindings/gpio/gpio.h` for the flags you can use - in devicetree. - -- If the LED is built in to your board hardware, the alias should be defined in - your :ref:`BOARD.dts file `. Otherwise, you can - define one in a :ref:`devicetree overlay `. diff --git a/test_code/app.overlay b/test_code/app.overlay new file mode 100644 index 0000000..e69de29 diff --git a/test_code/boards/arm/servo2350/servo2350-pinctrl.dtsi b/test_code/boards/arm/servo2350/servo2350-pinctrl.dtsi index aadd912..1d38dcf 100644 --- a/test_code/boards/arm/servo2350/servo2350-pinctrl.dtsi +++ b/test_code/boards/arm/servo2350/servo2350-pinctrl.dtsi @@ -1,74 +1,56 @@ #include &pinctrl { - uart0_default: uart0_default { - group1 { - pinmux = ; - }; - - group2 { - pinmux = ; - input-enable; - }; - }; - - uart1_default: uart1_default { - // These are the same pins as SPI - group1 { - pinmux = ; - }; - - group2 { - pinmux = ; - input-enable; - }; - }; - i2c0_default: i2c0_default { group1 { - pinmux = , ; + pinmux = , // SDA + ; // SCL input-enable; input-schmitt-enable; }; }; - spi0_default: spi0_default { + spi1_default: spi1_default { group1 { - pinmux = , , ; + pinmux = , // D29 + , // SPI_CLK + ; // SPI_MOSI }; group2 { - pinmux = ; + pinmux = ; // SPI_MISO input-enable; }; }; pwm_default: pwm_default { group1 { - pinmux = , - , - , - , - , - , - , - , - , - , - , - , - , - , - , - , - , - ; + pinmux = , // SERVO 1 + , // SERVO 2 + , // SERVO 3 + , // SERVO 4 + , // SERVO 5 + , // SERVO 6 + , // SERVO 7 + , // SERVO 8 + , // SERVO 9 + , // SERVO 10 + , // SERVO 11 + , // SERVO 12 + , // SERVO 13 + , // SERVO 14 + , // SERVO 15 + , // SERVO 16 + , // SERVO 17 + ; // SERVO 18 }; }; adc_default: adc_default { group1 { - pinmux = , , , ; + pinmux = , // MUX_COMMON + , // SERVO 17 + ; // SERVO 18 input-enable; }; }; diff --git a/test_code/boards/arm/servo2350/servo2350.dtsi b/test_code/boards/arm/servo2350/servo2350.dtsi index c5033f6..eab8eb6 100644 --- a/test_code/boards/arm/servo2350/servo2350.dtsi +++ b/test_code/boards/arm/servo2350/servo2350.dtsi @@ -16,9 +16,9 @@ }; zephyr,user { - io-channels = <&adc 0>, /* ADC channel 1 (adc2) - connected to mux */ - <&adc 1>, /* ADC channel 0 (adc1) - direct channel 16 */ - <&adc 2>; /* ADC channel 2 (adc3) - direct channel 17 */ + io-channels = <&adc 0>, + <&adc 1>, + <&adc 2>; }; leds { @@ -41,6 +41,11 @@ gpios = <&gpio0 22 GPIO_ACTIVE_HIGH>; label = "IMU_INT1"; }; + + imu_int2: imu_int2 { + gpios = <&gpio0 23 GPIO_ACTIVE_HIGH>; + label = "IMU_INT2"; + }; }; digital_inputs { @@ -178,13 +183,22 @@ }; }; + mux: mux { + compatible = "cd74hc4067"; + status = "okay"; + select-gpios = <&gpio0_hi 2 GPIO_ACTIVE_HIGH>, + <&gpio0_hi 3 GPIO_ACTIVE_HIGH>, + <&gpio0_hi 6 GPIO_ACTIVE_HIGH>, + <&gpio0_hi 7 GPIO_ACTIVE_HIGH>; + }; aliases { watchdog0 = &wdt0; led0 = &led0; led1 = &led1; - imu = &icm45686; + imu = &imu; imu-int1 = &imu_int1; + imu-int2 = &imu_int2; digital1 = &digital1; digital2 = &digital2; digital3 = &digital3; @@ -215,45 +229,7 @@ servo16 = &servo16; servo17 = &servo17; servo18 = &servo18; - adc-mux = &adc1; - adc0 = &adc1; - adc2 = &adc2; - adc3 = &adc3; - uart0 = &uart0; - uart-bridge = &uart1; - }; - - pico_header: connector { - compatible = "raspberrypi,pico-header"; - #gpio-cells = <2>; - gpio-map-mask = <0xffffffff 0xffffffc0>; - gpio-map-pass-thru = <0 0x3f>; - gpio-map = <0 0 &gpio0 0 0>, /* GP0 */ - <1 0 &gpio0 1 0>, /* GP1 */ - <2 0 &gpio0 2 0>, /* GP2 */ - <3 0 &gpio0 3 0>, /* GP3 */ - <4 0 &gpio0 4 0>, /* GP4 */ - <5 0 &gpio0 5 0>, /* GP5 */ - <6 0 &gpio0 6 0>, /* GP6 */ - <7 0 &gpio0 7 0>, /* GP7 */ - <8 0 &gpio0 8 0>, /* GP8 */ - <9 0 &gpio0 9 0>, /* GP9 */ - <10 0 &gpio0 10 0>, /* GP10 */ - <11 0 &gpio0 11 0>, /* GP11 */ - <12 0 &gpio0 12 0>, /* GP12 */ - <13 0 &gpio0 13 0>, /* GP13 */ - <14 0 &gpio0 14 0>, /* GP14 */ - <15 0 &gpio0 15 0>, /* GP15 */ - <16 0 &gpio0 16 0>, /* GP16 */ - <17 0 &gpio0 17 0>, /* GP17 */ - <18 0 &gpio0 18 0>, /* GP18 */ - <19 0 &gpio0 19 0>, /* GP19 */ - <20 0 &gpio0 20 0>, /* GP20 */ - <21 0 &gpio0 21 0>, /* GP21 */ - <22 0 &gpio0 22 0>, /* GP22 */ - <26 0 &gpio0 26 0>, /* GP26 */ - <27 0 &gpio0 27 0>, /* GP27 */ - <28 0 &gpio0 28 0>; /* GP28 */ + mux = &mux; }; }; @@ -287,25 +263,11 @@ }; }; -&uart0 { - current-speed = <115200>; - status = "okay"; - pinctrl-0 = <&uart0_default>; - pinctrl-names = "default"; -}; - -&uart1 { - current-speed = <115200>; - status = "okay"; - pinctrl-0 = <&uart1_default>; - pinctrl-names = "default"; -}; - zephyr_udc0: &usbd { status = "okay"; cdc_acm_uart0: cdc_acm_uart0 { compatible = "zephyr,cdc-acm-uart"; - label = "Zephyr USB CDC-ACM"; + label = "USB_CDC"; }; }; @@ -318,20 +280,13 @@ gpio0_lo: &gpio0 { }; -&spi0 { - clock-frequency = ; - pinctrl-0 = <&spi0_default>; - pinctrl-names = "default"; - status = "disabled"; -}; - &i2c0 { pinctrl-0 = <&i2c0_default>; pinctrl-names = "default"; status = "okay"; clock-frequency = ; - icm45686: icm45686@68 { + imu: icm45686@68 { compatible = "invensense,icm42688"; reg = <0x68>; status = "okay"; @@ -350,14 +305,21 @@ gpio0_lo: &gpio0 { }; }; -adc0: &adc { +&spi1 { + clock-frequency = ; + pinctrl-0 = <&spi1_default>; + pinctrl-names = "default"; + status = "disabled"; +}; + +&adc { pinctrl-0 = <&adc_default>; pinctrl-names = "default"; status = "okay"; #address-cells = <1>; #size-cells = <0>; - adc1: channel@0 { + adc0: channel@0 { reg = <0>; zephyr,gain = "ADC_GAIN_1"; zephyr,reference = "ADC_REF_INTERNAL"; @@ -365,7 +327,7 @@ adc0: &adc { zephyr,resolution = <12>; }; - adc2: channel@1 { + adc1: channel@1 { reg = <1>; zephyr,gain = "ADC_GAIN_1"; zephyr,reference = "ADC_REF_INTERNAL"; @@ -373,7 +335,7 @@ adc0: &adc { zephyr,resolution = <12>; }; - adc3: channel@2 { + adc2: channel@2 { reg = <2>; zephyr,gain = "ADC_GAIN_1"; zephyr,reference = "ADC_REF_INTERNAL"; @@ -392,15 +354,3 @@ adc0: &adc { &timer0 { status = "okay"; }; - -pico_spi: &spi0 {}; - -pico_i2c0: &i2c0 {}; - -pico_i2c1: &i2c1 {}; - -pico_serial: &uart0 {}; - -zephyr_i2c: &i2c0 {}; - -imu: &icm45686 {}; diff --git a/test_code/drivers/multiplexer/cd74hc4067/CMakeLists.txt b/test_code/drivers/multiplexer/cd74hc4067/CMakeLists.txt index 9e9f043..57896cd 100644 --- a/test_code/drivers/multiplexer/cd74hc4067/CMakeLists.txt +++ b/test_code/drivers/multiplexer/cd74hc4067/CMakeLists.txt @@ -1,5 +1,4 @@ -if(CONFIG_CD74HC4067) - target_sources(app PRIVATE ${CMAKE_CURRENT_LIST_DIR}/cd74hc4067.c) - target_include_directories(app PRIVATE ${CMAKE_CURRENT_LIST_DIR}) -endif() +target_sources(app PRIVATE ${CMAKE_CURRENT_LIST_DIR}/cd74hc4067.c) + +target_include_directories(app PRIVATE ${CMAKE_CURRENT_LIST_DIR}) \ No newline at end of file diff --git a/test_code/drivers/multiplexer/cd74hc4067/Kconfig b/test_code/drivers/multiplexer/cd74hc4067/Kconfig index 35c17a5..c1d2d51 100644 --- a/test_code/drivers/multiplexer/cd74hc4067/Kconfig +++ b/test_code/drivers/multiplexer/cd74hc4067/Kconfig @@ -2,8 +2,8 @@ menuconfig CD74HC4067 bool "TI CD74HC4067 analog multiplexer" depends on GPIO help - Enable support for the TI CD74HC4067 16-channel analog/digital - multiplexer/demultiplexer controlled through GPIO selection pins. + Enable support for the TI CD74HC4067 16-channel analog + multiplexer controlled through GPIO selection pins. if CD74HC4067 @@ -11,10 +11,10 @@ config CD74HC4067_INIT_PRIORITY int "Initialization priority" default 80 help - Device initialization priority for the CD74HC4067 driver. This should - be higher (lower numeric value) than any clients that depend on the - multiplexer being ready. + Device initialization priority for the CD74HC4067 driver. + +# Creates "CONFIG_CD74HC4067_LOG_LEVEL" module = CD74HC4067 module-str = cd74hc4067 source "subsys/logging/Kconfig.template.log_config" diff --git a/test_code/drivers/multiplexer/cd74hc4067/cd74hc4067.c b/test_code/drivers/multiplexer/cd74hc4067/cd74hc4067.c index f2301eb..ef40a62 100644 --- a/test_code/drivers/multiplexer/cd74hc4067/cd74hc4067.c +++ b/test_code/drivers/multiplexer/cd74hc4067/cd74hc4067.c @@ -1,42 +1,74 @@ - #define DT_DRV_COMPAT cd74hc4067 #include -#include -#include -#include -#include -#include #include -#include #include "cd74hc4067.h" -LOG_MODULE_REGISTER(cd74hc4067, CONFIG_CD74HC4067_LOG_LEVEL); +LOG_MODULE_REGISTER(cd74hc4067); -#define CD74HC4067_CHANNEL_COUNT 16U -#define CD74HC4067_SELECTION_PIN_COUNT 4U +//------------------------------------------------------------------------- +// Declarations -struct cd74hc4067_config { - struct gpio_dt_spec sel[CD74HC4067_SELECTION_PIN_COUNT]; - struct gpio_dt_spec enable; - bool has_enable; -}; +static int cd74hc4067_init(const struct device *dev); +static int cd74hc4067_enable_pin(const struct device *dev, bool state); -struct cd74hc4067_data { - struct k_mutex lock; - uint8_t current_channel; - bool enabled; -}; +//------------------------------------------------------------------------- +// Private -static int cd74hc4067_sync_enable_state(const struct device *dev, bool enable) -{ - const struct cd74hc4067_config *cfg = dev->config; + +static int cd74hc4067_init(const struct device *dev) { + int ret; + + const struct cd74hc4067_cfg *cfg = dev->config; + struct cd74hc4067_data *data = dev->data; + + k_mutex_init(&data->lock); + data->current_channel = 0U; + data->enabled = !cfg->has_enable; + + // Init the select pins + for (int i = 0; i < CD74HC4067_SELECT_PIN_COUNT; i++) { + const struct gpio_dt_spec *pin = &(cfg->select[i]); + if (!gpio_is_ready_dt(pin)) { + LOG_ERR("Select pin %d not ready.\r\n", i); + return -ENODEV; + } + + ret = gpio_pin_configure_dt(pin, GPIO_OUTPUT_INACTIVE); + if (ret < 0) { + LOG_ERR("Could not configure select pin %d as output\r\n", i); + return -ENODEV; + } + } + + if (cfg->has_enable) { + // Init the enable pin + const struct gpio_dt_spec *enable = &cfg->enable; + LOG_DBG("Initializing enable pin.\r\n"); + + if (!gpio_is_ready_dt(enable)) { + LOG_ERR("Enable pin not ready.\r\n"); + return -ENODEV; + } + + ret = gpio_pin_configure_dt(enable, GPIO_OUTPUT_INACTIVE); + if (ret < 0) { + LOG_ERR("Could not configure ENABLE pin as output\r\n"); + return -ENODEV; + } + } + + return 0; +} + +static int cd74hc4067_enable_pin(const struct device *dev, bool state) { + const struct cd74hc4067_cfg *cfg = dev->config; struct cd74hc4067_data *data = dev->data; int ret = 0; if (!cfg->has_enable) { - if (!enable) { + if (!state) { return -ENOTSUP; } @@ -49,95 +81,68 @@ static int cd74hc4067_sync_enable_state(const struct device *dev, bool enable) return -ENODEV; } - ret = gpio_pin_set_dt(&cfg->enable, enable ? 0 : 1); + ret = gpio_pin_set_dt(&cfg->enable, state ? 0 : 1); if (ret == 0) { - data->enabled = enable; + data->enabled = state; } return ret; } -static int cd74hc4067_configure_gpios(const struct device *dev) -{ - const struct cd74hc4067_config *cfg = dev->config; - int ret; +//------------------------------------------------------------------------- +// Public - for (size_t i = 0; i < ARRAY_SIZE(cfg->sel); i++) { - const struct gpio_dt_spec *sel = &cfg->sel[i]; - - if (!device_is_ready(sel->port)) { - LOG_ERR("%s sel%zu GPIO not ready", dev->name, i); - return -ENODEV; - } - - ret = gpio_pin_configure_dt(sel, GPIO_OUTPUT_INACTIVE); - if (ret != 0) { - LOG_ERR("%s failed to configure sel%zu GPIO (%d)", dev->name, i, ret); - return ret; - } - } - - if (cfg->has_enable) { - if (!device_is_ready(cfg->enable.port)) { - LOG_ERR("%s enable GPIO not ready", dev->name); - return -ENODEV; - } - - ret = gpio_pin_configure_dt(&cfg->enable, GPIO_OUTPUT_HIGH); - if (ret != 0) { - LOG_ERR("%s failed to configure enable GPIO (%d)", dev->name, ret); - return ret; - } - } - - return 0; -} - -static int cd74hc4067_init(const struct device *dev) -{ - const struct cd74hc4067_config *cfg = dev->config; +int cd74hc4067_enable(const struct device *dev) { struct cd74hc4067_data *data = dev->data; int ret; - k_mutex_init(&data->lock); - data->current_channel = 0U; - data->enabled = !cfg->has_enable; - - ret = cd74hc4067_configure_gpios(dev); - if (ret != 0) { - return ret; - } - - /* Latch the default channel (0) */ - ret = cd74hc4067_select_channel(dev, 0U); - if (ret != 0) { - return ret; - } - - LOG_INF("%s initialized (enable pin %s)", dev->name, cfg->has_enable ? "present" : "absent"); - - return 0; -} - -int cd74hc4067_select_channel(const struct device *dev, uint8_t channel) -{ - const struct cd74hc4067_config *cfg = dev->config; - struct cd74hc4067_data *data = dev->data; - int ret = 0; - - if (channel >= CD74HC4067_CHANNEL_COUNT) { - return -EINVAL; - } - ret = k_mutex_lock(&data->lock, K_FOREVER); if (ret != 0) { return ret; } - for (size_t i = 0; i < ARRAY_SIZE(cfg->sel); i++) { - ret = gpio_pin_set_dt(&cfg->sel[i], (channel >> i) & 0x1); - if (ret != 0) { - LOG_ERR("%s failed to drive sel%zu GPIO (%d)", dev->name, i, ret); + ret = cd74hc4067_enable_pin(dev, true); + + k_mutex_unlock(&data->lock); + return ret; +} + +int cd74hc4067_disable(const struct device *dev) { + struct cd74hc4067_data *data = dev->data; + int ret; + + ret = k_mutex_lock(&data->lock, K_FOREVER); + if (ret != 0) { + return ret; + } + + ret = cd74hc4067_enable_pin(dev, false); + + k_mutex_unlock(&data->lock); + return ret; +} + +int cd74hc4067_select_channel(const struct device *dev, uint8_t channel) { + if (channel >= CD74HC4067_SELECT_CHANNEL_COUNT) { + LOG_ERR("Channel not found.\r\n"); + return -ENODEV; + } + + int ret; + const struct cd74hc4067_cfg *cfg = dev->config; + struct cd74hc4067_data *data = dev->data; + + ret = k_mutex_lock(&data->lock, K_FOREVER); + if (ret < 0) { + LOG_ERR("Mutex error"); + return ret; + } + + for (int i = 0; i < CD74HC4067_SELECT_PIN_COUNT; i++) { + bool state = (channel >> i) & 0x1; + ret = gpio_pin_set_dt(&cfg->select[i], state); + if (ret < 0) { + LOG_ERR("Unable to set channel pin: %d", ret); goto out; } } @@ -149,69 +154,38 @@ out: return ret; } -int cd74hc4067_enable(const struct device *dev) -{ - struct cd74hc4067_data *data = dev->data; - int ret; +//------------------------------------------------------------------------- +// Devicetree - ret = k_mutex_lock(&data->lock, K_FOREVER); - if (ret != 0) { - return ret; - } +// static const struct cd74hc4067_api cd74hc4067_api_funcs = { +// .enable = cd74hc4067_enable, +// .disable = cd74hc4067_disable, +// .select_channel = cd74hc4067_select_channel, +// }; - ret = cd74hc4067_sync_enable_state(dev, true); - k_mutex_unlock(&data->lock); - return ret; -} - -int cd74hc4067_disable(const struct device *dev) -{ - struct cd74hc4067_data *data = dev->data; - int ret; - - ret = k_mutex_lock(&data->lock, K_FOREVER); - if (ret != 0) { - return ret; - } - - ret = cd74hc4067_sync_enable_state(dev, false); - - k_mutex_unlock(&data->lock); - return ret; -} - -uint8_t cd74hc4067_get_current_channel(const struct device *dev) -{ - struct cd74hc4067_data *data = dev->data; - uint8_t channel; - - k_mutex_lock(&data->lock, K_FOREVER); - channel = data->current_channel; - k_mutex_unlock(&data->lock); - - return channel; -} - -#define CD74HC4067_DEFINE(inst) \ - BUILD_ASSERT(DT_PROP_LEN(DT_DRV_INST(inst), sel_gpios) == CD74HC4067_SELECTION_PIN_COUNT, \ - "cd74hc4067 requires exactly 4 selection GPIOs"); \ - static const struct cd74hc4067_config cd74hc4067_config_##inst = { \ - .sel = { \ - GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), sel_gpios, 0), \ - GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), sel_gpios, 1), \ - GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), sel_gpios, 2), \ - GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), sel_gpios, 3), \ - }, \ - .enable = COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, enable_gpios), \ - (GPIO_DT_SPEC_GET(DT_DRV_INST(inst), enable_gpios)), \ - ((struct gpio_dt_spec){0})), \ - .has_enable = DT_INST_NODE_HAS_PROP(inst, enable_gpios), \ - }; \ - static struct cd74hc4067_data cd74hc4067_data_##inst; \ - DEVICE_DT_INST_DEFINE(inst, cd74hc4067_init, NULL, &cd74hc4067_data_##inst, \ - &cd74hc4067_config_##inst, POST_KERNEL, \ - CONFIG_CD74HC4067_INIT_PRIORITY, NULL); - -DT_INST_FOREACH_STATUS_OKAY(CD74HC4067_DEFINE) +#define CD74HC4067_DEFINE(inst) \ + \ + static const struct cd74hc4067_cfg cd74hc4067_cfg_##inst = { \ + .select = { \ + GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 0), \ + GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 1), \ + GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 2), \ + GPIO_DT_SPEC_GET_BY_IDX(DT_DRV_INST(inst), select_gpios, 3), \ + }, \ + .enable = COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, enable_gpio), \ + (GPIO_DT_SPEC_GET(DT_DRV_INST(inst), enable_gpio)), \ + ((struct gpio_dt_spec){0})), \ + .has_enable = DT_INST_NODE_HAS_PROP(inst, enable_gpio), \ + }; \ + static struct cd74hc4067_data cd74hc4067_data_##inst; \ + DEVICE_DT_INST_DEFINE( inst, \ + cd74hc4067_init, \ + NULL, \ + &cd74hc4067_data_##inst, \ + &cd74hc4067_cfg_##inst, \ + POST_KERNEL, \ + CONFIG_CD74HC4067_INIT_PRIORITY, \ + NULL); +DT_INST_FOREACH_STATUS_OKAY(CD74HC4067_DEFINE) \ No newline at end of file diff --git a/test_code/drivers/multiplexer/cd74hc4067/cd74hc4067.h b/test_code/drivers/multiplexer/cd74hc4067/cd74hc4067.h index a72ded9..b7c7a4f 100644 --- a/test_code/drivers/multiplexer/cd74hc4067/cd74hc4067.h +++ b/test_code/drivers/multiplexer/cd74hc4067/cd74hc4067.h @@ -3,58 +3,39 @@ #include +#include #include +#include #ifdef __cplusplus extern "C" { #endif -/** - * @brief Select one of the 16 multiplexed channels. - * - * @param dev CD74HC4067 device instance. - * @param channel Channel number to route (0-15). - * - * @retval 0 If the channel was selected. - * @retval -EINVAL If the channel number is out of range. - * @retval -ENODEV If one of the selection GPIOs is not ready. - * @retval other Negative errno value from the GPIO driver. - */ + +// struct cd74hc4067_api { +// int (*enable)(const struct device *dev); +// int (*disable)(const struct device *dev); +// int (*select_channel)(const struct device *dev, uint8_t channel); +// }; + +struct cd74hc4067_data { + struct k_mutex lock; + uint8_t current_channel; + bool enabled; +}; + +#define CD74HC4067_SELECT_CHANNEL_COUNT 16U +#define CD74HC4067_SELECT_PIN_COUNT 4U +struct cd74hc4067_cfg { + struct gpio_dt_spec select[CD74HC4067_SELECT_PIN_COUNT]; + struct gpio_dt_spec enable; + bool has_enable; +}; + +int cd74hc4067_enable(const struct device *dev); +int cd74hc4067_disable(const struct device *dev); int cd74hc4067_select_channel(const struct device *dev, uint8_t channel); -/** - * @brief Enable the multiplexer output (drives the /EN pin low). - * - * If the instance does not provide an enable GPIO, this call is a no-op. - * - * @param dev CD74HC4067 device instance. - * - * @retval 0 On success. - * @retval -ENODEV If the enable GPIO is not ready. - * @retval negative errno on GPIO failures. - */ -int cd74hc4067_enable(const struct device *dev); - -/** - * @brief Disable the multiplexer output (drives the /EN pin high). - * - * @param dev CD74HC4067 device instance. - * - * @retval 0 On success. - * @retval -ENOTSUP If the instance does not expose an enable GPIO. - * @retval -ENODEV If the enable GPIO is not ready. - * @retval negative errno on GPIO failures. - */ -int cd74hc4067_disable(const struct device *dev); - -/** - * @brief Return the last channel value written to the device. - * - * @param dev CD74HC4067 device instance. - * - * @return Channel number in the range [0, 15]. - */ -uint8_t cd74hc4067_get_current_channel(const struct device *dev); #ifdef __cplusplus } diff --git a/test_code/dts/bindings/cd74hc4067.yaml b/test_code/dts/bindings/cd74hc4067.yaml index 0974969..add805a 100644 --- a/test_code/dts/bindings/cd74hc4067.yaml +++ b/test_code/dts/bindings/cd74hc4067.yaml @@ -1,27 +1,23 @@ description: | - GPIO-controlled 16-channel analog/digital multiplexer/demultiplexer. - - The CD74HC4067 has 4 selection pins (S0-S3) that select one of 16 channels. - Supports optional common enable pin for power management. + TI CD74HC4067 16-channel analog multiplexer. compatible: "cd74hc4067" include: base.yaml properties: - sel-gpios: + select-gpios: type: phandle-array required: true description: | GPIO pins for channel selection (S0-S3). Array must contain exactly 4 GPIOs: - - Index 0: S0 (LSB) + - Index 0: S0 - Index 1: S1 - Index 2: S2 - - Index 3: S3 (MSB) + - Index 3: S3 - enable-gpios: - type: phandle-array + enable-gpio: + type: phandle description: | - Optional GPIO for common enable pin (E). - When low, enables the multiplexer. When high, disables it. \ No newline at end of file + Optional GPIO for common enable pin (E). \ No newline at end of file diff --git a/test_code/extra_adc.overlay b/test_code/extra_adc.overlay new file mode 100644 index 0000000..c0cfe05 --- /dev/null +++ b/test_code/extra_adc.overlay @@ -0,0 +1,68 @@ +/ { + // Redefine the user io-channels (does not append) + zephyr,user { + io-channels = <&adc 0>, + <&adc 1>, + <&adc 2>, + <&adc 3>, + <&adc 4>, + <&adc 5>, + <&adc 6>, + <&adc 7>; + }; +}; + +// Add extra pins to pinctrl for the driver to configure the pins +&adc_default { + group2 { + pinmux = , // ADC3 + , // ADC4 + , // ADC5 + , // D46 + ; // D47 + input-enable; + }; +}; + +// Add the extra channels to the ADC block +&adc { + adc3: channel@3 { + reg = <3>; + zephyr,gain = "ADC_GAIN_1"; + zephyr,reference = "ADC_REF_INTERNAL"; + zephyr,acquisition-time = ; + zephyr,resolution = <12>; + }; + + adc4: channel@4 { + reg = <4>; + zephyr,gain = "ADC_GAIN_1"; + zephyr,reference = "ADC_REF_INTERNAL"; + zephyr,acquisition-time = ; + zephyr,resolution = <12>; + }; + + adc5: channel@5 { + reg = <5>; + zephyr,gain = "ADC_GAIN_1"; + zephyr,reference = "ADC_REF_INTERNAL"; + zephyr,acquisition-time = ; + zephyr,resolution = <12>; + }; + + adc6: channel@6 { + reg = <6>; + zephyr,gain = "ADC_GAIN_1"; + zephyr,reference = "ADC_REF_INTERNAL"; + zephyr,acquisition-time = ; + zephyr,resolution = <12>; + }; + + adc7: channel@7 { + reg = <7>; + zephyr,gain = "ADC_GAIN_1"; + zephyr,reference = "ADC_REF_INTERNAL"; + zephyr,acquisition-time = ; + zephyr,resolution = <12>; + }; +}; diff --git a/test_code/extra_uart.overlay b/test_code/extra_uart.overlay new file mode 100644 index 0000000..ff96daa --- /dev/null +++ b/test_code/extra_uart.overlay @@ -0,0 +1,25 @@ +/ { + aliases { + uart1 = &uart1; + }; +}; + +&pinctrl { + uart_backup: uart_backup { + group1 { + pinmux = ; // SPI_CLK + }; + group2 { + pinmux = ; // SPI_MOSI + input-enable; + }; + }; +}; + +// Uses spi1 pins. Make sure to disable it +&uart1 { + current-speed = <115200>; + status = "okay"; + pinctrl-0 = <&uart_backup>; + pinctrl-names = "default"; +}; diff --git a/test_code/prj.conf b/test_code/prj.conf index 2703587..47ad3d6 100644 --- a/test_code/prj.conf +++ b/test_code/prj.conf @@ -1,6 +1,7 @@ # Basic CONFIG_GPIO=y CONFIG_PWM=y +CONFIG_ADC=y # Serial CONFIG_SERIAL=y @@ -16,7 +17,7 @@ CONFIG_CDC_ACM_SERIAL_PRODUCT_STRING="Servo2350" CONFIG_CDC_ACM_SERIAL_PID=0x0004 # Route console/logs to USB CDC ACM (configured via device tree) -CONFIG_LOG=n +CONFIG_LOG=y CONFIG_LOG_DEFAULT_LEVEL=3 CONFIG_UART_CONSOLE=y CONFIG_LOG_BACKEND_UART=y @@ -28,4 +29,10 @@ CONFIG_SHELL_HISTORY=y # ZBUS Configuration CONFIG_ZBUS=y -CONFIG_ZBUS_RUNTIME_OBSERVERS=y \ No newline at end of file +CONFIG_ZBUS_RUNTIME_OBSERVERS=y + +# I2C Configuration +CONFIG_I2C=y + +# Multiplexer +CONFIG_CD74HC4067=y \ No newline at end of file diff --git a/test_code/sample.yaml b/test_code/sample.yaml deleted file mode 100644 index de71191..0000000 --- a/test_code/sample.yaml +++ /dev/null @@ -1,12 +0,0 @@ -sample: - name: Blinky Sample -tests: - sample.basic.blinky: - tags: - - LED - - gpio - filter: dt_enabled_alias_with_parent_compat("led0", "gpio-leds") - depends_on: gpio - harness: led - integration_platforms: - - frdm_k64f diff --git a/test_code/src/adc.c b/test_code/src/adc.c new file mode 100644 index 0000000..d444ade --- /dev/null +++ b/test_code/src/adc.c @@ -0,0 +1,122 @@ +#include "adc.h" +#include "mux.h" + +#include +#include +#include + +LOG_MODULE_REGISTER(adc); + + +#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; +} + + +int32_t adc_read_id(uint8_t id) { + int ret; + int32_t val_mv = 0; + adc_buffer = 0; + + if (id < MUX_CHANNELS) { + // Read muxed channels + (void)adc_sequence_init_dt(&adc_mux_spec, &sequence); + + // Set mux + 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 = (int32_t)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 = (int32_t)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 = (int32_t)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/test_code/src/adc.h b/test_code/src/adc.h new file mode 100644 index 0000000..fee9913 --- /dev/null +++ b/test_code/src/adc.h @@ -0,0 +1,14 @@ +#ifndef ADC_H +#define ADC_H + + +#include + +#define NUM_ADC_CHANNELS 18 + +int adc_init_all(void); +int32_t adc_read_id(uint8_t id); + + +#endif /* ADC_H */ + diff --git a/test_code/src/led.c b/test_code/src/led.c index 97a1180..6d1c685 100644 --- a/test_code/src/led.c +++ b/test_code/src/led.c @@ -8,7 +8,7 @@ #include #include -LOG_MODULE_REGISTER(led, LOG_LEVEL_INF); +LOG_MODULE_REGISTER(led); #define LED0_NODE DT_ALIAS(led0) #define LED1_NODE DT_ALIAS(led1) diff --git a/test_code/src/led.h b/test_code/src/led.h index 1c1d38d..083b62f 100644 --- a/test_code/src/led.h +++ b/test_code/src/led.h @@ -1,17 +1,11 @@ #ifndef LED_H #define LED_H -/** - * @file led.h - * @brief LED control via GPIO and ZBUS - */ -/** - * @brief Initialize LED driver - * - * @return 0 on success, negative errno on failure - */ + int led_init(void); + + #endif /* LED_H */ diff --git a/test_code/src/main.c b/test_code/src/main.c index a55ea1f..bfcf5ee 100644 --- a/test_code/src/main.c +++ b/test_code/src/main.c @@ -1,26 +1,38 @@ -/* - * Copyright (c) 2016 Intel Corporation - * - * SPDX-License-Identifier: Apache-2.0 - */ - #include #include #include -LOG_MODULE_REGISTER(main, LOG_LEVEL_INF); +#include "led.h" +#include "servo.h" +#include "adc.h" +#include "uart.h" + +LOG_MODULE_REGISTER(main); -int main(void) -{ +int main(void) { int ret; - /* LED driver - status indicators */ ret = led_init(); if (ret != 0) { LOG_ERR("Failed to initialize LED driver: %d", ret); } + ret = servo_init(); + if (ret != 0) { + LOG_ERR("Failed to initialize SERVO driver: %d", ret); + } + + ret = adc_init_all(); + if (ret != 0) { + LOG_ERR("Failed to initialize ADC driver: %d", ret); + } + + ret = uart_init(); + if (ret != 0) { + LOG_ERR("Failed to initialize UART driver: %d", ret); + } + while (1) { k_msleep(1000); diff --git a/test_code/src/mux.c b/test_code/src/mux.c new file mode 100644 index 0000000..e867222 --- /dev/null +++ b/test_code/src/mux.c @@ -0,0 +1,44 @@ +#include "mux.h" + +#include +#include + +LOG_MODULE_REGISTER(mux); + +#define MUX_NODE DT_ALIAS(mux) + +static const struct device *mux_dev = DEVICE_DT_GET(MUX_NODE); + +int mux_init(void) { + int ret; + + if (!DT_NODE_HAS_STATUS(MUX_NODE, okay)) { + LOG_ERR("mux alias missing or disabled in devicetree"); + return -ENODEV; + } + + if (!device_is_ready(mux_dev)) { + LOG_ERR("Multiplexer device not ready"); + return -ENODEV; + } + + ret = cd74hc4067_enable(mux_dev); + if ((ret != 0) && (ret != -ENOTSUP)) { + LOG_ERR("Failed to enable multiplexer: %d", ret); + return ret; + } + + ret = cd74hc4067_select_channel(mux_dev, 0U); + if (ret != 0) { + LOG_ERR("Failed to select initial channel: %d", ret); + return ret; + } + + LOG_INF("Multiplexer HAL initialized"); + + return 0; +} + +int mux_select_channel(int channel) { + return cd74hc4067_select_channel(mux_dev, channel); +} \ No newline at end of file diff --git a/test_code/src/mux.h b/test_code/src/mux.h new file mode 100644 index 0000000..9508b64 --- /dev/null +++ b/test_code/src/mux.h @@ -0,0 +1,13 @@ +#ifndef MUX_H +#define MUX_H + + +#include "cd74hc4067.h" + +#define MUX_CHANNELS CD74HC4067_SELECT_CHANNEL_COUNT + +int mux_init(void); +int mux_select_channel(int channel); + + +#endif // MUX_H \ No newline at end of file diff --git a/test_code/src/servo.c b/test_code/src/servo.c new file mode 100644 index 0000000..fdbd70e --- /dev/null +++ b/test_code/src/servo.c @@ -0,0 +1,97 @@ +#include "servo.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(servo); + +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)), + PWM_DT_SPEC_GET(DT_ALIAS(servo3)), + PWM_DT_SPEC_GET(DT_ALIAS(servo4)), + PWM_DT_SPEC_GET(DT_ALIAS(servo5)), + PWM_DT_SPEC_GET(DT_ALIAS(servo6)), + PWM_DT_SPEC_GET(DT_ALIAS(servo7)), + PWM_DT_SPEC_GET(DT_ALIAS(servo8)), + PWM_DT_SPEC_GET(DT_ALIAS(servo9)), + PWM_DT_SPEC_GET(DT_ALIAS(servo10)), + PWM_DT_SPEC_GET(DT_ALIAS(servo11)), + PWM_DT_SPEC_GET(DT_ALIAS(servo12)), + PWM_DT_SPEC_GET(DT_ALIAS(servo13)), + PWM_DT_SPEC_GET(DT_ALIAS(servo14)), + PWM_DT_SPEC_GET(DT_ALIAS(servo15)), + PWM_DT_SPEC_GET(DT_ALIAS(servo16)), + PWM_DT_SPEC_GET(DT_ALIAS(servo17)), + PWM_DT_SPEC_GET(DT_ALIAS(servo18)), +}; + +int servo_init(void) { + int ret; + + for (uint8_t 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 (uint8_t 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; + } + } + + return 0; +} + +uint32_t servo_angle_to_pulse_us(int angle_deg) { + if (angle_deg > SERVO_MAX_ANGLE) { + angle_deg = SERVO_MAX_ANGLE; + } else if (angle_deg < SERVO_MIN_ANGLE) { + angle_deg = SERVO_MIN_ANGLE; + } + + /* Convert: 0 deg = center, ±max_angle = ±half_pulse_range from center */ + int16_t half_pulse_range = ((int16_t)(SERVO_MAX_PULSE - SERVO_MIN_PULSE)) / 2; + int16_t pulse_offset_us = (angle_deg * half_pulse_range) / SERVO_MAX_ANGLE; + uint32_t pulse_us = (uint32_t)(SERVO_CENTER_PULSE + pulse_offset_us); + + if (pulse_us < SERVO_MIN_PULSE) { + pulse_us = SERVO_MIN_PULSE; + } else if (pulse_us > SERVO_MAX_PULSE) { + pulse_us = SERVO_MAX_PULSE; + } + + return pulse_us; +} + +int servo_set_angle(uint8_t channel, int angle_deg) { + if (channel >= NUM_SERVO_CHANNELS) { + return -EINVAL; + } + + int offset_deg = 0; + int effective_angle_deg = angle_deg + offset_deg; + + uint32_t pulse_us = servo_angle_to_pulse_us(effective_angle_deg); + 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; +} + diff --git a/test_code/src/servo.h b/test_code/src/servo.h new file mode 100644 index 0000000..e6719b5 --- /dev/null +++ b/test_code/src/servo.h @@ -0,0 +1,21 @@ +#ifndef SERVO_H +#define SERVO_H + +#include +#include +#include + +#define NUM_SERVO_CHANNELS 18 +#define SERVO_CENTER_PULSE 1500 +#define SERVO_MIN_PULSE 750 +#define SERVO_MAX_PULSE 2500 +#define SERVO_MAX_ANGLE 135 +#define SERVO_MIN_ANGLE -135 + +int servo_init(void); +uint32_t servo_angle_to_pulse_us(int angle_deg); +int servo_set_angle(uint8_t channel, int angle_deg); + + +#endif /* SERVO_H */ + diff --git a/test_code/src/shell.c b/test_code/src/shell.c index 84f6b7e..e867214 100644 --- a/test_code/src/shell.c +++ b/test_code/src/shell.c @@ -1,25 +1,20 @@ #include "zbus_message.h" +#include "servo.h" +#include "adc.h" +#include "uart.h" #include #include #include #include +#include -LOG_MODULE_REGISTER(led_shell, LOG_LEVEL_INF); +LOG_MODULE_REGISTER(shell); -/** - * @brief Parse and publish an LED command to leds_chan. - * - * Usage: - * led set - * - * @param shell Shell instance - * @param argc Argument count (must be 3: "set", id, state) - * @param argv Argument vector - * @return 0 on success, negative errno on failure - */ -static int cmd_led_set(const struct shell *shell, size_t argc, char **argv) -{ +// --------------------------------------------------------------------- +// LEDs + +static int cmd_led_set(const struct shell *shell, size_t argc, char **argv) { struct leds_message msg; int ret; @@ -54,11 +49,7 @@ static int cmd_led_set(const struct shell *shell, size_t argc, char **argv) return 0; } -/** - * @brief Turn on all LEDs. - */ -static int cmd_led_all_on(const struct shell *shell, size_t argc, char **argv) -{ +static int cmd_led_all_on(const struct shell *shell, size_t argc, char **argv) { struct leds_message msg; int ret; @@ -77,11 +68,7 @@ static int cmd_led_all_on(const struct shell *shell, size_t argc, char **argv) return 0; } -/** - * @brief Turn off all LEDs. - */ -static int cmd_led_all_off(const struct shell *shell, size_t argc, char **argv) -{ +static int cmd_led_all_off(const struct shell *shell, size_t argc, char **argv) { struct leds_message msg; int ret; @@ -124,4 +111,166 @@ SHELL_CMD_REGISTER(led, &sub_led, " led set - Set LED state\n" " led allon - Turn all LEDs on\n" " led alloff - Turn all LEDs off", + NULL); + +// --------------------------------------------------------------------- +// SERVO + +static int cmd_servo_set(const struct shell *shell, size_t argc, char **argv) { + /* Parse SERVO id */ + char *endptr; + long id = strtol(argv[1], &endptr, 10); + + if (*endptr != '\0' || id < 0 || id > NUM_SERVO_CHANNELS) { + shell_error(shell, "Invalid SERVO id '%s'.", argv[1]); + return -EINVAL; + } + + long value = strtol(argv[2], &endptr, 10); + + if (*endptr != '\0' || value < SERVO_MIN_ANGLE || value > SERVO_MAX_ANGLE) { + shell_error(shell, "Invalid SERVO value '%s'.(%d - %d)", argv[2], SERVO_MIN_ANGLE, SERVO_MAX_ANGLE); + return -EINVAL; + } + + servo_set_angle((int)id, (int)value); + + shell_print(shell, "SERVO%d turned %d", (int)id, (int)value); + return 0; +} + +/* Subcommands for the "servo" root command */ +SHELL_STATIC_SUBCMD_SET_CREATE(sub_servo, + SHELL_CMD_ARG(set, NULL, + "Set a single SERVO state.\n" + "Usage: servo set \n", + cmd_servo_set, 3, 0), + SHELL_SUBCMD_SET_END +); + +SHELL_CMD_REGISTER(servo, &sub_servo, + "SERVO control commands.\n" + " servo set - Set SERVO state\n", + NULL); + +// --------------------------------------------------------------------- +// ADC + +static int cmd_adc_read(const struct shell *shell, size_t argc, char **argv) { + /* Parse ADC id */ + char *endptr; + long id = strtol(argv[1], &endptr, 10); + + if (*endptr != '\0' || id < 0 || id > NUM_SERVO_CHANNELS) { + shell_error(shell, "Invalid ADC id '%s'.", argv[1]); + return -EINVAL; + } + + int value = adc_read_id((int)id); + + shell_print(shell, "ADC%d value: %d", (int)id, (int)value); + return 0; +} + +static int cmd_adc_read_for(const struct shell *shell, size_t argc, char **argv) { + /* Parse ADC id */ + char *endptr; + long id = strtol(argv[1], &endptr, 10); + + if (*endptr != '\0' || id < 0 || id > NUM_SERVO_CHANNELS) { + shell_error(shell, "Invalid ADC id '%s'.", argv[1]); + return -EINVAL; + } + + long count = strtol(argv[2], &endptr, 10); + + if (*endptr != '\0') { + return -EINVAL; + } + + for (int i = 0; i < count; i++) { + int value = adc_read_id((int)id); + shell_print(shell, "ADC%d value: %d", (int)id, (int)value); + } + + return 0; +} + +static int cmd_adc_scan(const struct shell *shell, size_t argc, char **argv) { + for (int i = 0; i < NUM_ADC_CHANNELS; i++) { + int value = adc_read_id(i); + // shell_print(shell, "ADC%d value: %d", i, (int)value); + char buf[16]; + snprintf(buf, sizeof(buf), "ADC,%d,%d\n\r", i, (int)value); + print_uart(buf); + } + return 0; +} + +int buffer[18][1024]; +uint32_t tick_buffer[18][1024]; +uint32_t tick_speed; +static int cmd_adc_scan_for(const struct shell *shell, size_t argc, char **argv) { + /* Parse ADC id */ + char *endptr; + + long count = strtol(argv[1], &endptr, 10); + + if (*endptr != '\0' || count > 1024) { + return -EINVAL; + } + + tick_speed = sys_clock_hw_cycles_per_sec(); + shell_print(shell, "Systick: %d", (int)tick_speed); + + for (int i = 0; i < count; i++) { + for (int j = 0; j < NUM_ADC_CHANNELS; j++) { + int value = adc_read_id(j); + buffer[j][i] = value; + tick_buffer[j][i] = k_cycle_get_32(); + // shell_print(shell, "ADC%d value: %d", j, (int)value); + // char buf[16]; + // snprintf(buf, sizeof(buf), "%d,ADC,%d,%d\n\r", i, j, (int)value); + // print_uart(buf); + } + } + + for (int i = 0; i < count; i++) { + for (int j = 0; j < NUM_ADC_CHANNELS; j++) { + char buf[32]; + snprintf(buf, sizeof(buf), "%d,ADC,%d,%d,%u\n\r", i, j, (int)buffer[j][i], tick_buffer[j][i]); + print_uart(buf); + } + } + + return 0; +} + +/* Subcommands for the "servo" root command */ +SHELL_STATIC_SUBCMD_SET_CREATE(sub_adc, + SHELL_CMD_ARG(read, NULL, + "Read a single ADC state.\n" + "Usage: adc read \n", + cmd_adc_read, 2, 0), + SHELL_CMD_ARG(read_for, NULL, + "Read X amount of ADC states.\n" + "Usage: adc read_for \n", + cmd_adc_read_for, 3, 0), + SHELL_CMD(scan, NULL, + "Read all ADC channels.\n" + "Usage: adc scan", + cmd_adc_scan), + SHELL_CMD_ARG(scan_for, NULL, + "Read all ADC, X amount of ADC states.\n" + "Usage: adc scan_for \n", + cmd_adc_scan_for, 2, 0), + SHELL_SUBCMD_SET_END +); + +SHELL_CMD_REGISTER(adc, &sub_adc, + "ADC control commands.\n" + " adc read - Read ADC\n" + " adc read_for - Read ADC X amount of times\n" + " adc scan - Read all ADC channels\n" + " adc scan_for - Read all ADC X amount of times\n", NULL); \ No newline at end of file diff --git a/test_code/src/uart.c b/test_code/src/uart.c new file mode 100644 index 0000000..af46a0e --- /dev/null +++ b/test_code/src/uart.c @@ -0,0 +1,32 @@ +#include "uart.h" + +#include +#include + +#include + +LOG_MODULE_REGISTER(uart); + +#define UART_DEVICE_NODE DT_ALIAS(uart1) +static const struct device *const uart_dev = DEVICE_DT_GET(UART_DEVICE_NODE); + +int uart_init(void) { + int ret; + + if (!device_is_ready(uart_dev)) { + LOG_ERR("UART device not ready"); + return -ENODEV; + } + + LOG_INF("UART initialized"); + + return 0; +} + +void print_uart(char *buf) { + int msg_len = strlen(buf); + + for (int i = 0; i < msg_len; i++) { + uart_poll_out(uart_dev, buf[i]); + } +} \ No newline at end of file diff --git a/test_code/src/uart.h b/test_code/src/uart.h new file mode 100644 index 0000000..58c9744 --- /dev/null +++ b/test_code/src/uart.h @@ -0,0 +1,9 @@ +#ifndef UART_H +#define UART_H + + +int uart_init(void); +void print_uart(char *buf); + + +#endif // UART_H \ No newline at end of file