From 36895b35aac5753ceea35998644358a604faf5b1 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Thu, 31 Oct 2024 11:54:48 +0100 Subject: [PATCH 01/13] Add open bot brain Zephyr primitives --- platforms/Zephyr/CMakeLists.txt | 3 + .../Zephyr/boards/stm32l496g_disco.overlay | 258 +++--- platforms/Zephyr/main.cpp | 23 +- src/Primitives/zephyr.cpp | 742 +++++++++++++++++- 4 files changed, 877 insertions(+), 149 deletions(-) diff --git a/platforms/Zephyr/CMakeLists.txt b/platforms/Zephyr/CMakeLists.txt index b17ff7fd..0ed056c0 100644 --- a/platforms/Zephyr/CMakeLists.txt +++ b/platforms/Zephyr/CMakeLists.txt @@ -2,6 +2,9 @@ cmake_minimum_required(VERSION 3.20.0) find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) project(warduino) +# Note on _POSIX_C_SOURCE: If you define this macro to a value greater than or equal to 200809L, then the functionality from the 2008 edition of the POSIX standard (IEEE Standard 1003.1-2008) is made available. +target_compile_definitions(app PRIVATE _POSIX_C_SOURCE=200809L) + add_custom_command( OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/upload.h COMMAND xxd -i upload.wasm > upload.h diff --git a/platforms/Zephyr/boards/stm32l496g_disco.overlay b/platforms/Zephyr/boards/stm32l496g_disco.overlay index 71d4ca49..01aa7098 100644 --- a/platforms/Zephyr/boards/stm32l496g_disco.overlay +++ b/platforms/Zephyr/boards/stm32l496g_disco.overlay @@ -1,132 +1,148 @@ #include "../app.overlay" / { - zephyr,user { - warduino-gpios = - <&gpioa 0 0>, - <&gpioa 1 0>, - <&gpioa 2 0>, - <&gpioa 3 0>, - <&gpioa 4 0>, - <&gpioa 5 0>, - <&gpioa 6 0>, - <&gpioa 7 0>, - <&gpioa 8 0>, - <&gpioa 9 0>, - <&gpioa 10 0>, - <&gpioa 11 0>, - <&gpioa 12 0>, - <&gpioa 15 0>, - <&gpiob 0 0>, - <&gpiob 1 0>, - <&gpiob 2 0>, - <&gpiob 3 0>, - <&gpiob 4 0>, - <&gpiob 5 0>, - <&gpiob 6 0>, - <&gpiob 7 0>, - <&gpiob 8 0>, - <&gpiob 9 0>, - <&gpiob 12 0>, - <&gpiob 13 0>, - <&gpiob 14 0>, - <&gpiob 15 0>, - <&gpioc 0 0>, - <&gpioc 1 0>, - <&gpioc 2 0>, - <&gpioc 3 0>, - <&gpioc 4 0>, - <&gpioc 5 0>, - <&gpioc 6 0>, - <&gpioc 7 0>, - <&gpioc 8 0>, - <&gpioc 9 0>, - <&gpioc 12 0>, - <&gpioc 15 0>, - <&gpiod 0 0>, - <&gpiod 1 0>, - <&gpiod 2 0>, - <&gpiod 3 0>, - <&gpiod 4 0>, - <&gpiod 7 0>, - <&gpiod 8 0>, - <&gpiod 9 0>, - <&gpiod 10 0>, - <&gpiod 11 0>, - <&gpiod 12 0>, - <&gpiod 13 0>, - <&gpiod 14 0>, - <&gpiod 15 0>, - <&gpioe 0 0>, - <&gpioe 1 0>, - <&gpioe 2 0>, - <&gpioe 3 0>, - <&gpioe 4 0>, - <&gpioe 7 0>, - <&gpioe 8 0>, - <&gpioe 10 0>, - <&gpioe 11 0>, - <&gpioe 12 0>, - <&gpioe 13 0>, - <&gpioe 14 0>, - <&gpioe 15 0>, - <&gpiof 3 0>, - <&gpiof 4 0>, - <&gpiof 5 0>, - <&gpiof 6 0>, - <&gpiof 7 0>, - <&gpiof 8 0>, - <&gpiof 9 0>, - <&gpiof 10 0>, - <&gpiof 11 0>, - <&gpiof 12 0>, - <&gpiof 13 0>, - <&gpiof 14 0>, - <&gpiog 2 0>, - <&gpiog 3 0>, - <&gpiog 4 0>, - <&gpiog 5 0>, - <&gpiog 6 0>, - <&gpiog 7 0>, - <&gpiog 8 0>, - <&gpiog 11 0>, - <&gpiog 12 0>, - <&gpiog 15 0>, - <&gpiob 10 0>, - <&gpiob 11 0>, - <&gpioc 10 0>, - <&gpioc 11 0>, - <&gpiog 13 0>, - <&gpiog 14 0>, - <&gpiod 5 0>, - <&gpiod 6 0>, - <&gpiof 0 0>, - <&gpiof 1 0>, - <&gpiog 9 0>, - <&gpiog 10 0>; + zephyr,user { + warduino-gpios = + <&gpioa 0 0>, + <&gpioa 1 0>, + <&gpioa 2 0>, + <&gpioa 3 0>, + <&gpioa 4 0>, + <&gpioa 5 0>, + <&gpioa 6 0>, + <&gpioa 7 0>, + <&gpioa 8 0>, + <&gpioa 9 0>, + <&gpioa 10 0>, + <&gpioa 11 0>, + <&gpioa 12 0>, + <&gpioa 15 0>, + <&gpiob 0 0>, + <&gpiob 1 0>, + <&gpiob 2 0>, + <&gpiob 3 0>, + <&gpiob 4 0>, + <&gpiob 5 0>, + <&gpiob 6 0>, + <&gpiob 7 0>, + <&gpiob 8 0>, + <&gpiob 9 0>, + <&gpiob 12 0>, + <&gpiob 13 0>, + <&gpiob 14 0>, + <&gpiob 15 0>, + <&gpioc 0 0>, + <&gpioc 1 0>, + <&gpioc 2 0>, + <&gpioc 3 0>, + <&gpioc 4 0>, + <&gpioc 5 0>, + <&gpioc 6 0>, + <&gpioc 7 0>, + <&gpioc 8 0>, + <&gpioc 9 0>, + <&gpioc 12 0>, + <&gpioc 15 0>, + <&gpiod 0 0>, + <&gpiod 1 0>, + <&gpiod 2 0>, + <&gpiod 3 0>, + <&gpiod 4 0>, + <&gpiod 7 0>, + <&gpiod 8 0>, + <&gpiod 9 0>, + <&gpiod 10 0>, + <&gpiod 11 0>, + <&gpiod 12 0>, + <&gpiod 13 0>, + <&gpiod 14 0>, + <&gpiod 15 0>, + <&gpioe 0 0>, + <&gpioe 1 0>, + <&gpioe 2 0>, + <&gpioe 3 0>, + <&gpioe 4 0>, + <&gpioe 7 0>, + <&gpioe 8 0>, + <&gpioe 10 0>, + <&gpioe 11 0>, + <&gpioe 12 0>, + <&gpioe 13 0>, + <&gpioe 14 0>, + <&gpioe 15 0>, + <&gpiof 3 0>, + <&gpiof 4 0>, + <&gpiof 5 0>, + <&gpiof 6 0>, + <&gpiof 7 0>, + <&gpiof 8 0>, + <&gpiof 9 0>, + <&gpiof 10 0>, + <&gpiof 11 0>, + <&gpiof 12 0>, + <&gpiof 13 0>, + <&gpiof 14 0>, + <&gpiog 2 0>, + <&gpiog 3 0>, + <&gpiog 4 0>, + <&gpiog 5 0>, + <&gpiog 6 0>, + <&gpiog 7 0>, + <&gpiog 8 0>, + <&gpiog 11 0>, + <&gpiog 12 0>, + <&gpiog 15 0>, + <&gpiob 10 0>, + <&gpiob 11 0>, + <&gpioc 10 0>, + <&gpioc 11 0>, + <&gpiog 13 0>, + <&gpiog 14 0>, + <&gpiod 5 0>, + <&gpiod 6 0>, + <&gpiof 0 0>, + <&gpiof 1 0>, + <&gpiog 9 0>, + <&gpiog 10 0>; - pwms = - <&pwm8 3 10000 PWM_POLARITY_NORMAL>, - <&pwm8 4 10000 PWM_POLARITY_NORMAL>, - <&pwm8 1 10000 PWM_POLARITY_NORMAL>, - <&pwm8 2 10000 PWM_POLARITY_NORMAL>; - - warduino-uarts = - <&usart1>; - }; + pwms = + <&pwm8 3 10000 PWM_POLARITY_NORMAL>, + <&pwm8 4 10000 PWM_POLARITY_NORMAL>, + <&pwm8 1 10000 PWM_POLARITY_NORMAL>, + <&pwm8 2 10000 PWM_POLARITY_NORMAL>, + <&pwm1 3 10000 PWM_POLARITY_NORMAL>, + <&pwm1 4 10000 PWM_POLARITY_NORMAL>, + <&pwm1 1 10000 PWM_POLARITY_NORMAL>, + <&pwm1 2 10000 PWM_POLARITY_NORMAL>; + + warduino-uarts = + <&usart1>; + }; }; &timers8 { - status = "okay"; - pwm8: pwm { - status = "okay"; - pinctrl-0 = <&tim8_ch3_pc8 &tim8_ch4_pc9 &tim8_ch1_pc6 &tim8_ch2_pc7>; - pinctrl-names = - "default"; - }; + status = "okay"; + + pwm8: pwm { + status = "okay"; + pinctrl-0 = <&tim8_ch3_pc8 &tim8_ch4_pc9 &tim8_ch1_pc6 &tim8_ch2_pc7>; + pinctrl-names = + "default"; + }; +}; + +&timers1 { + status = "okay"; + + pwm1: pwm { + status = "okay"; + pinctrl-0 = <&tim1_ch3_pe13 &tim1_ch4_pe14 &tim1_ch1_pa8 &tim1_ch2_pa9>; + pinctrl-names = + "default"; + }; }; &usart1 { - pinctrl-0 = <&usart1_tx_pg9 &usart1_rx_pg10>; - current-speed = <2400>; + pinctrl-0 = <&usart1_tx_pg9 &usart1_rx_pg10>; + current-speed = <2400>; }; diff --git a/platforms/Zephyr/main.cpp b/platforms/Zephyr/main.cpp index 2640d541..82af5fa2 100644 --- a/platforms/Zephyr/main.cpp +++ b/platforms/Zephyr/main.cpp @@ -55,21 +55,26 @@ int war_console_init(void) { WARDuino *wac = WARDuino::instance(); +int valread; +uint8_t buffer[1024] = {0}; + +void read_debug_messages() { + while ((valread = war_console_read(NULL, buffer, 1024)) > 0) { + wac->handleInterrupt(valread, buffer); + } +} + void startDebuggerStd() { Channel *duplex = new Duplex(stdin, stdout); wac->debugger->setChannel(duplex); duplex->open(); war_console_init(); - int valread; - uint8_t buffer[1024] = {0}; - while (true) { - k_msleep(1000); - - while ((valread = war_console_read(NULL, buffer, 1024)) > 0) { - wac->handleInterrupt(valread, buffer); - } - } + /*while (true) { + k_msleep(500); + //printf("From debugger thread!\n"); + read_debug_messages(); + }*/ } K_THREAD_DEFINE(debugger_tid, DEBUGGER_STACK_SIZE, startDebuggerStd, NULL, NULL, diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index d7273b0d..f6717422 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -33,7 +33,7 @@ #include "../Utils/util.h" #include "primitives.h" -#define ALL_PRIMITIVES 5 +#define ALL_PRIMITIVES 12 // Global index for installing primitives int prim_index = 0; @@ -223,6 +223,7 @@ Type NoneToOneU64 = {.form = FUNC, .mask = 0x82000}; def_prim(chip_delay, oneToNoneU32) { + k_yield(); k_msleep(arg0.uint32); pop_args(1); return true; @@ -231,6 +232,31 @@ def_prim(chip_delay, oneToNoneU32) { struct gpio_dt_spec specs[] = {DT_FOREACH_PROP_ELEM_SEP( DT_PATH(zephyr_user), warduino_gpios, GPIO_DT_SPEC_GET_BY_IDX, (, ))}; +/*static const struct pwm_dt_spec pwm_led0 = +PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 0); static const struct pwm_dt_spec +pwm_led1 = PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 1);*/ +static const struct pwm_dt_spec pwm_led0 = + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 2); +static const struct pwm_dt_spec pwm_led1 = + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 3); + +// TODO: Use Zephyr FOREACH macro here +const struct pwm_dt_spec pwm_specs[] = { + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 0), + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 1), + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 2), + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 3), + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 4), + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 5), + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 6), + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 7), +}; + +/*struct gpio_dt_spec pwm_specs[] = { + DT_FOREACH_PROP_ELEM_SEP(DT_PATH(zephyr_user), pmws, + PWM_DT_SPEC_GET_BY_IDX, (,)) +};*/ + def_prim(chip_pin_mode, twoToNoneU32) { gpio_dt_spec pin_spec = specs[arg1.uint32]; gpio_pin_configure(pin_spec.port, pin_spec.pin, @@ -239,14 +265,40 @@ def_prim(chip_pin_mode, twoToNoneU32) { return true; } +std::unordered_map io_map; + def_prim(chip_digital_write, twoToNoneU32) { printf("chip_digital_write(%u,%u)\n", arg1.uint32, arg0.uint32); gpio_dt_spec pin_spec = specs[arg1.uint32]; gpio_pin_set_raw(pin_spec.port, pin_spec.pin, arg0.uint32); + io_map[arg1.uint32] = arg0.uint32; pop_args(2); return true; } +def_prim_serialize(chip_digital_write) { + for (auto pair : io_map) { + IOStateElement *state = new IOStateElement(); + state->output = true; + state->key = "p" + std::to_string(pair.first); + state->value = pair.second; + external_state.push_back(state); + } +} + +def_prim_reverse(chip_digital_write) { + for (IOStateElement state : external_state) { + if (!state.output) { + continue; + } + + if (state.key[0] == 'p') { + invoke_primitive(m, "chip_digital_write", stoi(state.key.substr(1)), + (uint32_t)state.value); + } + } +} + def_prim(chip_digital_read, oneToOneU32) { printf("chip_digital_read(%u)\n", arg0.uint32); gpio_dt_spec pin_spec = specs[arg0.uint32]; @@ -262,6 +314,653 @@ def_prim(print_int, oneToNoneU32) { return true; } +bool drive_pwm(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float pwm1, + float pwm2) { + if (!pwm_is_ready_dt(&pwm1_spec)) { + printf("Error: PWM device %s is not ready\n", pwm1_spec.dev->name); + return false; + } + + int ret = pwm_set_pulse_dt(&pwm1_spec, pwm1 * pwm1_spec.period); + if (ret) { + printf("Error %d: failed to set pulse width, pwm1 = %f\n", ret, pwm1); + return false; + } + + ret = pwm_set_pulse_dt(&pwm2_spec, pwm2 * pwm2_spec.period); + if (ret) { + printf("Error %d: failed to set pulse width, pwm2 = %f\n", ret, pwm2); + return false; + } + return true; +} + +bool drive_motor(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float speed) { + float pwm1 = 0; + float pwm2 = 0; + if (speed > 0) { + pwm1 = speed; + } else { + pwm2 = -speed; + } + + return drive_pwm(pwm1_spec, pwm2_spec, pwm1, pwm2); +} + +def_prim(motor_test, oneToNoneU32) { + int32_t speed = (int32_t)arg0.uint32; + printf("motor_test(%d)\n", speed); + drive_motor(pwm_led0, pwm_led1, speed / 10000.0f); + pop_args(1); + return true; +} + +def_prim(drive_motor_for_ms, twoToNoneU32) { + int32_t speed = (int32_t)arg0.uint32; + printf("drive_motor_for_ms(%d, %d)\n", speed, arg1.uint32); + + drive_motor(pwm_led0, pwm_led1, speed / 10000.0f); + k_msleep(arg1.uint32); + drive_pwm(pwm_led0, pwm_led1, 1.0f, 1.0f); + pop_args(2); + return true; +} + +class MotorEncoder { + static void encoder_pin5_edge_rising(const struct device *dev, + struct gpio_callback *cb, + uint32_t pins) { + MotorEncoder *encoder = + CONTAINER_OF(cb, MotorEncoder, pin5_encoder_cb_data); + if (!encoder->expect_pin5_int) return; + + if (!gpio_pin_get_raw(encoder->pin6_encoder_spec.port, + encoder->pin6_encoder_spec.pin)) { + encoder->angle++; + } else { + encoder->angle--; + } + encoder->last_update = k_uptime_get(); + // printk("Rising edge detected on encoder pin5, angle %d\n", + // encoder->angle); printk("%d\n", + // gpio_pin_get_raw(encoder->pin6_encoder_spec.port, + // encoder->pin6_encoder_spec.pin)); + encoder->expect_pin5_int = false; + encoder->expect_pin6_int = true; + } + + static void encoder_pin6_edge_rising(const struct device *dev, + struct gpio_callback *cb, + uint32_t pins) { + MotorEncoder *encoder = + CONTAINER_OF(cb, MotorEncoder, pin6_encoder_cb_data); + if (!encoder->expect_pin6_int) return; + + if (gpio_pin_get_raw(encoder->pin5_encoder_spec.port, + encoder->pin5_encoder_spec.pin)) { + encoder->angle++; + } else { + encoder->angle--; + } + // printk("Rising edge detected on encoder pin6, angle %d\n", + // encoder->angle); printk("%d\n", + // gpio_pin_get_raw(encoder->pin5_encoder_spec.port, + // encoder->pin5_encoder_spec.pin)); + encoder->expect_pin6_int = false; + encoder->expect_pin5_int = true; + } + + public: + MotorEncoder(gpio_dt_spec pin5_encoder_spec, gpio_dt_spec pin6_encoder_spec, const char *name) + : pin5_encoder_spec(pin5_encoder_spec), + pin6_encoder_spec(pin6_encoder_spec), + angle(0), + target_angle(0), + expect_pin5_int(true), + expect_pin6_int(true), + last_update(0) { + if (gpio_pin_configure_dt(&pin5_encoder_spec, GPIO_INPUT)) { + FATAL("Failed to configure GPIO encoder pin5\n"); + } + if (gpio_pin_configure_dt(&pin6_encoder_spec, GPIO_INPUT)) { + FATAL("Failed to configure GPIO encoder pin6\n"); + } + + int result = gpio_pin_interrupt_configure_dt(&pin5_encoder_spec, + GPIO_INT_EDGE_RISING); + if (result != 0) { + printf("Failed to configure interrupt on pin5 for %s, error code %d\n", name, result); + } + gpio_init_callback(&pin5_encoder_cb_data, + MotorEncoder::encoder_pin5_edge_rising, + BIT(pin5_encoder_spec.pin)); + gpio_add_callback(pin5_encoder_spec.port, &pin5_encoder_cb_data); + + result = gpio_pin_interrupt_configure_dt(&pin6_encoder_spec, + GPIO_INT_EDGE_RISING); + if (result != 0) { + printf("Failed to configure interrupt on pin6 for %s, error code %d\n", name, result); + } + gpio_init_callback(&pin6_encoder_cb_data, + MotorEncoder::encoder_pin6_edge_rising, + BIT(pin6_encoder_spec.pin)); + gpio_add_callback(pin6_encoder_spec.port, &pin6_encoder_cb_data); + } + + ~MotorEncoder() { + gpio_remove_callback(pin5_encoder_spec.port, &pin5_encoder_cb_data); + gpio_remove_callback(pin6_encoder_spec.port, &pin6_encoder_cb_data); + } + + int get_angle() { return angle; } + + void reset_angle() { angle = 0; } + + int get_target_angle() { return target_angle; } + + void set_target_angle(int target_angle) { + this->target_angle = target_angle; + } + + int64_t get_last_update() { return last_update; } + + private: + gpio_dt_spec pin5_encoder_spec; + gpio_dt_spec pin6_encoder_spec; + struct gpio_callback pin5_encoder_cb_data; + struct gpio_callback pin6_encoder_cb_data; + volatile int angle; + int target_angle; + bool expect_pin5_int; + bool expect_pin6_int; + + public: + volatile int64_t last_update; +}; + +// MotorEncoder encoder(specs[51], specs[50]); +// MotorEncoder encoder(specs[57], specs[58]); +// MotorEncoder encoder(specs[58], specs[57]); + +MotorEncoder *encoders[] = {new MotorEncoder(specs[51], specs[50], "Port A"), + new MotorEncoder(specs[57], specs[58], "Port B"), + //MotorEncoder(specs[17], specs[13], "Port C"), // TODO: Disable when using motor D, causes conflict with interrupts + nullptr, + new MotorEncoder(specs[27], specs[26], "Port D")}; +// MotorEncoder test_encoder = MotorEncoder(specs[57], specs[58]); + +void drive_motor_to_target(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, MotorEncoder *encoder, int32_t speed) { + printf("drift = %d\n", + abs(encoder->get_angle() - encoder->get_target_angle())); + + int drift = encoder->get_angle() - encoder->get_target_angle(); + // Reset stall timer, otherwise it will instantly think it's not moving. + encoder->last_update = k_uptime_get(); + while (abs(drift) > 0) { + int speed_sign = std::signbit(drift) ? -1 : 1; + drive_motor(pwm1_spec, pwm2_spec, speed_sign * speed / 10000.0f); + while (speed_sign * + (encoder->get_angle() - encoder->get_target_angle()) > + 0 && + k_uptime_get() - encoder->get_last_update() < 150) { + } + bool not_moving = k_uptime_get() - encoder->get_last_update() >= 150; + if (not_moving) { + speed += 100; + printf("Not moving, increasing speed to %d, %llims since last movement\n", speed, k_uptime_get() - encoder->get_last_update()); + drive_motor(pwm1_spec, pwm2_spec, speed_sign * speed / 10000.0f); + + // Wait for 10ms or movement. + uint64_t start_time = k_uptime_get(); + while (k_uptime_get() - start_time < 10 && k_uptime_get() - encoder->get_last_update() >= 150) {} + continue; + } + encoder->last_update = k_uptime_get(); + printf("%lli\n", k_uptime_get() - encoder->get_last_update()); + /*printf("PWM device %s\n", pwm1_spec.dev->name); + printf("PWM device %s\n", pwm2_spec.dev->name);*/ + drive_pwm(pwm1_spec, pwm2_spec, 1.0f, 1.0f); + k_msleep(50); + drift = encoder->get_angle() - encoder->get_target_angle(); + printf("drift = %d, speed = %d\n", drift, speed); + // speed = std::max(775, speed - speed/3); // Reduce speed when going + // fast to do corrections speed = 775; + if (not_moving) { + speed += 100; + } else { + speed = 800; + } + } +} + +bool drive_motor_degrees_relative(int32_t motor_index, int32_t degrees, int32_t speed) { + printf("drive_motor_degrees(%d, %d, %d)\n", motor_index, degrees, speed); + + if (motor_index > 3) { + return false; + } + + pwm_dt_spec pwm1_spec = pwm_specs[motor_index * 2]; + pwm_dt_spec pwm2_spec = pwm_specs[motor_index * 2 + 1]; + MotorEncoder *encoder = encoders[motor_index]; + + encoder->set_target_angle(encoder->get_target_angle() + degrees); + + drive_motor_to_target(pwm1_spec, pwm2_spec, encoder, speed); + return true; +} + +def_prim(drive_motor_degrees, threeToNoneU32) { + int32_t speed = arg0.int32; + int32_t degrees = arg1.uint32; + int32_t motor_index = arg2.uint32; + pop_args(3); + return drive_motor_degrees_relative(motor_index, degrees, speed); +} + +def_prim_reverse(drive_motor_degrees) { + for (IOStateElement state : external_state) { + if (!state.output) { + continue; + } + + if (state.key[0] == 'e') { + printf("Motor target location %d\n", state.value); + int motor_index = stoi(state.key.substr(1)); + // TODO: This is a hack, we should take snapshots before calling + // primitives instead of after and just not restore io when + // restoring the last snapshot and transfer overrides from a future + // snapshot when doing forward execution. + invoke_primitive(m, "drive_motor_degrees_absolute", + motor_index, (uint32_t)state.value, + motor_index == 0 ? 10000 : 2000); + } + } +} + +def_prim_serialize(drive_motor_degrees) { + for (int i = 0; i < 2; i++) { + IOStateElement *state = new IOStateElement(); + state->output = true; + state->key = "e" + std::to_string(i); + state->value = encoders[i]->get_target_angle(); + external_state.push_back(state); + } +} + +def_prim(drive_motor_degrees_absolute, threeToNoneU32) { + int32_t speed = (int32_t)arg0.uint32; + int32_t degrees = (int32_t)arg1.uint32; + int32_t motor_index = (int32_t)arg2.uint32; + printf("drive_motor_degrees_absolute(%d, %d, %d)\n", motor_index, degrees, + speed); + + if (motor_index > 1) { + printf("Invalid motor index %d\n", motor_index); + pop_args(3); + return true; + } + + pwm_dt_spec pwm1_spec = pwm_specs[motor_index * 2]; + pwm_dt_spec pwm2_spec = pwm_specs[motor_index * 2 + 1]; + MotorEncoder *encoder = encoders[motor_index]; + + encoder->set_target_angle(degrees); + + drive_motor_to_target(pwm1_spec, pwm2_spec, encoder, speed); + + pop_args(3); + return true; +} + +def_prim(drive_motor, threeToNoneU32) { + int32_t brake = (int32_t)arg0.uint32; + int32_t speed = (int32_t)arg1.uint32; + int32_t motor_index = (int32_t)arg2.uint32; + + printf("drive_motor(%d, %d, %d)\n", motor_index, speed, brake); + + if (motor_index > 3) { + FATAL("Motor index out of bounds!"); + return false; + } + + pwm_dt_spec pwm1_spec = pwm_specs[motor_index * 2]; + pwm_dt_spec pwm2_spec = pwm_specs[motor_index * 2 + 1]; + + drive_motor(pwm1_spec, pwm2_spec, speed / 10000.0f); + + if (speed == 0 && brake == 1) { + drive_pwm(pwm1_spec, pwm2_spec, 1.0f, 1.0f); + } + + pop_args(3); + return true; +} + +static const struct device *const uart_dev = + DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); +volatile int payload_bytes = 0; +int payload_index = 0; +unsigned int current_payload = 0; +char checksum; +int baudrate = -1; +int bytes_received = 0; + +uint8_t mode = 2; +int mode0_format_location = 0; + +volatile bool data_mode = false; +bool data_byte = false; +volatile int sensor_value = 0; + +void serial_cb(const struct device *dev, void *user_data) { + uint8_t data; + + if (!uart_irq_update(uart_dev)) { + return; + } + + if (!uart_irq_rx_ready(uart_dev)) { + return; + } + + /* read until FIFO empty */ + while (uart_fifo_read(uart_dev, &data, 1) == 1) { + bytes_received++; + /*printk("0x%02x '%c' ", data, (char) data); + printk("0b"); + for (int i = 7; i >= 0; i--) { + printk("%d", (data & 1 << i) > 0); + } + printk("\n");*/ + + if (payload_bytes > 0) { + printk("payload = %d\n", data); + // Print in binary: + printk("0b"); + for (int i = 7; i >= 0; i--) { + printk("%d", (data & 1 << i) > 0); + } + printk("\n"); + payload_bytes--; + + if (payload_bytes > 1) { + checksum ^= data; + printk("before current_payload = %d\n", current_payload); + current_payload = current_payload | + (((unsigned long)data) << payload_index * 8); + payload_index++; + printk("shift = %d, current_payload = %d\n", payload_index * 8, + (int)current_payload); + // printk("checksum = %d\n", checksum); + } else if (checksum == data) { + printk("Checksum matches!\n"); + printk("Baudrate = %d\n", current_payload); + baudrate = current_payload; // TODO: Set actual baudrate here + } + } + + // If data is ACK send an ACK back. + /*if (data == 0b00000100) { + printk("Bytes received = %d\n", bytes_received); + uart_poll_out(uart_dev, 0b00000100); + uart_poll_out(uart_dev, 0b00000010); + printk("Sent ACK\n"); + + // If we received a baudrate, change it after sending the ACK. + if (baudrate >= 0) { + printk("Changing baudrate to %d\n", baudrate); + uart_config cfg; + uart_config_get(uart_dev, &cfg); + cfg.baudrate = baudrate; + + int config_err = uart_configure(uart_dev, &cfg); + printk("config_err = %d\n", config_err); + if (config_err) { + printk("UART configure error %d", config_err); + } + + uart_config_get(uart_dev, &cfg); + printk("current baudrate after config change = %d\n", + cfg.baudrate); + } + }*/ + + // When we receive an ACK message. + if (data == 0b00000100) { + // printk("%d\n", bytes_received - mode0_format_location); + if (bytes_received - mode0_format_location == 7 && baudrate >= 0) { + printk("SPECIAL_LINE\n"); + + uart_poll_out(uart_dev, 0b00000100); // Send ACK + data_mode = true; + + // Change baudrate: + /*printk("Changing baudrate to %d\n", baudrate); + uart_config cfg; + uart_config_get(uart_dev, &cfg); + cfg.baudrate = baudrate; + + int config_err = uart_configure(uart_dev, &cfg); + printk("config_err = %d\n", config_err); + if (config_err) { + printk("UART configure error %d", config_err); + } + + config_err = uart_config_get(uart_dev, &cfg); + printk("current baudrate after config change = %d\n", + cfg.baudrate); printk("config_err = %d\n", config_err);*/ + + /*while (true) { + k_msleep(100); + printk("Send NACK\n"); + uart_poll_out(uart_dev, 0b00000010); + }*/ + } + } + + // HACK + if (data == 0b10010000 && baudrate >= 0) { + mode0_format_location = bytes_received; + // Receive last bits of data and ACK it: + /*k_msleep(50); + uart_poll_out(uart_dev, 0b00000100); // Send ACK + + // Change baudrate: + printk("Changing baudrate to %d\n", baudrate); + uart_config cfg; + uart_config_get(uart_dev, &cfg); + cfg.baudrate = baudrate; + + int config_err = uart_configure(uart_dev, &cfg); + printk("config_err = %d\n", config_err); + if (config_err) { + printk("UART configure error %d", config_err); + } + + uart_config_get(uart_dev, &cfg); + printk("current baudrate after config change = %d\n", + cfg.baudrate);*/ + } + + if (data_mode) { + if (data_byte) { + sensor_value = data; + data_byte = false; + } + + // Check if it' a data message. + if (0b11000000 & data) { + // Next byte will be data + data_byte = true; + } + } + + if (data == 0b01010010) { + printk("Speed command\n"); + payload_bytes = 5; + payload_index = 0; + current_payload = 0; + checksum = 0xff ^ 0b01010010; + + // EV3 Colour sensor sent 57600 as it's baudrate + } + } + // uart_poll_out(uart_dev, 0b00000100); + // uart_poll_out(uart_dev, 0b00000010); + // printk("Sent ACK\n"); +} + + +bool baudrate_configured = false; + +extern void read_debug_messages(); + +void uartHeartbeat() { + if (data_mode && !baudrate_configured) { + printk("Changing baudrate to %d\n", baudrate); + uart_config cfg; + uart_config_get(uart_dev, &cfg); + cfg.baudrate = baudrate; + + int config_err = uart_configure(uart_dev, &cfg); + printk("config_err = %d\n", config_err); + if (config_err) { + printk("UART configure error %d", config_err); + } + + config_err = uart_config_get(uart_dev, &cfg); + printk("current baudrate after config change = %d\n", cfg.baudrate); + printk("config_err = %d\n", config_err); + baudrate_configured = true; + + // Change to mode 2 + uart_poll_out(uart_dev, 0x43); + uart_poll_out(uart_dev, mode); + uart_poll_out(uart_dev, 0xff ^ 0x43 ^ mode); + } + + if (data_mode && baudrate_configured) { + //k_msleep(100); + // printk("Send NACK\n"); + uart_poll_out(uart_dev, 0b00000010); + + // printk("sensor_value = %d\n", sensor_value); + + // This timer can sometimes block other things so let's read in new debug messages: + //read_debug_messages(); + } +} + +void my_work_handler(struct k_work *work) { + uartHeartbeat(); +} + +K_WORK_DEFINE(my_work, my_work_handler); + +void debug_work_handler(struct k_work *work) { + read_debug_messages(); +} + +K_WORK_DEFINE(debug_work, debug_work_handler); + +struct k_timer heartbeat_timer; +void my_timer_func(struct k_timer *timer_id) { + uartHeartbeat(); + //printf("My timer func!\n"); + //k_work_submit(&my_work); + k_work_submit(&debug_work); +} + +def_prim(setup_uart_sensor, twoToNoneU32) { + if (!device_is_ready(uart_dev)) { + printk("Input port is not ready!\n"); + return 0; + } + + printk("Setting up uart\n"); + int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, NULL); + if (ret < 0) { + if (ret == -ENOTSUP) { + printk("Interrupt-driven UART API support not enabled\n"); + } else if (ret == -ENOSYS) { + printk("UART device does not support interrupt-driven API\n"); + } else { + printk("Error setting UART callback: %d\n", ret); + } + return 0; + } + uart_irq_rx_enable(uart_dev); + uint8_t new_mode = arg0.uint32; + if (new_mode != mode && data_mode) { + uart_poll_out(uart_dev, 0x43); + uart_poll_out(uart_dev, new_mode); + uart_poll_out(uart_dev, 0xff ^ 0x43 ^ new_mode); + } + mode = new_mode; + pop_args(2); + return true; +} + +/*#define UART_HEARTBEAT_STACK_SIZE 2048 +#define UART_HEARTBEAT_PRIORITY 0 + +K_THREAD_DEFINE(uart_heartbeat_tid, UART_HEARTBEAT_STACK_SIZE, uartHeartbeat, + NULL, NULL, NULL, UART_HEARTBEAT_PRIORITY, 0, 0);*/ + +def_prim(colour_sensor, oneToOneU32) { + if (!device_is_ready(uart_dev)) { + printk("Input port is not ready!\n"); + return 0; + } + + // uint16_t data; + // int res = uart_poll_in_u16(uart_dev, &data); + // printk("data = %d, res = %d\n", data, res); +#if 0 + unsigned char data; + int res = uart_poll_in(uart_dev, &data); + while (res == 0) { + printk("data = %d, res = %d\n", (int) data, res); + for (int i = 7; i >= 0; i--) { + printk("%d", (data & 1 << i) > 0); + } + printk("\n"); + //res = uart_poll_in_u16(uart_dev, &data); + //k_msleep(100); + k_msleep(10); + res = uart_poll_in(uart_dev, &data); + } + printk("res = %d\n", res); + uart_poll_out(uart_dev, 0b00000010); +#endif + +#if 0 + printk("Setting up uart\n"); + int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, NULL); + if (ret < 0) { + if (ret == -ENOTSUP) { + printk("Interrupt-driven UART API support not enabled\n"); + } else if (ret == -ENOSYS) { + printk("UART device does not support interrupt-driven API\n"); + } else { + printk("Error setting UART callback: %d\n", ret); + } + return 0; + } + uart_irq_rx_enable(uart_dev); +#endif + + // Send NACK + /*uart_poll_out(uart_dev, 0b00000010); + printk("Send NACK\n");*/ + + pop_args(1); + pushUInt32(sensor_value); + return true; +} + //------------------------------------------------------ // Installing all the primitives //------------------------------------------------------ @@ -270,8 +969,20 @@ void install_primitives() { install_primitive(chip_delay); install_primitive(chip_pin_mode); install_primitive(chip_digital_write); + install_primitive_reverse(chip_digital_write); install_primitive(chip_digital_read); install_primitive(print_int); + install_primitive(motor_test); + install_primitive(drive_motor_for_ms); + install_primitive(drive_motor_degrees); + install_primitive_reverse(drive_motor_degrees); + install_primitive(drive_motor_degrees_absolute); + install_primitive(drive_motor); + install_primitive(colour_sensor); + install_primitive(setup_uart_sensor); + + k_timer_init(&heartbeat_timer, my_timer_func, nullptr); + k_timer_start(&heartbeat_timer, K_MSEC(1000), K_MSEC(1000)); } //------------------------------------------------------ @@ -317,24 +1028,17 @@ bool resolve_external_memory(char *symbol, Memory **val) { //------------------------------------------------------ void restore_external_state(Module *m, const std::vector &external_state) { - uint8_t opcode = *m->pc_ptr; - // TODO: Maybe primitives can also be called using the other call - // instructions such as call_indirect - // maybe there should just be a function that checks if a certain function - // is being called that handles all these cases? - if (opcode == 0x10) { // call opcode - uint8_t *pc_copy = m->pc_ptr + 1; - uint32_t fidx = read_LEB_32(&pc_copy); - if (fidx < m->import_count) { - for (auto &primitive : primitives) { - if (!strcmp(primitive.name, m->functions[fidx].import_field)) { - if (primitive.f_reverse) { - debug("Reversing action for primitive %s\n", - primitive.name); - primitive.f_reverse(m, external_state); - } - return; - } + std::set prim_names; + for (uint32_t i = 0; i < m->import_count; i++) { + prim_names.emplace(m->functions[i].import_field); + } + + for (PrimitiveEntry &p : primitives) { + if (prim_names.find(p.name) != prim_names.end()) { + printf("%s\n", p.name); + if (p.f_reverse) { + printf("Reversing action for primitive %s\n", p.name); + p.f_reverse(m, external_state); } } } From 8b1886334317497a7f7c71d2fbab9c5285f73b06 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Mon, 4 Nov 2024 14:09:43 +0100 Subject: [PATCH 02/13] Move some motor code into a separate file --- platforms/Zephyr/CMakeLists.txt | 1 + src/Primitives/Mindstorms/Motor.cpp | 120 ++++++++++++ src/Primitives/Mindstorms/Motor.h | 93 +++++++++ src/Primitives/zephyr.cpp | 289 ++++------------------------ 4 files changed, 248 insertions(+), 255 deletions(-) create mode 100644 src/Primitives/Mindstorms/Motor.cpp create mode 100644 src/Primitives/Mindstorms/Motor.h diff --git a/platforms/Zephyr/CMakeLists.txt b/platforms/Zephyr/CMakeLists.txt index 0ed056c0..940ac0ab 100644 --- a/platforms/Zephyr/CMakeLists.txt +++ b/platforms/Zephyr/CMakeLists.txt @@ -19,6 +19,7 @@ target_sources(app PRIVATE ../../src/Interpreter/instructions.cpp ../../src/Interpreter/interpreter.cpp ../../src/Primitives/zephyr.cpp + ../../src/Primitives/Mindstorms/Motor.cpp ../../src/Memory/mem.cpp ../../src/Utils/util.cpp ../../src/Utils/util_arduino.cpp diff --git a/src/Primitives/Mindstorms/Motor.cpp b/src/Primitives/Mindstorms/Motor.cpp new file mode 100644 index 00000000..738f3799 --- /dev/null +++ b/src/Primitives/Mindstorms/Motor.cpp @@ -0,0 +1,120 @@ +#include "Motor.h" +#include "../../Utils/macros.h" + +MotorEncoder::MotorEncoder(gpio_dt_spec pin5_encoder_spec, gpio_dt_spec pin6_encoder_spec, const char *name) + : pin5_encoder_spec(pin5_encoder_spec), + pin6_encoder_spec(pin6_encoder_spec), + angle(0), + target_angle(0), + expect_pin5_int(true), + expect_pin6_int(true), + last_update(0) { + if (gpio_pin_configure_dt(&pin5_encoder_spec, GPIO_INPUT)) { + FATAL("Failed to configure GPIO encoder pin5\n"); + } + if (gpio_pin_configure_dt(&pin6_encoder_spec, GPIO_INPUT)) { + FATAL("Failed to configure GPIO encoder pin6\n"); + } + + int result = gpio_pin_interrupt_configure_dt(&pin5_encoder_spec, + GPIO_INT_EDGE_RISING); + if (result != 0) { + printf("Failed to configure interrupt on pin5 for %s, error code %d\n", name, result); + } + gpio_init_callback(&pin5_encoder_cb_data, + MotorEncoder::encoder_pin5_edge_rising, + BIT(pin5_encoder_spec.pin)); + gpio_add_callback(pin5_encoder_spec.port, &pin5_encoder_cb_data); + + result = gpio_pin_interrupt_configure_dt(&pin6_encoder_spec, + GPIO_INT_EDGE_RISING); + if (result != 0) { + printf("Failed to configure interrupt on pin6 for %s, error code %d\n", name, result); + } + gpio_init_callback(&pin6_encoder_cb_data, + MotorEncoder::encoder_pin6_edge_rising, + BIT(pin6_encoder_spec.pin)); + gpio_add_callback(pin6_encoder_spec.port, &pin6_encoder_cb_data); +} + +MotorEncoder::~MotorEncoder() { + gpio_remove_callback(pin5_encoder_spec.port, &pin5_encoder_cb_data); + gpio_remove_callback(pin6_encoder_spec.port, &pin6_encoder_cb_data); +} + +bool drive_pwm(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float pwm1, + float pwm2) { + if (!pwm_is_ready_dt(&pwm1_spec)) { + printf("Error: PWM device %s is not ready\n", pwm1_spec.dev->name); + return false; + } + + int ret = pwm_set_pulse_dt(&pwm1_spec, pwm1 * pwm1_spec.period); + if (ret) { + printf("Error %d: failed to set pulse width, pwm1 = %f\n", ret, pwm1); + return false; + } + + ret = pwm_set_pulse_dt(&pwm2_spec, pwm2 * pwm2_spec.period); + if (ret) { + printf("Error %d: failed to set pulse width, pwm2 = %f\n", ret, pwm2); + return false; + } + return true; +} + +bool drive_motor(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float speed) { + float pwm1 = 0; + float pwm2 = 0; + if (speed > 0) { + pwm1 = speed; + } else { + pwm2 = -speed; + } + + return drive_pwm(pwm1_spec, pwm2_spec, pwm1, pwm2); +} + +void drive_motor_to_target(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, MotorEncoder *encoder, int32_t speed) { + printf("drift = %d\n", + abs(encoder->get_angle() - encoder->get_target_angle())); + + int drift = encoder->get_angle() - encoder->get_target_angle(); + // Reset stall timer, otherwise it will instantly think it's not moving. + encoder->last_update = k_uptime_get(); + while (abs(drift) > 0) { + int speed_sign = std::signbit(drift) ? -1 : 1; + drive_motor(pwm1_spec, pwm2_spec, speed_sign * speed / 10000.0f); + while (speed_sign * + (encoder->get_angle() - encoder->get_target_angle()) > + 0 && + k_uptime_get() - encoder->get_last_update() < 150) { + } + bool not_moving = k_uptime_get() - encoder->get_last_update() >= 150; + if (not_moving) { + speed += 100; + printf("Not moving, increasing speed to %d, %llims since last movement\n", speed, k_uptime_get() - encoder->get_last_update()); + drive_motor(pwm1_spec, pwm2_spec, speed_sign * speed / 10000.0f); + + // Wait for 10ms or movement. + uint64_t start_time = k_uptime_get(); + while (k_uptime_get() - start_time < 10 && k_uptime_get() - encoder->get_last_update() >= 150) {} + continue; + } + encoder->last_update = k_uptime_get(); + printf("%lli\n", k_uptime_get() - encoder->get_last_update()); + /*printf("PWM device %s\n", pwm1_spec.dev->name); + printf("PWM device %s\n", pwm2_spec.dev->name);*/ + drive_pwm(pwm1_spec, pwm2_spec, 1.0f, 1.0f); + k_msleep(50); + drift = encoder->get_angle() - encoder->get_target_angle(); + printf("drift = %d, speed = %d\n", drift, speed); + // speed = std::max(775, speed - speed/3); // Reduce speed when going + // fast to do corrections speed = 775; + if (not_moving) { + speed += 100; + } else { + speed = 800; + } + } +} diff --git a/src/Primitives/Mindstorms/Motor.h b/src/Primitives/Mindstorms/Motor.h new file mode 100644 index 00000000..39a078b6 --- /dev/null +++ b/src/Primitives/Mindstorms/Motor.h @@ -0,0 +1,93 @@ +#ifndef WARDUINO_MOTOR_H +#define WARDUINO_MOTOR_H + +#include +#include + +#include +#include +#include + +class MotorEncoder { + static void encoder_pin5_edge_rising(const struct device *dev, + struct gpio_callback *cb, + uint32_t pins) { + MotorEncoder *encoder = + CONTAINER_OF(cb, MotorEncoder, pin5_encoder_cb_data); + if (!encoder->expect_pin5_int) return; + + if (!gpio_pin_get_raw(encoder->pin6_encoder_spec.port, + encoder->pin6_encoder_spec.pin)) { + encoder->angle++; + } else { + encoder->angle--; + } + encoder->last_update = k_uptime_get(); + // printk("Rising edge detected on encoder pin5, angle %d\n", + // encoder->angle); printk("%d\n", + // gpio_pin_get_raw(encoder->pin6_encoder_spec.port, + // encoder->pin6_encoder_spec.pin)); + encoder->expect_pin5_int = false; + encoder->expect_pin6_int = true; + } + + static void encoder_pin6_edge_rising(const struct device *dev, + struct gpio_callback *cb, + uint32_t pins) { + MotorEncoder *encoder = + CONTAINER_OF(cb, MotorEncoder, pin6_encoder_cb_data); + if (!encoder->expect_pin6_int) return; + + if (gpio_pin_get_raw(encoder->pin5_encoder_spec.port, + encoder->pin5_encoder_spec.pin)) { + encoder->angle++; + } else { + encoder->angle--; + } + // printk("Rising edge detected on encoder pin6, angle %d\n", + // encoder->angle); printk("%d\n", + // gpio_pin_get_raw(encoder->pin5_encoder_spec.port, + // encoder->pin5_encoder_spec.pin)); + encoder->expect_pin6_int = false; + encoder->expect_pin5_int = true; + } + + public: + MotorEncoder(gpio_dt_spec pin5_encoder_spec, gpio_dt_spec pin6_encoder_spec, const char *name); + + ~MotorEncoder(); + + inline int get_angle() { return angle; } + + inline void reset_angle() { angle = 0; } + + inline int get_target_angle() { return target_angle; } + + inline void set_target_angle(int new_target_angle) { + this->target_angle = new_target_angle; + } + + inline int64_t get_last_update() { return last_update; } + + private: + gpio_dt_spec pin5_encoder_spec; + gpio_dt_spec pin6_encoder_spec; + struct gpio_callback pin5_encoder_cb_data; + struct gpio_callback pin6_encoder_cb_data; + volatile int angle; + int target_angle; + bool expect_pin5_int; + bool expect_pin6_int; + + public: + volatile int64_t last_update; +}; + +bool drive_pwm(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float pwm1, + float pwm2); + +bool drive_motor(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float speed); + +void drive_motor_to_target(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, MotorEncoder *encoder, int32_t speed); + +#endif // WARDUINO_MOTOR_H diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index f6717422..37017101 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -32,8 +32,9 @@ #include "../Utils/macros.h" #include "../Utils/util.h" #include "primitives.h" +#include "Mindstorms/Motor.h" -#define ALL_PRIMITIVES 12 +#define ALL_PRIMITIVES 10 // Global index for installing primitives int prim_index = 0; @@ -314,50 +315,35 @@ def_prim(print_int, oneToNoneU32) { return true; } -bool drive_pwm(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float pwm1, - float pwm2) { - if (!pwm_is_ready_dt(&pwm1_spec)) { - printf("Error: PWM device %s is not ready\n", pwm1_spec.dev->name); - return false; - } +def_prim(drive_motor, threeToNoneU32) { + int32_t brake = (int32_t)arg0.uint32; + int32_t speed = (int32_t)arg1.uint32; + int32_t motor_index = (int32_t)arg2.uint32; - int ret = pwm_set_pulse_dt(&pwm1_spec, pwm1 * pwm1_spec.period); - if (ret) { - printf("Error %d: failed to set pulse width, pwm1 = %f\n", ret, pwm1); - return false; - } + printf("drive_motor(%d, %d, %d)\n", motor_index, speed, brake); - ret = pwm_set_pulse_dt(&pwm2_spec, pwm2 * pwm2_spec.period); - if (ret) { - printf("Error %d: failed to set pulse width, pwm2 = %f\n", ret, pwm2); - return false; + if (motor_index > 3) { + printf("Invalid motor index %d\n", motor_index); + pop_args(3); + return true; } - return true; -} -bool drive_motor(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float speed) { - float pwm1 = 0; - float pwm2 = 0; - if (speed > 0) { - pwm1 = speed; - } else { - pwm2 = -speed; - } + pwm_dt_spec pwm1_spec = pwm_specs[motor_index * 2]; + pwm_dt_spec pwm2_spec = pwm_specs[motor_index * 2 + 1]; - return drive_pwm(pwm1_spec, pwm2_spec, pwm1, pwm2); -} + drive_motor(pwm1_spec, pwm2_spec, speed / 10000.0f); -def_prim(motor_test, oneToNoneU32) { - int32_t speed = (int32_t)arg0.uint32; - printf("motor_test(%d)\n", speed); - drive_motor(pwm_led0, pwm_led1, speed / 10000.0f); - pop_args(1); + if (speed == 0 && brake == 1) { + drive_pwm(pwm1_spec, pwm2_spec, 1.0f, 1.0f); + } + + pop_args(3); return true; } -def_prim(drive_motor_for_ms, twoToNoneU32) { +def_prim(drive_motor_ms, twoToNoneU32) { int32_t speed = (int32_t)arg0.uint32; - printf("drive_motor_for_ms(%d, %d)\n", speed, arg1.uint32); + printf("drive_motor_ms(%d, %d)\n", speed, arg1.uint32); drive_motor(pwm_led0, pwm_led1, speed / 10000.0f); k_msleep(arg1.uint32); @@ -366,174 +352,13 @@ def_prim(drive_motor_for_ms, twoToNoneU32) { return true; } -class MotorEncoder { - static void encoder_pin5_edge_rising(const struct device *dev, - struct gpio_callback *cb, - uint32_t pins) { - MotorEncoder *encoder = - CONTAINER_OF(cb, MotorEncoder, pin5_encoder_cb_data); - if (!encoder->expect_pin5_int) return; - - if (!gpio_pin_get_raw(encoder->pin6_encoder_spec.port, - encoder->pin6_encoder_spec.pin)) { - encoder->angle++; - } else { - encoder->angle--; - } - encoder->last_update = k_uptime_get(); - // printk("Rising edge detected on encoder pin5, angle %d\n", - // encoder->angle); printk("%d\n", - // gpio_pin_get_raw(encoder->pin6_encoder_spec.port, - // encoder->pin6_encoder_spec.pin)); - encoder->expect_pin5_int = false; - encoder->expect_pin6_int = true; - } - - static void encoder_pin6_edge_rising(const struct device *dev, - struct gpio_callback *cb, - uint32_t pins) { - MotorEncoder *encoder = - CONTAINER_OF(cb, MotorEncoder, pin6_encoder_cb_data); - if (!encoder->expect_pin6_int) return; - - if (gpio_pin_get_raw(encoder->pin5_encoder_spec.port, - encoder->pin5_encoder_spec.pin)) { - encoder->angle++; - } else { - encoder->angle--; - } - // printk("Rising edge detected on encoder pin6, angle %d\n", - // encoder->angle); printk("%d\n", - // gpio_pin_get_raw(encoder->pin5_encoder_spec.port, - // encoder->pin5_encoder_spec.pin)); - encoder->expect_pin6_int = false; - encoder->expect_pin5_int = true; - } - - public: - MotorEncoder(gpio_dt_spec pin5_encoder_spec, gpio_dt_spec pin6_encoder_spec, const char *name) - : pin5_encoder_spec(pin5_encoder_spec), - pin6_encoder_spec(pin6_encoder_spec), - angle(0), - target_angle(0), - expect_pin5_int(true), - expect_pin6_int(true), - last_update(0) { - if (gpio_pin_configure_dt(&pin5_encoder_spec, GPIO_INPUT)) { - FATAL("Failed to configure GPIO encoder pin5\n"); - } - if (gpio_pin_configure_dt(&pin6_encoder_spec, GPIO_INPUT)) { - FATAL("Failed to configure GPIO encoder pin6\n"); - } - - int result = gpio_pin_interrupt_configure_dt(&pin5_encoder_spec, - GPIO_INT_EDGE_RISING); - if (result != 0) { - printf("Failed to configure interrupt on pin5 for %s, error code %d\n", name, result); - } - gpio_init_callback(&pin5_encoder_cb_data, - MotorEncoder::encoder_pin5_edge_rising, - BIT(pin5_encoder_spec.pin)); - gpio_add_callback(pin5_encoder_spec.port, &pin5_encoder_cb_data); - - result = gpio_pin_interrupt_configure_dt(&pin6_encoder_spec, - GPIO_INT_EDGE_RISING); - if (result != 0) { - printf("Failed to configure interrupt on pin6 for %s, error code %d\n", name, result); - } - gpio_init_callback(&pin6_encoder_cb_data, - MotorEncoder::encoder_pin6_edge_rising, - BIT(pin6_encoder_spec.pin)); - gpio_add_callback(pin6_encoder_spec.port, &pin6_encoder_cb_data); - } - - ~MotorEncoder() { - gpio_remove_callback(pin5_encoder_spec.port, &pin5_encoder_cb_data); - gpio_remove_callback(pin6_encoder_spec.port, &pin6_encoder_cb_data); - } - - int get_angle() { return angle; } - - void reset_angle() { angle = 0; } - - int get_target_angle() { return target_angle; } - - void set_target_angle(int target_angle) { - this->target_angle = target_angle; - } - - int64_t get_last_update() { return last_update; } - - private: - gpio_dt_spec pin5_encoder_spec; - gpio_dt_spec pin6_encoder_spec; - struct gpio_callback pin5_encoder_cb_data; - struct gpio_callback pin6_encoder_cb_data; - volatile int angle; - int target_angle; - bool expect_pin5_int; - bool expect_pin6_int; - - public: - volatile int64_t last_update; -}; - -// MotorEncoder encoder(specs[51], specs[50]); -// MotorEncoder encoder(specs[57], specs[58]); -// MotorEncoder encoder(specs[58], specs[57]); - MotorEncoder *encoders[] = {new MotorEncoder(specs[51], specs[50], "Port A"), new MotorEncoder(specs[57], specs[58], "Port B"), //MotorEncoder(specs[17], specs[13], "Port C"), // TODO: Disable when using motor D, causes conflict with interrupts nullptr, new MotorEncoder(specs[27], specs[26], "Port D")}; -// MotorEncoder test_encoder = MotorEncoder(specs[57], specs[58]); - -void drive_motor_to_target(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, MotorEncoder *encoder, int32_t speed) { - printf("drift = %d\n", - abs(encoder->get_angle() - encoder->get_target_angle())); - - int drift = encoder->get_angle() - encoder->get_target_angle(); - // Reset stall timer, otherwise it will instantly think it's not moving. - encoder->last_update = k_uptime_get(); - while (abs(drift) > 0) { - int speed_sign = std::signbit(drift) ? -1 : 1; - drive_motor(pwm1_spec, pwm2_spec, speed_sign * speed / 10000.0f); - while (speed_sign * - (encoder->get_angle() - encoder->get_target_angle()) > - 0 && - k_uptime_get() - encoder->get_last_update() < 150) { - } - bool not_moving = k_uptime_get() - encoder->get_last_update() >= 150; - if (not_moving) { - speed += 100; - printf("Not moving, increasing speed to %d, %llims since last movement\n", speed, k_uptime_get() - encoder->get_last_update()); - drive_motor(pwm1_spec, pwm2_spec, speed_sign * speed / 10000.0f); - - // Wait for 10ms or movement. - uint64_t start_time = k_uptime_get(); - while (k_uptime_get() - start_time < 10 && k_uptime_get() - encoder->get_last_update() >= 150) {} - continue; - } - encoder->last_update = k_uptime_get(); - printf("%lli\n", k_uptime_get() - encoder->get_last_update()); - /*printf("PWM device %s\n", pwm1_spec.dev->name); - printf("PWM device %s\n", pwm2_spec.dev->name);*/ - drive_pwm(pwm1_spec, pwm2_spec, 1.0f, 1.0f); - k_msleep(50); - drift = encoder->get_angle() - encoder->get_target_angle(); - printf("drift = %d, speed = %d\n", drift, speed); - // speed = std::max(775, speed - speed/3); // Reduce speed when going - // fast to do corrections speed = 775; - if (not_moving) { - speed += 100; - } else { - speed = 800; - } - } -} -bool drive_motor_degrees_relative(int32_t motor_index, int32_t degrees, int32_t speed) { +bool drive_motor_degrees_absolute(int32_t motor_index, int32_t degrees, int32_t speed) { printf("drive_motor_degrees(%d, %d, %d)\n", motor_index, degrees, speed); if (motor_index > 3) { @@ -544,12 +369,20 @@ bool drive_motor_degrees_relative(int32_t motor_index, int32_t degrees, int32_t pwm_dt_spec pwm2_spec = pwm_specs[motor_index * 2 + 1]; MotorEncoder *encoder = encoders[motor_index]; - encoder->set_target_angle(encoder->get_target_angle() + degrees); + encoder->set_target_angle(degrees); drive_motor_to_target(pwm1_spec, pwm2_spec, encoder, speed); return true; } +bool drive_motor_degrees_relative(int32_t motor_index, int32_t degrees, int32_t speed) { + printf("drive_motor_degrees(%d, %d, %d)\n", motor_index, degrees, speed); + + MotorEncoder *encoder = encoders[motor_index]; + drive_motor_degrees_absolute(motor_index, encoder->get_target_angle() + degrees, speed); + return true; +} + def_prim(drive_motor_degrees, threeToNoneU32) { int32_t speed = arg0.int32; int32_t degrees = arg1.uint32; @@ -571,9 +404,7 @@ def_prim_reverse(drive_motor_degrees) { // primitives instead of after and just not restore io when // restoring the last snapshot and transfer overrides from a future // snapshot when doing forward execution. - invoke_primitive(m, "drive_motor_degrees_absolute", - motor_index, (uint32_t)state.value, - motor_index == 0 ? 10000 : 2000); + drive_motor_degrees_absolute(motor_index, (int32_t) state.value, motor_index == 0 ? 10000 : 2000); } } } @@ -588,56 +419,6 @@ def_prim_serialize(drive_motor_degrees) { } } -def_prim(drive_motor_degrees_absolute, threeToNoneU32) { - int32_t speed = (int32_t)arg0.uint32; - int32_t degrees = (int32_t)arg1.uint32; - int32_t motor_index = (int32_t)arg2.uint32; - printf("drive_motor_degrees_absolute(%d, %d, %d)\n", motor_index, degrees, - speed); - - if (motor_index > 1) { - printf("Invalid motor index %d\n", motor_index); - pop_args(3); - return true; - } - - pwm_dt_spec pwm1_spec = pwm_specs[motor_index * 2]; - pwm_dt_spec pwm2_spec = pwm_specs[motor_index * 2 + 1]; - MotorEncoder *encoder = encoders[motor_index]; - - encoder->set_target_angle(degrees); - - drive_motor_to_target(pwm1_spec, pwm2_spec, encoder, speed); - - pop_args(3); - return true; -} - -def_prim(drive_motor, threeToNoneU32) { - int32_t brake = (int32_t)arg0.uint32; - int32_t speed = (int32_t)arg1.uint32; - int32_t motor_index = (int32_t)arg2.uint32; - - printf("drive_motor(%d, %d, %d)\n", motor_index, speed, brake); - - if (motor_index > 3) { - FATAL("Motor index out of bounds!"); - return false; - } - - pwm_dt_spec pwm1_spec = pwm_specs[motor_index * 2]; - pwm_dt_spec pwm2_spec = pwm_specs[motor_index * 2 + 1]; - - drive_motor(pwm1_spec, pwm2_spec, speed / 10000.0f); - - if (speed == 0 && brake == 1) { - drive_pwm(pwm1_spec, pwm2_spec, 1.0f, 1.0f); - } - - pop_args(3); - return true; -} - static const struct device *const uart_dev = DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); volatile int payload_bytes = 0; @@ -972,12 +753,10 @@ void install_primitives() { install_primitive_reverse(chip_digital_write); install_primitive(chip_digital_read); install_primitive(print_int); - install_primitive(motor_test); - install_primitive(drive_motor_for_ms); + install_primitive(drive_motor); + install_primitive(drive_motor_ms); install_primitive(drive_motor_degrees); install_primitive_reverse(drive_motor_degrees); - install_primitive(drive_motor_degrees_absolute); - install_primitive(drive_motor); install_primitive(colour_sensor); install_primitive(setup_uart_sensor); From 737e3e57e59c2baaba3117c2f05ba03befebdb46 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Tue, 5 Nov 2024 18:04:02 +0100 Subject: [PATCH 03/13] Refactor motor code a bit more by creating a Motor class that does most of the work --- src/Primitives/Mindstorms/Motor.cpp | 33 ++++++++------- src/Primitives/Mindstorms/Motor.h | 14 +++++++ src/Primitives/zephyr.cpp | 64 ++++++++++++++--------------- 3 files changed, 65 insertions(+), 46 deletions(-) diff --git a/src/Primitives/Mindstorms/Motor.cpp b/src/Primitives/Mindstorms/Motor.cpp index 738f3799..abd23c4a 100644 --- a/src/Primitives/Mindstorms/Motor.cpp +++ b/src/Primitives/Mindstorms/Motor.cpp @@ -63,7 +63,15 @@ bool drive_pwm(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float pwm1, return true; } -bool drive_motor(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float speed) { +Motor::Motor(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, + MotorEncoder *encoder) + : pwm1_spec(pwm1_spec), pwm2_spec(pwm2_spec), encoder(encoder) {} + +void Motor::halt() { + drive_pwm(pwm1_spec, pwm2_spec, 1.0f, 1.0f); +} + +bool Motor::set_speed(float speed) { float pwm1 = 0; float pwm2 = 0; if (speed > 0) { @@ -75,7 +83,7 @@ bool drive_motor(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float speed) { return drive_pwm(pwm1_spec, pwm2_spec, pwm1, pwm2); } -void drive_motor_to_target(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, MotorEncoder *encoder, int32_t speed) { +void Motor::drive_to_target(int32_t speed) { printf("drift = %d\n", abs(encoder->get_angle() - encoder->get_target_angle())); @@ -84,7 +92,7 @@ void drive_motor_to_target(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, MotorEn encoder->last_update = k_uptime_get(); while (abs(drift) > 0) { int speed_sign = std::signbit(drift) ? -1 : 1; - drive_motor(pwm1_spec, pwm2_spec, speed_sign * speed / 10000.0f); + set_speed(speed_sign * speed / 10000.0f); while (speed_sign * (encoder->get_angle() - encoder->get_target_angle()) > 0 && @@ -94,7 +102,7 @@ void drive_motor_to_target(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, MotorEn if (not_moving) { speed += 100; printf("Not moving, increasing speed to %d, %llims since last movement\n", speed, k_uptime_get() - encoder->get_last_update()); - drive_motor(pwm1_spec, pwm2_spec, speed_sign * speed / 10000.0f); + set_speed(speed_sign * speed / 10000.0f); // Wait for 10ms or movement. uint64_t start_time = k_uptime_get(); @@ -103,18 +111,15 @@ void drive_motor_to_target(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, MotorEn } encoder->last_update = k_uptime_get(); printf("%lli\n", k_uptime_get() - encoder->get_last_update()); - /*printf("PWM device %s\n", pwm1_spec.dev->name); - printf("PWM device %s\n", pwm2_spec.dev->name);*/ - drive_pwm(pwm1_spec, pwm2_spec, 1.0f, 1.0f); + halt(); k_msleep(50); drift = encoder->get_angle() - encoder->get_target_angle(); printf("drift = %d, speed = %d\n", drift, speed); - // speed = std::max(775, speed - speed/3); // Reduce speed when going - // fast to do corrections speed = 775; - if (not_moving) { - speed += 100; - } else { - speed = 800; - } + speed = 800; } } + +void Motor::drive_to_angle(int32_t speed, int32_t degrees) { + encoder->set_target_angle(degrees); + drive_to_target(speed); +} diff --git a/src/Primitives/Mindstorms/Motor.h b/src/Primitives/Mindstorms/Motor.h index 39a078b6..0c541c97 100644 --- a/src/Primitives/Mindstorms/Motor.h +++ b/src/Primitives/Mindstorms/Motor.h @@ -83,6 +83,20 @@ class MotorEncoder { volatile int64_t last_update; }; +class Motor { + public: + Motor(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, MotorEncoder *encoder); + + pwm_dt_spec pwm1_spec; + pwm_dt_spec pwm2_spec; + MotorEncoder *encoder; + + void halt(); + bool set_speed(float speed); + void drive_to_target(int32_t speed); + void drive_to_angle(int32_t speed, int32_t degrees); +}; + bool drive_pwm(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, float pwm1, float pwm2); diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index 37017101..6c4b6370 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include "../Memory/mem.h" #include "../Utils/macros.h" @@ -315,10 +316,24 @@ def_prim(print_int, oneToNoneU32) { return true; } +MotorEncoder *encoders[] = {new MotorEncoder(specs[51], specs[50], "Port A"), + new MotorEncoder(specs[57], specs[58], "Port B"), + //MotorEncoder(specs[17], specs[13], "Port C"), // TODO: Disable when using motor D, causes conflict with interrupts + nullptr, + new MotorEncoder(specs[27], specs[26], "Port D")}; + +std::optional get_motor(int32_t motor_index) { + if (motor_index > 3) { + return {}; + } + + return std::make_optional(pwm_specs[motor_index * 2], pwm_specs[motor_index * 2 + 1], encoders[motor_index]); +} + def_prim(drive_motor, threeToNoneU32) { - int32_t brake = (int32_t)arg0.uint32; - int32_t speed = (int32_t)arg1.uint32; - int32_t motor_index = (int32_t)arg2.uint32; + int32_t brake = arg0.int32; + int32_t speed = arg1.int32; + int32_t motor_index = arg2.int32; printf("drive_motor(%d, %d, %d)\n", motor_index, speed, brake); @@ -328,13 +343,11 @@ def_prim(drive_motor, threeToNoneU32) { return true; } - pwm_dt_spec pwm1_spec = pwm_specs[motor_index * 2]; - pwm_dt_spec pwm2_spec = pwm_specs[motor_index * 2 + 1]; - - drive_motor(pwm1_spec, pwm2_spec, speed / 10000.0f); + Motor motor = get_motor(motor_index).value(); + motor.set_speed(speed / 10000.0f); if (speed == 0 && brake == 1) { - drive_pwm(pwm1_spec, pwm2_spec, 1.0f, 1.0f); + motor.halt(); } pop_args(3); @@ -342,42 +355,27 @@ def_prim(drive_motor, threeToNoneU32) { } def_prim(drive_motor_ms, twoToNoneU32) { + int32_t motor_index = arg1.int32; int32_t speed = (int32_t)arg0.uint32; - printf("drive_motor_ms(%d, %d)\n", speed, arg1.uint32); + printf("drive_motor_ms(%d, %d)\n", motor_index, speed); - drive_motor(pwm_led0, pwm_led1, speed / 10000.0f); + Motor motor = get_motor(motor_index).value(); + motor.set_speed(speed / 10000.0f); k_msleep(arg1.uint32); - drive_pwm(pwm_led0, pwm_led1, 1.0f, 1.0f); + motor.halt(); pop_args(2); return true; } -MotorEncoder *encoders[] = {new MotorEncoder(specs[51], specs[50], "Port A"), - new MotorEncoder(specs[57], specs[58], "Port B"), - //MotorEncoder(specs[17], specs[13], "Port C"), // TODO: Disable when using motor D, causes conflict with interrupts - nullptr, - new MotorEncoder(specs[27], specs[26], "Port D")}; - bool drive_motor_degrees_absolute(int32_t motor_index, int32_t degrees, int32_t speed) { - printf("drive_motor_degrees(%d, %d, %d)\n", motor_index, degrees, speed); - - if (motor_index > 3) { - return false; + if (auto motor = get_motor(motor_index)) { + motor->drive_to_angle(speed, degrees); + return true; } - - pwm_dt_spec pwm1_spec = pwm_specs[motor_index * 2]; - pwm_dt_spec pwm2_spec = pwm_specs[motor_index * 2 + 1]; - MotorEncoder *encoder = encoders[motor_index]; - - encoder->set_target_angle(degrees); - - drive_motor_to_target(pwm1_spec, pwm2_spec, encoder, speed); - return true; + return false; } bool drive_motor_degrees_relative(int32_t motor_index, int32_t degrees, int32_t speed) { - printf("drive_motor_degrees(%d, %d, %d)\n", motor_index, degrees, speed); - MotorEncoder *encoder = encoders[motor_index]; drive_motor_degrees_absolute(motor_index, encoder->get_target_angle() + degrees, speed); return true; @@ -753,10 +751,12 @@ void install_primitives() { install_primitive_reverse(chip_digital_write); install_primitive(chip_digital_read); install_primitive(print_int); + install_primitive(drive_motor); install_primitive(drive_motor_ms); install_primitive(drive_motor_degrees); install_primitive_reverse(drive_motor_degrees); + install_primitive(colour_sensor); install_primitive(setup_uart_sensor); From b3a0656f9d75ad71174a7ed352fdbf0fa2826edb Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Mon, 2 Dec 2024 18:03:16 +0100 Subject: [PATCH 04/13] Only use one interrupt for the motor encoders instead of two, this also resolves the conflict with motor c and the other motors! So now you can control all 4 motors at the same time! --- src/Primitives/Mindstorms/Motor.cpp | 14 +------ src/Primitives/Mindstorms/Motor.h | 57 ++++++++++++----------------- src/Primitives/zephyr.cpp | 3 +- 3 files changed, 27 insertions(+), 47 deletions(-) diff --git a/src/Primitives/Mindstorms/Motor.cpp b/src/Primitives/Mindstorms/Motor.cpp index abd23c4a..dff5e4e0 100644 --- a/src/Primitives/Mindstorms/Motor.cpp +++ b/src/Primitives/Mindstorms/Motor.cpp @@ -16,18 +16,8 @@ MotorEncoder::MotorEncoder(gpio_dt_spec pin5_encoder_spec, gpio_dt_spec pin6_enc FATAL("Failed to configure GPIO encoder pin6\n"); } - int result = gpio_pin_interrupt_configure_dt(&pin5_encoder_spec, - GPIO_INT_EDGE_RISING); - if (result != 0) { - printf("Failed to configure interrupt on pin5 for %s, error code %d\n", name, result); - } - gpio_init_callback(&pin5_encoder_cb_data, - MotorEncoder::encoder_pin5_edge_rising, - BIT(pin5_encoder_spec.pin)); - gpio_add_callback(pin5_encoder_spec.port, &pin5_encoder_cb_data); - - result = gpio_pin_interrupt_configure_dt(&pin6_encoder_spec, - GPIO_INT_EDGE_RISING); + int result = gpio_pin_interrupt_configure_dt(&pin6_encoder_spec, + GPIO_INT_EDGE_RISING | GPIO_INT_EDGE_FALLING); if (result != 0) { printf("Failed to configure interrupt on pin6 for %s, error code %d\n", name, result); } diff --git a/src/Primitives/Mindstorms/Motor.h b/src/Primitives/Mindstorms/Motor.h index 0c541c97..22e4b0e2 100644 --- a/src/Primitives/Mindstorms/Motor.h +++ b/src/Primitives/Mindstorms/Motor.h @@ -9,47 +9,38 @@ #include class MotorEncoder { - static void encoder_pin5_edge_rising(const struct device *dev, - struct gpio_callback *cb, - uint32_t pins) { - MotorEncoder *encoder = - CONTAINER_OF(cb, MotorEncoder, pin5_encoder_cb_data); - if (!encoder->expect_pin5_int) return; - - if (!gpio_pin_get_raw(encoder->pin6_encoder_spec.port, - encoder->pin6_encoder_spec.pin)) { - encoder->angle++; - } else { - encoder->angle--; - } - encoder->last_update = k_uptime_get(); - // printk("Rising edge detected on encoder pin5, angle %d\n", - // encoder->angle); printk("%d\n", - // gpio_pin_get_raw(encoder->pin6_encoder_spec.port, - // encoder->pin6_encoder_spec.pin)); - encoder->expect_pin5_int = false; - encoder->expect_pin6_int = true; - } - static void encoder_pin6_edge_rising(const struct device *dev, struct gpio_callback *cb, uint32_t pins) { MotorEncoder *encoder = CONTAINER_OF(cb, MotorEncoder, pin6_encoder_cb_data); - if (!encoder->expect_pin6_int) return; - if (gpio_pin_get_raw(encoder->pin5_encoder_spec.port, - encoder->pin5_encoder_spec.pin)) { - encoder->angle++; + int rising = gpio_pin_get_raw(encoder->pin6_encoder_spec.port, + encoder->pin6_encoder_spec.pin); + int pin5 = gpio_pin_get_raw(encoder->pin5_encoder_spec.port, + encoder->pin5_encoder_spec.pin); + printf("%d\n", rising); + + if (rising) { + if (pin5) { + printf("++\n"); + encoder->angle++; + } + else { + printf("--\n"); + encoder->angle--; + } } else { - encoder->angle--; + if (pin5) { + printf("--\n"); + encoder->angle--; + } + else { + printf("++\n"); + encoder->angle++; + } } - // printk("Rising edge detected on encoder pin6, angle %d\n", - // encoder->angle); printk("%d\n", - // gpio_pin_get_raw(encoder->pin5_encoder_spec.port, - // encoder->pin5_encoder_spec.pin)); - encoder->expect_pin6_int = false; - encoder->expect_pin5_int = true; + encoder->last_update = k_uptime_get(); } public: diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index 6c4b6370..57a31929 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -318,8 +318,7 @@ def_prim(print_int, oneToNoneU32) { MotorEncoder *encoders[] = {new MotorEncoder(specs[51], specs[50], "Port A"), new MotorEncoder(specs[57], specs[58], "Port B"), - //MotorEncoder(specs[17], specs[13], "Port C"), // TODO: Disable when using motor D, causes conflict with interrupts - nullptr, + new MotorEncoder(specs[17], specs[13], "Port C"), new MotorEncoder(specs[27], specs[26], "Port D")}; std::optional get_motor(int32_t motor_index) { From 62fde475fbee1e91db0e4027482436c9c2ffc624 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Tue, 3 Dec 2024 12:00:09 +0100 Subject: [PATCH 05/13] Enable CONFIG_STM32_ENABLE_DEBUG_SLEEP_STOP to allow flashing the board at all times without reset (Increases power consumption so should be disabled if power consumption is important, for example when just using open bot brain in a classroom) --- platforms/Zephyr/prj.conf | 3 +++ 1 file changed, 3 insertions(+) diff --git a/platforms/Zephyr/prj.conf b/platforms/Zephyr/prj.conf index 45bdf700..ab6098cc 100644 --- a/platforms/Zephyr/prj.conf +++ b/platforms/Zephyr/prj.conf @@ -30,3 +30,6 @@ CONFIG_CONSOLE_PUTCHAR_BUFSIZE=4096 CONFIG_POSIX_API=y CONFIG_MAIN_STACK_SIZE=4096 + +# Allow flashing the board without reset (increases power consumption so should only be used for development) +CONFIG_STM32_ENABLE_DEBUG_SLEEP_STOP=y From e08725507f758ec01cdfdf2bc9d8e67570873f99 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Wed, 4 Dec 2024 15:33:55 +0100 Subject: [PATCH 06/13] Clean up color sensor code a bit --- src/Primitives/zephyr.cpp | 271 +++++++++++--------------------------- 1 file changed, 76 insertions(+), 195 deletions(-) diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index 57a31929..932eff20 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -418,20 +418,27 @@ def_prim_serialize(drive_motor_degrees) { static const struct device *const uart_dev = DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); -volatile int payload_bytes = 0; + +int payload_bytes = 0; int payload_index = 0; unsigned int current_payload = 0; -char checksum; +unsigned char checksum; int baudrate = -1; -int bytes_received = 0; - -uint8_t mode = 2; -int mode0_format_location = 0; +uint8_t mode = 0; -volatile bool data_mode = false; bool data_byte = false; volatile int sensor_value = 0; +enum ReceiveState { + advertise, + final_mode_format, + modes_complete, + data +}; + +volatile uint32_t receive_state = ReceiveState::advertise; +bool baudrate_configured = false; + void serial_cb(const struct device *dev, void *user_data) { uint8_t data; @@ -443,200 +450,119 @@ void serial_cb(const struct device *dev, void *user_data) { return; } - /* read until FIFO empty */ while (uart_fifo_read(uart_dev, &data, 1) == 1) { - bytes_received++; - /*printk("0x%02x '%c' ", data, (char) data); - printk("0b"); - for (int i = 7; i >= 0; i--) { - printk("%d", (data & 1 << i) > 0); + if (receive_state == ReceiveState::data) { + if (data_byte) { + sensor_value = data; + data_byte = false; + } + + // Check if it's a data message. This indicates the byte after this + // will contain data. + if (0b11000000 & data) { + // Next byte will be data + data_byte = true; + } + continue; } - printk("\n");*/ if (payload_bytes > 0) { - printk("payload = %d\n", data); - // Print in binary: - printk("0b"); - for (int i = 7; i >= 0; i--) { - printk("%d", (data & 1 << i) > 0); - } - printk("\n"); payload_bytes--; if (payload_bytes > 1) { + if (receive_state == ReceiveState::final_mode_format && payload_bytes == 5 && data != 0x80) { + receive_state = ReceiveState::advertise; + payload_bytes = 0; + } checksum ^= data; - printk("before current_payload = %d\n", current_payload); current_payload = current_payload | (((unsigned long)data) << payload_index * 8); payload_index++; - printk("shift = %d, current_payload = %d\n", payload_index * 8, - (int)current_payload); - // printk("checksum = %d\n", checksum); } else if (checksum == data) { - printk("Checksum matches!\n"); - printk("Baudrate = %d\n", current_payload); - baudrate = current_payload; // TODO: Set actual baudrate here - } - } - - // If data is ACK send an ACK back. - /*if (data == 0b00000100) { - printk("Bytes received = %d\n", bytes_received); - uart_poll_out(uart_dev, 0b00000100); - uart_poll_out(uart_dev, 0b00000010); - printk("Sent ACK\n"); - - // If we received a baudrate, change it after sending the ACK. - if (baudrate >= 0) { - printk("Changing baudrate to %d\n", baudrate); - uart_config cfg; - uart_config_get(uart_dev, &cfg); - cfg.baudrate = baudrate; - - int config_err = uart_configure(uart_dev, &cfg); - printk("config_err = %d\n", config_err); - if (config_err) { - printk("UART configure error %d", config_err); + if (receive_state == ReceiveState::advertise) { + printf("Baudrate = %d\n", current_payload); + baudrate = current_payload; } - - uart_config_get(uart_dev, &cfg); - printk("current baudrate after config change = %d\n", - cfg.baudrate); - } - }*/ - - // When we receive an ACK message. - if (data == 0b00000100) { - // printk("%d\n", bytes_received - mode0_format_location); - if (bytes_received - mode0_format_location == 7 && baudrate >= 0) { - printk("SPECIAL_LINE\n"); - - uart_poll_out(uart_dev, 0b00000100); // Send ACK - data_mode = true; - - // Change baudrate: - /*printk("Changing baudrate to %d\n", baudrate); - uart_config cfg; - uart_config_get(uart_dev, &cfg); - cfg.baudrate = baudrate; - - int config_err = uart_configure(uart_dev, &cfg); - printk("config_err = %d\n", config_err); - if (config_err) { - printk("UART configure error %d", config_err); + else if (receive_state == ReceiveState::final_mode_format){ + receive_state = ReceiveState::modes_complete; } - - config_err = uart_config_get(uart_dev, &cfg); - printk("current baudrate after config change = %d\n", - cfg.baudrate); printk("config_err = %d\n", config_err);*/ - - /*while (true) { - k_msleep(100); - printk("Send NACK\n"); - uart_poll_out(uart_dev, 0b00000010); - }*/ } } - // HACK - if (data == 0b10010000 && baudrate >= 0) { - mode0_format_location = bytes_received; - // Receive last bits of data and ACK it: - /*k_msleep(50); - uart_poll_out(uart_dev, 0b00000100); // Send ACK - - // Change baudrate: - printk("Changing baudrate to %d\n", baudrate); - uart_config cfg; - uart_config_get(uart_dev, &cfg); - cfg.baudrate = baudrate; - - int config_err = uart_configure(uart_dev, &cfg); - printk("config_err = %d\n", config_err); - if (config_err) { - printk("UART configure error %d", config_err); + // Handle ACK message. + if (data == 0b00000100) { + // If we receive an ACK after the final format message and we + // know which speed to communicate at then we should send an ACK to + // switch to data mode. + if (receive_state == ReceiveState::modes_complete && baudrate > 0) { + printf("Completing pairing sequence\n"); + uart_poll_out(uart_dev, 0b00000100); // Send ACK back + receive_state = ReceiveState::data; } - - uart_config_get(uart_dev, &cfg); - printk("current baudrate after config change = %d\n", - cfg.baudrate);*/ } - if (data_mode) { - if (data_byte) { - sensor_value = data; - data_byte = false; - } - - // Check if it' a data message. - if (0b11000000 & data) { - // Next byte will be data - data_byte = true; + // Handle FORMAT commands. + if (data >> 3 == 0b10010) { + uint8_t sensor_mode = data & 0b111; + printf("FORMAT for mode %d\n", sensor_mode); + if (sensor_mode == 0) { + receive_state = ReceiveState::final_mode_format; + payload_bytes = 6; + payload_index = 0; + current_payload = 0; + checksum = 0xff ^ data; } } + // Handle speed command. if (data == 0b01010010) { - printk("Speed command\n"); payload_bytes = 5; payload_index = 0; current_payload = 0; checksum = 0xff ^ 0b01010010; - - // EV3 Colour sensor sent 57600 as it's baudrate } } - // uart_poll_out(uart_dev, 0b00000100); - // uart_poll_out(uart_dev, 0b00000010); - // printk("Sent ACK\n"); } - -bool baudrate_configured = false; - extern void read_debug_messages(); +void set_sensor_mode(uint8_t new_mode) { + uart_poll_out(uart_dev, 0x43); + uart_poll_out(uart_dev, new_mode); + uart_poll_out(uart_dev, 0xff ^ 0x43 ^ new_mode); +} + void uartHeartbeat() { - if (data_mode && !baudrate_configured) { + if (receive_state != ReceiveState::data) { + return; + } + + if (!baudrate_configured) { printk("Changing baudrate to %d\n", baudrate); uart_config cfg; uart_config_get(uart_dev, &cfg); cfg.baudrate = baudrate; int config_err = uart_configure(uart_dev, &cfg); - printk("config_err = %d\n", config_err); + printf("config_err = %d\n", config_err); if (config_err) { - printk("UART configure error %d", config_err); + printf("UART configure error %d", config_err); } config_err = uart_config_get(uart_dev, &cfg); printk("current baudrate after config change = %d\n", cfg.baudrate); - printk("config_err = %d\n", config_err); + printf("config_err = %d\n", config_err); baudrate_configured = true; - // Change to mode 2 - uart_poll_out(uart_dev, 0x43); - uart_poll_out(uart_dev, mode); - uart_poll_out(uart_dev, 0xff ^ 0x43 ^ mode); + // Change to the desired mode. + set_sensor_mode(mode); } - if (data_mode && baudrate_configured) { - //k_msleep(100); - // printk("Send NACK\n"); + if (baudrate_configured) { uart_poll_out(uart_dev, 0b00000010); - - // printk("sensor_value = %d\n", sensor_value); - - // This timer can sometimes block other things so let's read in new debug messages: - //read_debug_messages(); } } -void my_work_handler(struct k_work *work) { - uartHeartbeat(); -} - -K_WORK_DEFINE(my_work, my_work_handler); - void debug_work_handler(struct k_work *work) { read_debug_messages(); } @@ -644,10 +570,8 @@ void debug_work_handler(struct k_work *work) { K_WORK_DEFINE(debug_work, debug_work_handler); struct k_timer heartbeat_timer; -void my_timer_func(struct k_timer *timer_id) { +void heartbeat_timer_func(struct k_timer *timer_id) { uartHeartbeat(); - //printf("My timer func!\n"); - //k_work_submit(&my_work); k_work_submit(&debug_work); } @@ -671,10 +595,8 @@ def_prim(setup_uart_sensor, twoToNoneU32) { } uart_irq_rx_enable(uart_dev); uint8_t new_mode = arg0.uint32; - if (new_mode != mode && data_mode) { - uart_poll_out(uart_dev, 0x43); - uart_poll_out(uart_dev, new_mode); - uart_poll_out(uart_dev, 0xff ^ 0x43 ^ new_mode); + if (receive_state == ReceiveState::data && mode != new_mode) { + set_sensor_mode(new_mode); } mode = new_mode; pop_args(2); @@ -693,47 +615,6 @@ def_prim(colour_sensor, oneToOneU32) { return 0; } - // uint16_t data; - // int res = uart_poll_in_u16(uart_dev, &data); - // printk("data = %d, res = %d\n", data, res); -#if 0 - unsigned char data; - int res = uart_poll_in(uart_dev, &data); - while (res == 0) { - printk("data = %d, res = %d\n", (int) data, res); - for (int i = 7; i >= 0; i--) { - printk("%d", (data & 1 << i) > 0); - } - printk("\n"); - //res = uart_poll_in_u16(uart_dev, &data); - //k_msleep(100); - k_msleep(10); - res = uart_poll_in(uart_dev, &data); - } - printk("res = %d\n", res); - uart_poll_out(uart_dev, 0b00000010); -#endif - -#if 0 - printk("Setting up uart\n"); - int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, NULL); - if (ret < 0) { - if (ret == -ENOTSUP) { - printk("Interrupt-driven UART API support not enabled\n"); - } else if (ret == -ENOSYS) { - printk("UART device does not support interrupt-driven API\n"); - } else { - printk("Error setting UART callback: %d\n", ret); - } - return 0; - } - uart_irq_rx_enable(uart_dev); -#endif - - // Send NACK - /*uart_poll_out(uart_dev, 0b00000010); - printk("Send NACK\n");*/ - pop_args(1); pushUInt32(sensor_value); return true; From 730a189f5e791d27a1bb4cac492a7722d404ad4f Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Wed, 4 Dec 2024 15:35:59 +0100 Subject: [PATCH 07/13] Rename colour_sensor to color_sensor + add abort primitive + set heartbeat interval to 990ms instead of 1000 --- src/Primitives/zephyr.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index 932eff20..de1ec0de 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -35,7 +35,7 @@ #include "primitives.h" #include "Mindstorms/Motor.h" -#define ALL_PRIMITIVES 10 +#define ALL_PRIMITIVES 11 // Global index for installing primitives int prim_index = 0; @@ -609,7 +609,7 @@ def_prim(setup_uart_sensor, twoToNoneU32) { K_THREAD_DEFINE(uart_heartbeat_tid, UART_HEARTBEAT_STACK_SIZE, uartHeartbeat, NULL, NULL, NULL, UART_HEARTBEAT_PRIORITY, 0, 0);*/ -def_prim(colour_sensor, oneToOneU32) { +def_prim(color_sensor, oneToOneU32) { if (!device_is_ready(uart_dev)) { printk("Input port is not ready!\n"); return 0; @@ -620,6 +620,11 @@ def_prim(colour_sensor, oneToOneU32) { return true; } +def_prim(abort, NoneToNoneU32) { + printf("abort\n"); + return false; +} + //------------------------------------------------------ // Installing all the primitives //------------------------------------------------------ @@ -637,11 +642,13 @@ void install_primitives() { install_primitive(drive_motor_degrees); install_primitive_reverse(drive_motor_degrees); - install_primitive(colour_sensor); + install_primitive(color_sensor); install_primitive(setup_uart_sensor); - k_timer_init(&heartbeat_timer, my_timer_func, nullptr); - k_timer_start(&heartbeat_timer, K_MSEC(1000), K_MSEC(1000)); + install_primitive(abort); + + k_timer_init(&heartbeat_timer, heartbeat_timer_func, nullptr); + k_timer_start(&heartbeat_timer, K_MSEC(990), K_MSEC(990)); } //------------------------------------------------------ From f31c16b792dfc7269e87bf6eba5dfeaf079be700 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Wed, 4 Dec 2024 16:24:54 +0100 Subject: [PATCH 08/13] Minor cleanup --- src/Primitives/zephyr.cpp | 24 ++++++++++-------------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index de1ec0de..fa34f115 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -419,14 +419,16 @@ def_prim_serialize(drive_motor_degrees) { static const struct device *const uart_dev = DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); +// Variables used for setting up the sensor. int payload_bytes = 0; int payload_index = 0; unsigned int current_payload = 0; unsigned char checksum; +bool data_byte = false; + +// Variables that are used during setup but also by the heartbeat function. int baudrate = -1; uint8_t mode = 0; - -bool data_byte = false; volatile int sensor_value = 0; enum ReceiveState { @@ -524,8 +526,6 @@ void serial_cb(const struct device *dev, void *user_data) { } } -extern void read_debug_messages(); - void set_sensor_mode(uint8_t new_mode) { uart_poll_out(uart_dev, 0x43); uart_poll_out(uart_dev, new_mode); @@ -563,6 +563,8 @@ void uartHeartbeat() { } } +extern void read_debug_messages(); + void debug_work_handler(struct k_work *work) { read_debug_messages(); } @@ -578,11 +580,11 @@ void heartbeat_timer_func(struct k_timer *timer_id) { def_prim(setup_uart_sensor, twoToNoneU32) { if (!device_is_ready(uart_dev)) { printk("Input port is not ready!\n"); - return 0; + return false; } printk("Setting up uart\n"); - int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, NULL); + int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, nullptr); if (ret < 0) { if (ret == -ENOTSUP) { printk("Interrupt-driven UART API support not enabled\n"); @@ -591,7 +593,7 @@ def_prim(setup_uart_sensor, twoToNoneU32) { } else { printk("Error setting UART callback: %d\n", ret); } - return 0; + return false; } uart_irq_rx_enable(uart_dev); uint8_t new_mode = arg0.uint32; @@ -603,16 +605,10 @@ def_prim(setup_uart_sensor, twoToNoneU32) { return true; } -/*#define UART_HEARTBEAT_STACK_SIZE 2048 -#define UART_HEARTBEAT_PRIORITY 0 - -K_THREAD_DEFINE(uart_heartbeat_tid, UART_HEARTBEAT_STACK_SIZE, uartHeartbeat, - NULL, NULL, NULL, UART_HEARTBEAT_PRIORITY, 0, 0);*/ - def_prim(color_sensor, oneToOneU32) { if (!device_is_ready(uart_dev)) { printk("Input port is not ready!\n"); - return 0; + return false; } pop_args(1); From 5e8ab406ea448e946db5e6c67eb42532f85bb180 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Wed, 4 Dec 2024 16:33:56 +0100 Subject: [PATCH 09/13] Move sensor setup into separate function --- src/Primitives/zephyr.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index fa34f115..a30e4d4e 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -417,7 +417,7 @@ def_prim_serialize(drive_motor_degrees) { } static const struct device *const uart_dev = - DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); + DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); // Variables used for setting up the sensor. int payload_bytes = 0; @@ -577,7 +577,7 @@ void heartbeat_timer_func(struct k_timer *timer_id) { k_work_submit(&debug_work); } -def_prim(setup_uart_sensor, twoToNoneU32) { +bool configure_uart_sensor(uint8_t new_mode) { if (!device_is_ready(uart_dev)) { printk("Input port is not ready!\n"); return false; @@ -596,15 +596,19 @@ def_prim(setup_uart_sensor, twoToNoneU32) { return false; } uart_irq_rx_enable(uart_dev); - uint8_t new_mode = arg0.uint32; if (receive_state == ReceiveState::data && mode != new_mode) { set_sensor_mode(new_mode); } mode = new_mode; - pop_args(2); return true; } +def_prim(setup_uart_sensor, twoToNoneU32) { + bool result = configure_uart_sensor(arg0.uint32); + pop_args(2); + return result; +} + def_prim(color_sensor, oneToOneU32) { if (!device_is_ready(uart_dev)) { printk("Input port is not ready!\n"); From 2614f7962e400db535301eebe6bd77de712dceb9 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Wed, 4 Dec 2024 16:56:17 +0100 Subject: [PATCH 10/13] Move most of the color sensor code into the uart_sensor files --- platforms/Zephyr/CMakeLists.txt | 1 + src/Primitives/Mindstorms/Motor.cpp | 2 - src/Primitives/Mindstorms/Motor.h | 39 ++--- src/Primitives/Mindstorms/uart_sensor.cpp | 178 +++++++++++++++++++++ src/Primitives/Mindstorms/uart_sensor.h | 21 +++ src/Primitives/zephyr.cpp | 183 +--------------------- 6 files changed, 221 insertions(+), 203 deletions(-) create mode 100644 src/Primitives/Mindstorms/uart_sensor.cpp create mode 100644 src/Primitives/Mindstorms/uart_sensor.h diff --git a/platforms/Zephyr/CMakeLists.txt b/platforms/Zephyr/CMakeLists.txt index 940ac0ab..96e1c13a 100644 --- a/platforms/Zephyr/CMakeLists.txt +++ b/platforms/Zephyr/CMakeLists.txt @@ -20,6 +20,7 @@ target_sources(app PRIVATE ../../src/Interpreter/interpreter.cpp ../../src/Primitives/zephyr.cpp ../../src/Primitives/Mindstorms/Motor.cpp + ../../src/Primitives/Mindstorms/uart_sensor.cpp ../../src/Memory/mem.cpp ../../src/Utils/util.cpp ../../src/Utils/util_arduino.cpp diff --git a/src/Primitives/Mindstorms/Motor.cpp b/src/Primitives/Mindstorms/Motor.cpp index dff5e4e0..053be25e 100644 --- a/src/Primitives/Mindstorms/Motor.cpp +++ b/src/Primitives/Mindstorms/Motor.cpp @@ -6,8 +6,6 @@ MotorEncoder::MotorEncoder(gpio_dt_spec pin5_encoder_spec, gpio_dt_spec pin6_enc pin6_encoder_spec(pin6_encoder_spec), angle(0), target_angle(0), - expect_pin5_int(true), - expect_pin6_int(true), last_update(0) { if (gpio_pin_configure_dt(&pin5_encoder_spec, GPIO_INPUT)) { FATAL("Failed to configure GPIO encoder pin5\n"); diff --git a/src/Primitives/Mindstorms/Motor.h b/src/Primitives/Mindstorms/Motor.h index 22e4b0e2..f44254d5 100644 --- a/src/Primitives/Mindstorms/Motor.h +++ b/src/Primitives/Mindstorms/Motor.h @@ -1,7 +1,7 @@ #ifndef WARDUINO_MOTOR_H #define WARDUINO_MOTOR_H -#include +#include #include #include @@ -12,70 +12,60 @@ class MotorEncoder { static void encoder_pin6_edge_rising(const struct device *dev, struct gpio_callback *cb, uint32_t pins) { - MotorEncoder *encoder = - CONTAINER_OF(cb, MotorEncoder, pin6_encoder_cb_data); + MotorEncoder *encoder = CONTAINER_OF(cb, MotorEncoder, pin6_encoder_cb_data); int rising = gpio_pin_get_raw(encoder->pin6_encoder_spec.port, - encoder->pin6_encoder_spec.pin); + encoder->pin6_encoder_spec.pin); int pin5 = gpio_pin_get_raw(encoder->pin5_encoder_spec.port, - encoder->pin5_encoder_spec.pin); - printf("%d\n", rising); + encoder->pin5_encoder_spec.pin); if (rising) { if (pin5) { - printf("++\n"); encoder->angle++; - } - else { - printf("--\n"); + } else { encoder->angle--; } } else { if (pin5) { - printf("--\n"); encoder->angle--; - } - else { - printf("++\n"); + } else { encoder->angle++; } } encoder->last_update = k_uptime_get(); } - public: +public: MotorEncoder(gpio_dt_spec pin5_encoder_spec, gpio_dt_spec pin6_encoder_spec, const char *name); ~MotorEncoder(); - inline int get_angle() { return angle; } + [[nodiscard]] inline int get_angle() const { return angle; } inline void reset_angle() { angle = 0; } - inline int get_target_angle() { return target_angle; } + [[nodiscard]] inline int get_target_angle() const { return target_angle; } inline void set_target_angle(int new_target_angle) { this->target_angle = new_target_angle; } - inline int64_t get_last_update() { return last_update; } + [[nodiscard]] inline int64_t get_last_update() const { return last_update; } - private: +private: gpio_dt_spec pin5_encoder_spec; gpio_dt_spec pin6_encoder_spec; struct gpio_callback pin5_encoder_cb_data; struct gpio_callback pin6_encoder_cb_data; volatile int angle; int target_angle; - bool expect_pin5_int; - bool expect_pin6_int; - public: +public: volatile int64_t last_update; }; class Motor { - public: +public: Motor(pwm_dt_spec pwm1_spec, pwm_dt_spec pwm2_spec, MotorEncoder *encoder); pwm_dt_spec pwm1_spec; @@ -83,8 +73,11 @@ class Motor { MotorEncoder *encoder; void halt(); + bool set_speed(float speed); + void drive_to_target(int32_t speed); + void drive_to_angle(int32_t speed, int32_t degrees); }; diff --git a/src/Primitives/Mindstorms/uart_sensor.cpp b/src/Primitives/Mindstorms/uart_sensor.cpp new file mode 100644 index 00000000..51e030f2 --- /dev/null +++ b/src/Primitives/Mindstorms/uart_sensor.cpp @@ -0,0 +1,178 @@ +#include "uart_sensor.h" + +#include +#include + +static const struct device *const uart_dev = + DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); + +// Variables used for setting up the sensor. +int payload_bytes = 0; +int payload_index = 0; +unsigned int current_payload = 0; +unsigned char checksum; +bool data_byte = false; + +// Variables that are used during setup but also by the heartbeat function. +int baudrate = -1; +uint8_t mode = 0; +volatile int sensor_value = 0; + +volatile uint32_t receive_state = ReceiveState::advertise; +bool baudrate_configured = false; + +void serial_cb([[maybe_unused]] const struct device *dev, [[maybe_unused]] void *user_data) { + uint8_t data; + + if (!uart_irq_update(uart_dev)) { + return; + } + + if (!uart_irq_rx_ready(uart_dev)) { + return; + } + + while (uart_fifo_read(uart_dev, &data, 1) == 1) { + if (receive_state == ReceiveState::data) { + if (data_byte) { + sensor_value = data; + data_byte = false; + } + + // Check if it's a data message. This indicates the byte after this + // will contain data. + if (0b11000000 & data) { + // Next byte will be data + data_byte = true; + } + continue; + } + + if (payload_bytes > 0) { + payload_bytes--; + + if (payload_bytes > 1) { + if (receive_state == ReceiveState::final_mode_format && payload_bytes == 5 && data != 0x80) { + receive_state = ReceiveState::advertise; + payload_bytes = 0; + } + checksum ^= data; + current_payload = current_payload | + (((unsigned long)data) << payload_index * 8); + payload_index++; + } else if (checksum == data) { + if (receive_state == ReceiveState::advertise) { + printf("Baudrate = %d\n", current_payload); + baudrate = (int) current_payload; + } + else if (receive_state == ReceiveState::final_mode_format){ + receive_state = ReceiveState::modes_complete; + } + } + } + + // Handle ACK message. + if (data == 0b00000100) { + // If we receive an ACK after the final format message and we + // know which speed to communicate at then we should send an ACK to + // switch to data mode. + if (receive_state == ReceiveState::modes_complete && baudrate > 0) { + printf("Completing pairing sequence\n"); + uart_poll_out(uart_dev, 0b00000100); // Send ACK back + receive_state = ReceiveState::data; + } + } + + // Handle FORMAT commands. + if (data >> 3 == 0b10010) { + uint8_t sensor_mode = data & 0b111; + printf("FORMAT for mode %d\n", sensor_mode); + if (sensor_mode == 0) { + receive_state = ReceiveState::final_mode_format; + payload_bytes = 6; + payload_index = 0; + current_payload = 0; + checksum = 0xff ^ data; + } + } + + // Handle speed command. + if (data == 0b01010010) { + payload_bytes = 5; + payload_index = 0; + current_payload = 0; + checksum = 0xff ^ 0b01010010; + } + } +} + +void set_sensor_mode(uint8_t new_mode) { + uart_poll_out(uart_dev, 0x43); + uart_poll_out(uart_dev, new_mode); + uart_poll_out(uart_dev, 0xff ^ 0x43 ^ new_mode); +} + +void uartHeartbeat() { + if (receive_state != ReceiveState::data) { + return; + } + + if (!baudrate_configured) { + printf("Changing baudrate to %d\n", baudrate); + uart_config cfg{}; + uart_config_get(uart_dev, &cfg); + cfg.baudrate = baudrate; + + int config_err = uart_configure(uart_dev, &cfg); + printf("config_err = %d\n", config_err); + if (config_err) { + printf("UART configure error %d", config_err); + } + + config_err = uart_config_get(uart_dev, &cfg); + printf("current baudrate after config change = %d\n", cfg.baudrate); + printf("config_err = %d\n", config_err); + baudrate_configured = true; + + // Change to the desired mode. + set_sensor_mode(mode); + } + + if (baudrate_configured) { + uart_poll_out(uart_dev, 0b00000010); + } +} + +bool sensor_ready() { + return device_is_ready(uart_dev); +} + +int get_sensor_value() { + return sensor_value; +} + +bool configure_uart_sensor(uint8_t new_mode) { + if (!sensor_ready()) { + printf("Input port is not ready!\n"); + return false; + } + + printf("Setting up uart\n"); + int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, nullptr); + if (ret < 0) { + if (ret == -ENOTSUP) { + printf("Interrupt-driven UART API support not enabled\n"); + } else if (ret == -ENOSYS) { + printf("UART device does not support interrupt-driven API\n"); + } else { + printf("Error setting UART callback: %d\n", ret); + } + return false; + } + uart_irq_rx_enable(uart_dev); + if (receive_state == ReceiveState::data && mode != new_mode) { + set_sensor_mode(new_mode); + } + mode = new_mode; + return true; +} diff --git a/src/Primitives/Mindstorms/uart_sensor.h b/src/Primitives/Mindstorms/uart_sensor.h new file mode 100644 index 00000000..80dcd86f --- /dev/null +++ b/src/Primitives/Mindstorms/uart_sensor.h @@ -0,0 +1,21 @@ +#pragma once +#include + +enum ReceiveState { + advertise, + final_mode_format, + modes_complete, + data +}; + +void serial_cb([[maybe_unused]] const struct device *dev, [[maybe_unused]] void *user_data); + +void set_sensor_mode(uint8_t new_mode); + +void uartHeartbeat(); + +bool sensor_ready(); + +int get_sensor_value(); + +bool configure_uart_sensor(uint8_t new_mode); diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index a30e4d4e..a07019c8 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -12,21 +12,17 @@ * 4) Extend the install_primitives function * */ -#include #include #include #include #include #include -#include -#include #include #include #include #include #include -#include #include #include "../Memory/mem.h" @@ -34,6 +30,7 @@ #include "../Utils/util.h" #include "primitives.h" #include "Mindstorms/Motor.h" +#include "Mindstorms/uart_sensor.h" #define ALL_PRIMITIVES 11 @@ -416,153 +413,6 @@ def_prim_serialize(drive_motor_degrees) { } } -static const struct device *const uart_dev = - DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); - -// Variables used for setting up the sensor. -int payload_bytes = 0; -int payload_index = 0; -unsigned int current_payload = 0; -unsigned char checksum; -bool data_byte = false; - -// Variables that are used during setup but also by the heartbeat function. -int baudrate = -1; -uint8_t mode = 0; -volatile int sensor_value = 0; - -enum ReceiveState { - advertise, - final_mode_format, - modes_complete, - data -}; - -volatile uint32_t receive_state = ReceiveState::advertise; -bool baudrate_configured = false; - -void serial_cb(const struct device *dev, void *user_data) { - uint8_t data; - - if (!uart_irq_update(uart_dev)) { - return; - } - - if (!uart_irq_rx_ready(uart_dev)) { - return; - } - - while (uart_fifo_read(uart_dev, &data, 1) == 1) { - if (receive_state == ReceiveState::data) { - if (data_byte) { - sensor_value = data; - data_byte = false; - } - - // Check if it's a data message. This indicates the byte after this - // will contain data. - if (0b11000000 & data) { - // Next byte will be data - data_byte = true; - } - continue; - } - - if (payload_bytes > 0) { - payload_bytes--; - - if (payload_bytes > 1) { - if (receive_state == ReceiveState::final_mode_format && payload_bytes == 5 && data != 0x80) { - receive_state = ReceiveState::advertise; - payload_bytes = 0; - } - checksum ^= data; - current_payload = current_payload | - (((unsigned long)data) << payload_index * 8); - payload_index++; - } else if (checksum == data) { - if (receive_state == ReceiveState::advertise) { - printf("Baudrate = %d\n", current_payload); - baudrate = current_payload; - } - else if (receive_state == ReceiveState::final_mode_format){ - receive_state = ReceiveState::modes_complete; - } - } - } - - // Handle ACK message. - if (data == 0b00000100) { - // If we receive an ACK after the final format message and we - // know which speed to communicate at then we should send an ACK to - // switch to data mode. - if (receive_state == ReceiveState::modes_complete && baudrate > 0) { - printf("Completing pairing sequence\n"); - uart_poll_out(uart_dev, 0b00000100); // Send ACK back - receive_state = ReceiveState::data; - } - } - - // Handle FORMAT commands. - if (data >> 3 == 0b10010) { - uint8_t sensor_mode = data & 0b111; - printf("FORMAT for mode %d\n", sensor_mode); - if (sensor_mode == 0) { - receive_state = ReceiveState::final_mode_format; - payload_bytes = 6; - payload_index = 0; - current_payload = 0; - checksum = 0xff ^ data; - } - } - - // Handle speed command. - if (data == 0b01010010) { - payload_bytes = 5; - payload_index = 0; - current_payload = 0; - checksum = 0xff ^ 0b01010010; - } - } -} - -void set_sensor_mode(uint8_t new_mode) { - uart_poll_out(uart_dev, 0x43); - uart_poll_out(uart_dev, new_mode); - uart_poll_out(uart_dev, 0xff ^ 0x43 ^ new_mode); -} - -void uartHeartbeat() { - if (receive_state != ReceiveState::data) { - return; - } - - if (!baudrate_configured) { - printk("Changing baudrate to %d\n", baudrate); - uart_config cfg; - uart_config_get(uart_dev, &cfg); - cfg.baudrate = baudrate; - - int config_err = uart_configure(uart_dev, &cfg); - printf("config_err = %d\n", config_err); - if (config_err) { - printf("UART configure error %d", config_err); - } - - config_err = uart_config_get(uart_dev, &cfg); - printk("current baudrate after config change = %d\n", cfg.baudrate); - printf("config_err = %d\n", config_err); - baudrate_configured = true; - - // Change to the desired mode. - set_sensor_mode(mode); - } - - if (baudrate_configured) { - uart_poll_out(uart_dev, 0b00000010); - } -} - extern void read_debug_messages(); void debug_work_handler(struct k_work *work) { @@ -577,32 +427,6 @@ void heartbeat_timer_func(struct k_timer *timer_id) { k_work_submit(&debug_work); } -bool configure_uart_sensor(uint8_t new_mode) { - if (!device_is_ready(uart_dev)) { - printk("Input port is not ready!\n"); - return false; - } - - printk("Setting up uart\n"); - int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, nullptr); - if (ret < 0) { - if (ret == -ENOTSUP) { - printk("Interrupt-driven UART API support not enabled\n"); - } else if (ret == -ENOSYS) { - printk("UART device does not support interrupt-driven API\n"); - } else { - printk("Error setting UART callback: %d\n", ret); - } - return false; - } - uart_irq_rx_enable(uart_dev); - if (receive_state == ReceiveState::data && mode != new_mode) { - set_sensor_mode(new_mode); - } - mode = new_mode; - return true; -} - def_prim(setup_uart_sensor, twoToNoneU32) { bool result = configure_uart_sensor(arg0.uint32); pop_args(2); @@ -610,12 +434,15 @@ def_prim(setup_uart_sensor, twoToNoneU32) { } def_prim(color_sensor, oneToOneU32) { - if (!device_is_ready(uart_dev)) { + if (!sensor_ready()) { printk("Input port is not ready!\n"); return false; } pop_args(1); + int sensor_value = get_sensor_value(); + if (sensor_value < 0) sensor_value = 0; + if (sensor_value > 7) sensor_value = 7; pushUInt32(sensor_value); return true; } From 68b76e19656e36f5fe25ad68a43c8e2130720e97 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Wed, 4 Dec 2024 17:24:58 +0100 Subject: [PATCH 11/13] Fix case where invalid data was read from the sensor --- src/Primitives/Mindstorms/uart_sensor.cpp | 10 ++++++++-- src/Primitives/zephyr.cpp | 5 +---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/Primitives/Mindstorms/uart_sensor.cpp b/src/Primitives/Mindstorms/uart_sensor.cpp index 51e030f2..f6744227 100644 --- a/src/Primitives/Mindstorms/uart_sensor.cpp +++ b/src/Primitives/Mindstorms/uart_sensor.cpp @@ -38,10 +38,9 @@ void serial_cb([[maybe_unused]] const struct device *dev, [[maybe_unused]] void sensor_value = data; data_byte = false; } - // Check if it's a data message. This indicates the byte after this // will contain data. - if (0b11000000 & data) { + else if (data >> 6 == 0b11) { // Next byte will be data data_byte = true; } @@ -110,6 +109,10 @@ void set_sensor_mode(uint8_t new_mode) { uart_poll_out(uart_dev, 0x43); uart_poll_out(uart_dev, new_mode); uart_poll_out(uart_dev, 0xff ^ 0x43 ^ new_mode); + + // Invalidate current sensor values. This prevents the program reading values that it normally cannot read in a + // particular mode. + sensor_value = -1; } void uartHeartbeat() { @@ -148,6 +151,9 @@ bool sensor_ready() { } int get_sensor_value() { + if (!baudrate_configured || sensor_value < 0) { + return 0; + } return sensor_value; } diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index a07019c8..0bdeb6e6 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -440,10 +440,7 @@ def_prim(color_sensor, oneToOneU32) { } pop_args(1); - int sensor_value = get_sensor_value(); - if (sensor_value < 0) sensor_value = 0; - if (sensor_value > 7) sensor_value = 7; - pushUInt32(sensor_value); + pushUInt32(get_sensor_value()); return true; } From 5000e467dc7e17c2498180e5403e8ac8957a9409 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Thu, 5 Dec 2024 10:27:56 +0100 Subject: [PATCH 12/13] Move uart sensor data into a struct By doing so you could theoretically now have multiple uart sensors working at the same time. --- src/Primitives/Mindstorms/uart_sensor.cpp | 189 ++++++++++------------ src/Primitives/Mindstorms/uart_sensor.h | 32 +++- src/Primitives/zephyr.cpp | 14 +- 3 files changed, 121 insertions(+), 114 deletions(-) diff --git a/src/Primitives/Mindstorms/uart_sensor.cpp b/src/Primitives/Mindstorms/uart_sensor.cpp index f6744227..b330e522 100644 --- a/src/Primitives/Mindstorms/uart_sensor.cpp +++ b/src/Primitives/Mindstorms/uart_sensor.cpp @@ -3,69 +3,52 @@ #include #include -static const struct device *const uart_dev = - DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); - -// Variables used for setting up the sensor. -int payload_bytes = 0; -int payload_index = 0; -unsigned int current_payload = 0; -unsigned char checksum; -bool data_byte = false; - -// Variables that are used during setup but also by the heartbeat function. -int baudrate = -1; -uint8_t mode = 0; -volatile int sensor_value = 0; - -volatile uint32_t receive_state = ReceiveState::advertise; -bool baudrate_configured = false; - -void serial_cb([[maybe_unused]] const struct device *dev, [[maybe_unused]] void *user_data) { +void serial_cb(const struct device *dev, void *user_data) { + auto *sensor = static_cast(user_data); uint8_t data; - if (!uart_irq_update(uart_dev)) { + if (!uart_irq_update(dev)) { return; } - if (!uart_irq_rx_ready(uart_dev)) { + if (!uart_irq_rx_ready(dev)) { return; } - while (uart_fifo_read(uart_dev, &data, 1) == 1) { - if (receive_state == ReceiveState::data) { - if (data_byte) { - sensor_value = data; - data_byte = false; + while (uart_fifo_read(dev, &data, 1) == 1) { + if (sensor->receive_state == ReceiveState::data) { + if (sensor->data_byte) { + sensor->sensor_value = data; + sensor->data_byte = false; } // Check if it's a data message. This indicates the byte after this // will contain data. else if (data >> 6 == 0b11) { // Next byte will be data - data_byte = true; + sensor->data_byte = true; } continue; } - if (payload_bytes > 0) { - payload_bytes--; + if (sensor->payload_bytes > 0) { + sensor->payload_bytes--; - if (payload_bytes > 1) { - if (receive_state == ReceiveState::final_mode_format && payload_bytes == 5 && data != 0x80) { - receive_state = ReceiveState::advertise; - payload_bytes = 0; + if (sensor->payload_bytes > 1) { + if (sensor->receive_state == ReceiveState::final_mode_format && sensor->payload_bytes == 5 && data != 0x80) { + sensor->receive_state = ReceiveState::advertise; + sensor->payload_bytes = 0; } - checksum ^= data; - current_payload = current_payload | - (((unsigned long)data) << payload_index * 8); - payload_index++; - } else if (checksum == data) { - if (receive_state == ReceiveState::advertise) { - printf("Baudrate = %d\n", current_payload); - baudrate = (int) current_payload; + sensor->checksum ^= data; + sensor->current_payload = sensor->current_payload | + (((unsigned long)data) << sensor->payload_index * 8); + sensor->payload_index++; + } else if (sensor->checksum == data) { + if (sensor->receive_state == ReceiveState::advertise) { + printf("Baudrate = %d\n", sensor->current_payload); + sensor->baudrate = (int) sensor->current_payload; } - else if (receive_state == ReceiveState::final_mode_format){ - receive_state = ReceiveState::modes_complete; + else if (sensor->receive_state == ReceiveState::final_mode_format){ + sensor->receive_state = ReceiveState::modes_complete; } } } @@ -75,10 +58,10 @@ void serial_cb([[maybe_unused]] const struct device *dev, [[maybe_unused]] void // If we receive an ACK after the final format message and we // know which speed to communicate at then we should send an ACK to // switch to data mode. - if (receive_state == ReceiveState::modes_complete && baudrate > 0) { + if (sensor->receive_state == ReceiveState::modes_complete && sensor->baudrate > 0) { printf("Completing pairing sequence\n"); - uart_poll_out(uart_dev, 0b00000100); // Send ACK back - receive_state = ReceiveState::data; + uart_poll_out(dev, 0b00000100); // Send ACK back + sensor->receive_state = ReceiveState::data; } } @@ -87,98 +70,96 @@ void serial_cb([[maybe_unused]] const struct device *dev, [[maybe_unused]] void uint8_t sensor_mode = data & 0b111; printf("FORMAT for mode %d\n", sensor_mode); if (sensor_mode == 0) { - receive_state = ReceiveState::final_mode_format; - payload_bytes = 6; - payload_index = 0; - current_payload = 0; - checksum = 0xff ^ data; + sensor->receive_state = ReceiveState::final_mode_format; + sensor->payload_bytes = 6; + sensor->payload_index = 0; + sensor->current_payload = 0; + sensor->checksum = 0xff ^ data; } } // Handle speed command. if (data == 0b01010010) { - payload_bytes = 5; - payload_index = 0; - current_payload = 0; - checksum = 0xff ^ 0b01010010; + sensor->payload_bytes = 5; + sensor->payload_index = 0; + sensor->current_payload = 0; + sensor->checksum = 0xff ^ 0b01010010; } } } -void set_sensor_mode(uint8_t new_mode) { - uart_poll_out(uart_dev, 0x43); - uart_poll_out(uart_dev, new_mode); - uart_poll_out(uart_dev, 0xff ^ 0x43 ^ new_mode); +void set_sensor_mode(UartSensor *sensor, uint8_t new_mode) { + uart_poll_out(sensor->dev, 0x43); + uart_poll_out(sensor->dev, new_mode); + uart_poll_out(sensor->dev, 0xff ^ 0x43 ^ new_mode); // Invalidate current sensor values. This prevents the program reading values that it normally cannot read in a // particular mode. - sensor_value = -1; + sensor->sensor_value = -1; } -void uartHeartbeat() { - if (receive_state != ReceiveState::data) { +bool configure_uart_sensor(UartSensor *sensor, uint8_t new_mode) { + if (!sensor_ready(sensor)) { + printf("Input port is not ready!\n"); + return false; + } + + printf("Setting up uart\n"); + int ret = uart_irq_callback_user_data_set(sensor->dev, serial_cb, sensor); + if (ret < 0) { + if (ret == -ENOTSUP) { + printf("Interrupt-driven UART API support not enabled\n"); + } else if (ret == -ENOSYS) { + printf("UART device does not support interrupt-driven API\n"); + } else { + printf("Error setting UART callback: %d\n", ret); + } + return false; + } + uart_irq_rx_enable(sensor->dev); + if (sensor->receive_state == ReceiveState::data && sensor->mode != new_mode) { + set_sensor_mode(sensor, new_mode); + } + sensor->mode = new_mode; + return true; +} + +void uartHeartbeat(UartSensor *sensor) { + if (sensor->receive_state != ReceiveState::data) { return; } - if (!baudrate_configured) { - printf("Changing baudrate to %d\n", baudrate); + if (!sensor->baudrate_configured) { + printf("Changing baudrate to %d\n", sensor->baudrate); uart_config cfg{}; - uart_config_get(uart_dev, &cfg); - cfg.baudrate = baudrate; + uart_config_get(sensor->dev, &cfg); + cfg.baudrate = sensor->baudrate; - int config_err = uart_configure(uart_dev, &cfg); + int config_err = uart_configure(sensor->dev, &cfg); printf("config_err = %d\n", config_err); if (config_err) { printf("UART configure error %d", config_err); } - config_err = uart_config_get(uart_dev, &cfg); + config_err = uart_config_get(sensor->dev, &cfg); printf("current baudrate after config change = %d\n", cfg.baudrate); printf("config_err = %d\n", config_err); - baudrate_configured = true; + sensor->baudrate_configured = true; // Change to the desired mode. - set_sensor_mode(mode); + set_sensor_mode(sensor, sensor->mode); } - if (baudrate_configured) { - uart_poll_out(uart_dev, 0b00000010); - } + uart_poll_out(sensor->dev, 0b00000010); } -bool sensor_ready() { - return device_is_ready(uart_dev); +bool sensor_ready(UartSensor *sensor) { + return device_is_ready(sensor->dev); } -int get_sensor_value() { - if (!baudrate_configured || sensor_value < 0) { +int get_sensor_value(UartSensor *sensor) { + if (!sensor->baudrate_configured || sensor->sensor_value < 0) { return 0; } - return sensor_value; -} - -bool configure_uart_sensor(uint8_t new_mode) { - if (!sensor_ready()) { - printf("Input port is not ready!\n"); - return false; - } - - printf("Setting up uart\n"); - int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, nullptr); - if (ret < 0) { - if (ret == -ENOTSUP) { - printf("Interrupt-driven UART API support not enabled\n"); - } else if (ret == -ENOSYS) { - printf("UART device does not support interrupt-driven API\n"); - } else { - printf("Error setting UART callback: %d\n", ret); - } - return false; - } - uart_irq_rx_enable(uart_dev); - if (receive_state == ReceiveState::data && mode != new_mode) { - set_sensor_mode(new_mode); - } - mode = new_mode; - return true; + return sensor->sensor_value; } diff --git a/src/Primitives/Mindstorms/uart_sensor.h b/src/Primitives/Mindstorms/uart_sensor.h index 80dcd86f..6ddacf49 100644 --- a/src/Primitives/Mindstorms/uart_sensor.h +++ b/src/Primitives/Mindstorms/uart_sensor.h @@ -1,5 +1,6 @@ #pragma once #include +#include enum ReceiveState { advertise, @@ -8,14 +9,33 @@ enum ReceiveState { data }; -void serial_cb([[maybe_unused]] const struct device *dev, [[maybe_unused]] void *user_data); +struct UartSensor { + // Variables used for setting up the sensor. + int payload_bytes = 0; + int payload_index = 0; + unsigned int current_payload = 0; + unsigned char checksum = 0; + bool data_byte = false; -void set_sensor_mode(uint8_t new_mode); + // Variables that are used during setup but also by the heartbeat function. + int baudrate = -1; + uint8_t mode = 0; + volatile int sensor_value = 0; + volatile uint32_t receive_state = ReceiveState::advertise; + bool baudrate_configured = false; -void uartHeartbeat(); + // Associated UART device. + const struct device *dev; -bool sensor_ready(); + explicit UartSensor(const struct device *dev) : dev(dev) {} +}; + +void serial_cb(const struct device *dev, void *user_data); + +void uartHeartbeat(UartSensor *sensor); + +bool sensor_ready(UartSensor *sensor); -int get_sensor_value(); +int get_sensor_value(UartSensor *sensor); -bool configure_uart_sensor(uint8_t new_mode); +bool configure_uart_sensor(UartSensor *sensor, uint8_t new_mode); diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index 0bdeb6e6..7a1b0090 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -413,6 +413,11 @@ def_prim_serialize(drive_motor_degrees) { } } +static const struct device *const uart_dev = + DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); + +UartSensor sensor(uart_dev); + extern void read_debug_messages(); void debug_work_handler(struct k_work *work) { @@ -423,24 +428,25 @@ K_WORK_DEFINE(debug_work, debug_work_handler); struct k_timer heartbeat_timer; void heartbeat_timer_func(struct k_timer *timer_id) { - uartHeartbeat(); + uartHeartbeat(&sensor); k_work_submit(&debug_work); } def_prim(setup_uart_sensor, twoToNoneU32) { - bool result = configure_uart_sensor(arg0.uint32); + bool result = configure_uart_sensor(&sensor, arg0.uint32); pop_args(2); return result; } def_prim(color_sensor, oneToOneU32) { - if (!sensor_ready()) { + if (!sensor_ready(&sensor)) { printk("Input port is not ready!\n"); return false; } pop_args(1); - pushUInt32(get_sensor_value()); + int value = get_sensor_value(&sensor); + pushUInt32(value); return true; } From e2f3c568aa03c50382488bf869d29d4bf7565267 Mon Sep 17 00:00:00 2001 From: MaartenS11 Date: Thu, 5 Dec 2024 11:12:15 +0100 Subject: [PATCH 13/13] Misc type conversions --- src/Primitives/zephyr.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index 7a1b0090..ec7d1024 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -318,7 +318,7 @@ MotorEncoder *encoders[] = {new MotorEncoder(specs[51], specs[50], "Port A"), new MotorEncoder(specs[17], specs[13], "Port C"), new MotorEncoder(specs[27], specs[26], "Port D")}; -std::optional get_motor(int32_t motor_index) { +std::optional get_motor(uint32_t motor_index) { if (motor_index > 3) { return {}; } @@ -329,7 +329,7 @@ std::optional get_motor(int32_t motor_index) { def_prim(drive_motor, threeToNoneU32) { int32_t brake = arg0.int32; int32_t speed = arg1.int32; - int32_t motor_index = arg2.int32; + uint32_t motor_index = arg2.uint32; printf("drive_motor(%d, %d, %d)\n", motor_index, speed, brake); @@ -352,7 +352,7 @@ def_prim(drive_motor, threeToNoneU32) { def_prim(drive_motor_ms, twoToNoneU32) { int32_t motor_index = arg1.int32; - int32_t speed = (int32_t)arg0.uint32; + int32_t speed = arg0.int32; printf("drive_motor_ms(%d, %d)\n", motor_index, speed); Motor motor = get_motor(motor_index).value(); @@ -363,7 +363,7 @@ def_prim(drive_motor_ms, twoToNoneU32) { return true; } -bool drive_motor_degrees_absolute(int32_t motor_index, int32_t degrees, int32_t speed) { +bool drive_motor_degrees_absolute(uint32_t motor_index, int32_t degrees, int32_t speed) { if (auto motor = get_motor(motor_index)) { motor->drive_to_angle(speed, degrees); return true; @@ -371,7 +371,7 @@ bool drive_motor_degrees_absolute(int32_t motor_index, int32_t degrees, int32_t return false; } -bool drive_motor_degrees_relative(int32_t motor_index, int32_t degrees, int32_t speed) { +bool drive_motor_degrees_relative(uint32_t motor_index, int32_t degrees, int32_t speed) { MotorEncoder *encoder = encoders[motor_index]; drive_motor_degrees_absolute(motor_index, encoder->get_target_angle() + degrees, speed); return true; @@ -379,8 +379,8 @@ bool drive_motor_degrees_relative(int32_t motor_index, int32_t degrees, int32_t def_prim(drive_motor_degrees, threeToNoneU32) { int32_t speed = arg0.int32; - int32_t degrees = arg1.uint32; - int32_t motor_index = arg2.uint32; + int32_t degrees = arg1.int32; + uint32_t motor_index = arg2.uint32; pop_args(3); return drive_motor_degrees_relative(motor_index, degrees, speed); }