diff --git a/.vscode/settings.json b/.vscode/settings.json index 968ec96..790639d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -3,6 +3,8 @@ "${workspaceFolder}/build_4/PHF000-Firmware": "Launch PHF000-Firmware" }, "files.associations": { - "nrfx_temp.h": "c" + "nrfx_temp.h": "c", + "device.h": "c", + "counter.h": "c" } } \ No newline at end of file diff --git a/boards/Mi_labs/phf000_board/phf000_board.dts b/boards/Mi_labs/phf000_board/phf000_board.dts index 9aa324b..5181f87 100644 --- a/boards/Mi_labs/phf000_board/phf000_board.dts +++ b/boards/Mi_labs/phf000_board/phf000_board.dts @@ -183,4 +183,9 @@ zephyr_udc0: &usbd { &temp { compatible = "nordic,nrf-temp"; status = "okay"; +}; + +/* Enable Timer3 as counter backend */ +&timer3 { + status = "okay"; }; \ No newline at end of file diff --git a/boards/Mi_labs/phf000_board/phf000_board_defconfig b/boards/Mi_labs/phf000_board/phf000_board_defconfig index 257eda8..8a2633c 100644 --- a/boards/Mi_labs/phf000_board/phf000_board_defconfig +++ b/boards/Mi_labs/phf000_board/phf000_board_defconfig @@ -35,4 +35,7 @@ CONFIG_CLOCK_CONTROL_NRF_K32SRC_XTAL=y CONFIG_SENSOR=y CONFIG_MULTITHREADING=y -CONFIG_NRFX_TEMP=y \ No newline at end of file +CONFIG_NRFX_TEMP=y + +CONFIG_COUNTER=y +CONFIG_NRFX_TIMER3=y diff --git a/drivers/actuator/actuator.c b/drivers/actuator/actuator.c index 5bcab22..cd0114c 100644 --- a/drivers/actuator/actuator.c +++ b/drivers/actuator/actuator.c @@ -1,21 +1,191 @@ #include "actuator.h" +#include #include -#include +#include +#include +#include +#include +LOG_MODULE_REGISTER(actuator, LOG_LEVEL_INF); + +/* === GPIOs from devicetree === */ #define USER_NODE DT_PATH(zephyr_user) -static const struct gpio_dt_spec do1 = GPIO_DT_SPEC_GET(USER_NODE, do1_gpios); -static const struct gpio_dt_spec do2 = GPIO_DT_SPEC_GET(USER_NODE, do2_gpios); +static const struct gpio_dt_spec do1 = GPIO_DT_SPEC_GET(USER_NODE, do1_gpios); +static const struct gpio_dt_spec do2 = GPIO_DT_SPEC_GET(USER_NODE, do2_gpios); static const struct gpio_dt_spec do_en = GPIO_DT_SPEC_GET(USER_NODE, do_en_gpios); -void digital_out_init(void) { +/* === Counter device (Timer3) === */ +#define COUNTER_NODE DT_NODELABEL(timer3) +static const struct device *counter_dev = DEVICE_DT_GET(COUNTER_NODE); + +/* === Constants === */ +#define MOTOR_FREQ_HZ 200 +#define MOTOR_PERIOD_US (USEC_PER_SEC / MOTOR_FREQ_HZ) + +#define SERVO_FREQ_HZ 50 +#define SERVO_PERIOD_US (USEC_PER_SEC / SERVO_FREQ_HZ) + +/* Servo duty cycle limits */ +#define SERVO_MIN_PULSE_US (SERVO_PERIOD_US * 5 / 100) /* 5% → 1 ms */ +#define SERVO_MAX_PULSE_US (SERVO_PERIOD_US * 10 / 100) /* 10% → 2 ms */ + +/* === State === */ +static enum actuator_mode current_mode; +static bool initialized = false; +static int motor_speed = 0; /* -100..100 */ +static int servo_angle = 0; /* -90..90 */ + +static struct counter_alarm_cfg alarm_cfg; +static bool active_phase = false; + +/* === Forward declarations === */ +static void pwm_isr(const struct device *dev, + uint8_t chan_id, + uint32_t ticks, + void *user_data); + +/* === Helpers === */ +static int configure_gpio(void) +{ + if (!device_is_ready(do1.port) || !device_is_ready(do2.port) || !device_is_ready(do_en.port)) { + LOG_ERR("GPIO device not ready"); + return -ENODEV; + } + gpio_pin_configure_dt(&do1, GPIO_OUTPUT_INACTIVE); gpio_pin_configure_dt(&do2, GPIO_OUTPUT_INACTIVE); gpio_pin_configure_dt(&do_en, GPIO_OUTPUT_INACTIVE); + return 0; } -void digital_out_set_do1(int state) { gpio_pin_set_dt(&do1, state); } -void digital_out_set_do2(int state) { gpio_pin_set_dt(&do2, state); } -void digital_out_set_do_en(int state) { gpio_pin_set_dt(&do_en, state); } -void digital_out_toggle_do1(void) { gpio_pin_toggle_dt(&do1); } -void digital_out_toggle_do2(void) { gpio_pin_toggle_dt(&do2); } -void digital_out_toggle_do_en(void) { gpio_pin_toggle_dt(&do_en); } +static void start_counter(uint32_t us) +{ + uint32_t now_ticks; + int err = counter_get_value(counter_dev, &now_ticks); + if (err) + { + LOG_ERR("Failed to get counter value (%d)", err); + return; + } + + uint32_t delay_ticks = counter_us_to_ticks(counter_dev, us); + uint32_t next_ticks = now_ticks + delay_ticks; + + alarm_cfg.flags = COUNTER_ALARM_CFG_ABSOLUTE; + alarm_cfg.ticks = next_ticks; + alarm_cfg.callback = pwm_isr; + alarm_cfg.user_data = NULL; + + counter_set_channel_alarm(counter_dev, 0, &alarm_cfg); +} + +/* === ISR callback === */ +static void pwm_isr(const struct device *dev, + uint8_t chan_id, + uint32_t ticks, + void *user_data) +{ + ARG_UNUSED(dev); + ARG_UNUSED(chan_id); + ARG_UNUSED(ticks); + ARG_UNUSED(user_data); + + if (current_mode == ACTUATOR_MODE_MOTOR) { + if (motor_speed == 0) { + /* Brake mode */ + gpio_pin_set_dt(&do1, 1); + gpio_pin_set_dt(&do2, 1); + return; + } + + if (!active_phase) { + /* Start ON phase */ + if (motor_speed > 0) { + gpio_pin_set_dt(&do1, 1); + gpio_pin_set_dt(&do2, 0); + } else { + gpio_pin_set_dt(&do1, 0); + gpio_pin_set_dt(&do2, 1); + } + uint32_t duty_us = (MOTOR_PERIOD_US * abs(motor_speed)) / 100; + start_counter(duty_us); + active_phase = true; + } else { + /* Start OFF (coast) phase */ + gpio_pin_set_dt(&do1, 0); + gpio_pin_set_dt(&do2, 0); + uint32_t off_time = MOTOR_PERIOD_US - (MOTOR_PERIOD_US * abs(motor_speed)) / 100; + start_counter(off_time); + active_phase = false; + } + } + else if (current_mode == ACTUATOR_MODE_SERVO) { + if (!active_phase) { + /* Start ON phase */ + gpio_pin_set_dt(&do1, 1); + gpio_pin_set_dt(&do2, 1); + uint32_t duty_us = SERVO_MIN_PULSE_US + + (servo_angle + 90) * (SERVO_MAX_PULSE_US - SERVO_MIN_PULSE_US) / 180; + start_counter(duty_us); + active_phase = true; + } else { + /* Start OFF phase */ + gpio_pin_set_dt(&do2, 0); + uint32_t off_time = SERVO_PERIOD_US - + (SERVO_MIN_PULSE_US + (servo_angle + 90) * (SERVO_MAX_PULSE_US - SERVO_MIN_PULSE_US) / 180); + start_counter(off_time); + active_phase = false; + } + } +} + +/* === API implementation === */ +int actuator_init(enum actuator_mode mode) +{ + int ret = configure_gpio(); + if (ret) return ret; + + if (!device_is_ready(counter_dev)) { + LOG_ERR("Counter device not ready"); + return -ENODEV; + } + + counter_start(counter_dev); + + current_mode = mode; + initialized = true; + active_phase = false; + LOG_INF("Actuator initialized in mode %d", mode); + + return 0; +} + +int actuator_enable(bool enable) +{ + if (!initialized) return -EACCES; + return gpio_pin_set_dt(&do_en, enable ? 1 : 0); +} + +int actuator_motor_set_speed(int speed_percent) +{ + if (!initialized || current_mode != ACTUATOR_MODE_MOTOR) return -EACCES; + if (speed_percent < -100) speed_percent = -100; + if (speed_percent > 100) speed_percent = 100; + + motor_speed = speed_percent; + active_phase = false; + start_counter(1); /* trigger ISR soon */ + return 0; +} + +int actuator_servo_set_angle(int angle_deg) +{ + if (!initialized || current_mode != ACTUATOR_MODE_SERVO) return -EACCES; + if (angle_deg < -90) angle_deg = -90; + if (angle_deg > 90) angle_deg = 90; + + servo_angle = angle_deg; + active_phase = false; + start_counter(1); /* trigger ISR soon */ + return 0; +} diff --git a/drivers/actuator/actuator.h b/drivers/actuator/actuator.h index c6e8357..440fc3f 100644 --- a/drivers/actuator/actuator.h +++ b/drivers/actuator/actuator.h @@ -1,12 +1,25 @@ -#ifndef ACTUATOR_H -#define ACTUATOR_H +#ifndef ACTUATOR_H_ +#define ACTUATOR_H_ -void digital_out_init(void); -void digital_out_set_do1(int state); -void digital_out_set_do2(int state); -void digital_out_set_do_en(int state); -void digital_out_toggle_do1(void); -void digital_out_toggle_do2(void); -void digital_out_toggle_do_en(void); +#include +#include +#include +#include -#endif // ACTUATOR_H +/* Actuator modes */ +enum actuator_mode { + ACTUATOR_MODE_MOTOR = 0, + ACTUATOR_MODE_SERVO, +}; + +/* Public API */ +int actuator_init(enum actuator_mode mode); +int actuator_enable(bool enable); + +/* Motor mode */ +int actuator_motor_set_speed(int speed_percent); // -100..100 % + +/* Servo mode */ +int actuator_servo_set_angle(int angle_deg); // -90..90 degrees + +#endif /* ACTUATOR_H_ */ diff --git a/prj.conf b/prj.conf index faa0667..9e83858 100644 --- a/prj.conf +++ b/prj.conf @@ -21,4 +21,6 @@ CONFIG_USB_CDC_ACM=y CONFIG_USB_DEVICE_PRODUCT="Zephyr USB console sample" # STEP 1.1 - Enable MCUboot -CONFIG_BOOTLOADER_MCUBOOT=y \ No newline at end of file +CONFIG_BOOTLOADER_MCUBOOT=y + +CONFIG_COUNTER=y \ No newline at end of file diff --git a/src/main.c b/src/main.c index dde1032..84e7261 100644 --- a/src/main.c +++ b/src/main.c @@ -27,10 +27,13 @@ int main(void) return 0; } + actuator_init(ACTUATOR_MODE_MOTOR); + actuator_enable(1); + + actuator_motor_set_speed(50); // -100..100 % /* Init modules */ led_init(); button_init(); - digital_out_init(); ret = battery_adc_init(); if (ret < 0) { @@ -95,9 +98,6 @@ int main(void) if (old_val != val) { out_en = !out_en; - digital_out_toggle_do1(); - digital_out_toggle_do2(); - digital_out_toggle_do_en(); led_toggle(1); // toggle LED1 on press old_val = val; }