diff --git a/platforms/Zephyr/CMakeLists.txt b/platforms/Zephyr/CMakeLists.txt index 0ed056c0..96e1c13a 100644 --- a/platforms/Zephyr/CMakeLists.txt +++ b/platforms/Zephyr/CMakeLists.txt @@ -19,6 +19,8 @@ target_sources(app PRIVATE ../../src/Interpreter/instructions.cpp ../../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/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..9d5dd8b8 100644 --- a/platforms/Zephyr/main.cpp +++ b/platforms/Zephyr/main.cpp @@ -64,7 +64,7 @@ void startDebuggerStd() { int valread; uint8_t buffer[1024] = {0}; while (true) { - k_msleep(1000); + k_msleep(500); while ((valread = war_console_read(NULL, buffer, 1024)) > 0) { wac->handleInterrupt(valread, buffer); diff --git a/platforms/Zephyr/prj.conf b/platforms/Zephyr/prj.conf index c5107231..104dffb5 100644 --- a/platforms/Zephyr/prj.conf +++ b/platforms/Zephyr/prj.conf @@ -30,3 +30,7 @@ CONFIG_CONSOLE_PUTCHAR_BUFSIZE=4096 CONFIG_POSIX_API=y CONFIG_MAIN_STACK_SIZE=8192 + +# Allow flashing the board without reset (increases power consumption so should only be used for development) +CONFIG_STM32_ENABLE_DEBUG_SLEEP_STOP=y + diff --git a/src/Primitives/Mindstorms/Motor.cpp b/src/Primitives/Mindstorms/Motor.cpp new file mode 100644 index 00000000..19330dce --- /dev/null +++ b/src/Primitives/Mindstorms/Motor.cpp @@ -0,0 +1,123 @@ +#ifndef ARDUINO +#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), + 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( + &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); + } + 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; +} + +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) { + pwm1 = speed; + } else { + pwm2 = -speed; + } + + return drive_pwm(pwm1_spec, pwm2_spec, pwm1, pwm2); +} + +void Motor::drive_to_target(int32_t speed) { + printf("drift = %d\n", abs(get_drift())); + + int drift = get_drift(); + // 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; + set_speed(speed_sign * speed / 10000.0f); + while (speed_sign * (get_drift()) > 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()); + set_speed(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("elapsed = %lli\n", k_uptime_get() - encoder->get_last_update()); + halt(); + k_msleep(50); + drift = get_drift(); + printf("drift = %d, speed = %d\n", drift, speed); + speed = 800; + } +} + +int Motor::get_drift() { + return encoder->get_target_angle() - encoder->get_angle(); +} + +void Motor::drive_to_angle(int32_t speed, int32_t degrees) { + encoder->set_target_angle(degrees); + drive_to_target(speed); +} + +#endif diff --git a/src/Primitives/Mindstorms/Motor.h b/src/Primitives/Mindstorms/Motor.h new file mode 100644 index 00000000..afcd8750 --- /dev/null +++ b/src/Primitives/Mindstorms/Motor.h @@ -0,0 +1,95 @@ +#ifndef WARDUINO_MOTOR_H +#define WARDUINO_MOTOR_H + +#include +#include +#include + +#include +#include + +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); + + 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); + + if (rising) { + if (pin5) { + encoder->angle++; + } else { + encoder->angle--; + } + } else { + if (pin5) { + encoder->angle--; + } else { + encoder->angle++; + } + } + encoder->last_update = k_uptime_get(); + } + + public: + MotorEncoder(gpio_dt_spec pin5_encoder_spec, gpio_dt_spec pin6_encoder_spec, + const char *name); + + ~MotorEncoder(); + + [[nodiscard]] inline int get_angle() const { return angle; } + + inline void reset_angle() { angle = 0; } + + [[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; + } + + [[nodiscard]] inline int64_t get_last_update() const { 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; + + public: + 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); + int get_drift(); + + 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); + +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/Mindstorms/uart_sensor.cpp b/src/Primitives/Mindstorms/uart_sensor.cpp new file mode 100644 index 00000000..7ea33190 --- /dev/null +++ b/src/Primitives/Mindstorms/uart_sensor.cpp @@ -0,0 +1,185 @@ +#ifndef ARDUINO +#include "uart_sensor.h" + +#include + +#include + +void serial_cb(const struct device *dev, void *user_data) { + auto *sensor = static_cast(user_data); + uint8_t data; + + if (!uart_irq_update(dev)) { + return; + } + + if (!uart_irq_rx_ready(dev)) { + return; + } + + while (uart_fifo_read(dev, &data, 1) == 1) { + // printf("data: 0x%02x %c\n", data, data); + + if (sensor->receive_state == ReceiveState::data) { + if (sensor->data_byte > 0) { + //printf("data_byte = %d, data = %d, offset = %d\n", sensor->data_byte, data, 8 * (2 - sensor->data_byte)); + //sensor->sensor_value = data; + sensor->new_sensor_value |= static_cast(data) << 8 * (2 - sensor->data_byte); + sensor->data_byte--; + if (sensor->data_byte == 0) { + //printf("Writing sensor value %d\n", sensor->new_sensor_value); + sensor->sensor_value = sensor->new_sensor_value; + //printf("Sensor value = %d\n", sensor->sensor_value); + sensor->new_sensor_value = 0; + } + } + // Check if it's a data message. This indicates the byte after this + // will contain data. + else if (data >> 6 == 0b11) { + uint32_t len = (data >> 3) & 0b111; + // We only support length 2^1 currently. + if (len != 1) { + continue; + } + // Next byte will be data + sensor->data_byte = 2; + } + continue; + } + + if (sensor->payload_bytes > 0) { + sensor->payload_bytes--; + + 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; + } + 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 (sensor->receive_state == + ReceiveState::final_mode_format) { + sensor->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 (sensor->receive_state == ReceiveState::modes_complete && + sensor->baudrate > 0) { + printf("Completing pairing sequence\n"); + uart_poll_out(dev, 0b00000100); // Send ACK back + sensor->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) { + 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) { + sensor->payload_bytes = 5; + sensor->payload_index = 0; + sensor->current_payload = 0; + sensor->checksum = 0xff ^ 0b01010010; + } + } +} + +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->sensor_value = -1; +} + +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 (!sensor->baudrate_configured) { + printf("Changing baudrate to %d\n", sensor->baudrate); + uart_config cfg{}; + uart_config_get(sensor->dev, &cfg); + cfg.baudrate = sensor->baudrate; + + 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(sensor->dev, &cfg); + printf("current baudrate after config change = %d\n", cfg.baudrate); + printf("config_err = %d\n", config_err); + sensor->baudrate_configured = true; + + // Change to the desired mode. + set_sensor_mode(sensor, sensor->mode); + } + + uart_poll_out(sensor->dev, 0b00000010); +} + +bool sensor_ready(UartSensor *sensor) { return device_is_ready(sensor->dev); } + +int32_t get_sensor_value(UartSensor *sensor) { + if (!sensor->baudrate_configured) { + return 0; + } + return sensor->sensor_value; +} +#endif \ No newline at end of file diff --git a/src/Primitives/Mindstorms/uart_sensor.h b/src/Primitives/Mindstorms/uart_sensor.h new file mode 100644 index 00000000..d8bf0a44 --- /dev/null +++ b/src/Primitives/Mindstorms/uart_sensor.h @@ -0,0 +1,38 @@ +#pragma once +#include + +#include + +enum ReceiveState { advertise, final_mode_format, modes_complete, 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; + int data_byte = 0; + + // Variables that are used during setup but also by the heartbeat function. + int baudrate = -1; + uint8_t mode = 0; + volatile int32_t sensor_value = 0; + volatile int16_t new_sensor_value = 0; + volatile uint32_t receive_state = ReceiveState::advertise; + bool baudrate_configured = false; + + // Associated UART device. + const struct device *dev; + + 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); + +int32_t get_sensor_value(UartSensor *sensor); + +bool configure_uart_sensor(UartSensor *sensor, uint8_t new_mode); diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp index 4a9303ee..3117a332 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -11,28 +11,31 @@ * 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" #include "../Utils/macros.h" #include "../Utils/util.h" +#include "Mindstorms/Motor.h" +#include "Mindstorms/uart_sensor.h" #include "primitives.h" -#define ALL_PRIMITIVES 5 +#define OBB_PRIMITIVES 0 +#ifdef CONFIG_BOARD_STM32L496G_DISCO +#define OBB_PRIMITIVES 6 +#endif +#define ALL_PRIMITIVES OBB_PRIMITIVES + 6 // Global index for installing primitives int prim_index = 0; @@ -119,6 +122,15 @@ Type oneToNoneU32 = { .mask = 0x8001 /* 0x800 = no return ; 1 = I32*/ }; +Type oneToNoneI32 = { + .form = FUNC, + .param_count = 1, + .params = param_I32_arr_len1, + .result_count = 0, + .results = nullptr, + .mask = 0x8001 /* 0x800 = no return ; 1 = I32*/ +}; + Type twoToNoneU32 = { .form = FUNC, .param_count = 2, @@ -223,6 +235,7 @@ Type NoneToOneU64 = {.form = FUNC, .mask = 0x82000}; def_prim(chip_delay, oneToNoneU32) { + k_yield(); k_msleep(arg0.uint32); pop_args(1); return true; @@ -239,14 +252,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]; @@ -256,12 +295,165 @@ def_prim(chip_digital_read, oneToOneU32) { return true; } -def_prim(print_int, oneToNoneU32) { - printf("%u\n", arg0.uint32); +def_prim(print_int, oneToNoneI32) { + printf("%d\n", arg0.int32); pop_args(1); return true; } +def_prim(abort, NoneToNoneU32) { + printf("abort\n"); + return false; +} + +#ifdef CONFIG_BOARD_STM32L496G_DISCO + +MotorEncoder *encoders[] = {new MotorEncoder(specs[51], specs[50], "Port A"), + new MotorEncoder(specs[57], specs[58], "Port B"), + new MotorEncoder(specs[17], specs[13], "Port C"), + new MotorEncoder(specs[27], specs[26], "Port D")}; + +#define PWM_SPEC_GETTER(node_id, prop, idx) \ + PWM_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), idx) +struct pwm_dt_spec pwm_specs[] = {DT_FOREACH_PROP_ELEM_SEP( + DT_PATH(zephyr_user), pwms, PWM_SPEC_GETTER, (, ))}; + +std::optional get_motor(uint32_t motor_index) { + if (motor_index > sizeof(encoders) / sizeof(*encoders)) { + return {}; + } + + return std::make_optional(pwm_specs[motor_index * 2 + 1], + pwm_specs[motor_index * 2], + encoders[motor_index]); +} + +def_prim(drive_motor, twoToNoneU32) { + int32_t speed = arg0.int32; + uint32_t motor_index = arg1.uint32; + printf("drive_motor(%d, %d)\n", motor_index, speed); + pop_args(2); + + if (auto motor = get_motor(motor_index)) { + motor.value().set_speed(speed / 10000.0f); + return true; + } + + printf("Invalid motor index %d\n", motor_index); + return false; +} + +def_prim(stop_motor, oneToNoneU32) { + uint32_t motor_index = arg0.uint32; + pop_args(1); + + if (auto motor = get_motor(motor_index)) { + motor.value().halt(); + return true; + } + + printf("Invalid motor index %d\n", motor_index); + return false; +} + +def_prim(drive_motor_ms, threeToNoneU32) { + int32_t time = arg0.uint32; + int32_t speed = arg1.int32; + int32_t motor_index = arg2.int32; + printf("drive_motor_ms(%d, %d, %d)\n", motor_index, speed, time); + pop_args(3); + + if (auto motor = get_motor(motor_index)) { + motor.value().set_speed(speed / 10000.0f); + k_msleep(time); + motor.value().halt(); + return true; + } + + printf("Invalid motor index %d\n", motor_index); + return false; +} + +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; + } + return false; +} + +bool drive_motor_degrees_relative(uint32_t motor_index, int32_t degrees, + int32_t speed) { + MotorEncoder *encoder = encoders[motor_index]; + return drive_motor_degrees_absolute( + motor_index, encoder->get_target_angle() + degrees, speed); +} + +def_prim(drive_motor_degrees, threeToNoneU32) { + int32_t degrees = arg0.int32; + int32_t speed = arg1.int32; + uint32_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. + drive_motor_degrees_absolute(motor_index, (int32_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); + } +} + +static const struct device *const uart_dev = + DEVICE_DT_GET(DT_PROP(DT_PATH(zephyr_user), warduino_uarts)); + +UartSensor sensor(uart_dev); + +def_prim(setup_uart_sensor, twoToNoneU32) { + bool result = configure_uart_sensor(&sensor, arg0.uint32); + pop_args(2); + return result; +} + +def_prim(color_sensor, oneToOneI32) { + if (!sensor_ready(&sensor)) { + printk("Input port is not ready!\n"); + return false; + } + + pop_args(1); + int32_t value = get_sensor_value(&sensor); + pushInt32(value); + return true; +} + +struct k_timer heartbeat_timer; +void heartbeat_timer_func(struct k_timer *timer_id) { uartHeartbeat(&sensor); } +#endif + //------------------------------------------------------ // Installing all the primitives //------------------------------------------------------ @@ -270,8 +462,24 @@ 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(abort); + +#ifdef CONFIG_BOARD_STM32L496G_DISCO + install_primitive(drive_motor); + install_primitive(stop_motor); + install_primitive(drive_motor_ms); + install_primitive(drive_motor_degrees); + install_primitive_reverse(drive_motor_degrees); + + install_primitive(color_sensor); + install_primitive(setup_uart_sensor); + + k_timer_init(&heartbeat_timer, heartbeat_timer_func, nullptr); + k_timer_start(&heartbeat_timer, K_MSEC(990), K_MSEC(990)); +#endif } //------------------------------------------------------ @@ -317,24 +525,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); } } }