diff --git a/platforms/Zephyr/CMakeLists.txt b/platforms/Zephyr/CMakeLists.txt index b17ff7fd..96e1c13a 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 @@ -16,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..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/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 diff --git a/src/Primitives/Mindstorms/Motor.cpp b/src/Primitives/Mindstorms/Motor.cpp new file mode 100644 index 00000000..053be25e --- /dev/null +++ b/src/Primitives/Mindstorms/Motor.cpp @@ -0,0 +1,113 @@ +#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(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; + set_speed(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()); + 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("%lli\n", k_uptime_get() - encoder->get_last_update()); + halt(); + k_msleep(50); + drift = encoder->get_angle() - encoder->get_target_angle(); + printf("drift = %d, speed = %d\n", drift, speed); + 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 new file mode 100644 index 00000000..f44254d5 --- /dev/null +++ b/src/Primitives/Mindstorms/Motor.h @@ -0,0 +1,91 @@ +#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); + + 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..b330e522 --- /dev/null +++ b/src/Primitives/Mindstorms/uart_sensor.cpp @@ -0,0 +1,165 @@ +#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) { + 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 + sensor->data_byte = true; + } + 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); +} + +int get_sensor_value(UartSensor *sensor) { + if (!sensor->baudrate_configured || sensor->sensor_value < 0) { + return 0; + } + return sensor->sensor_value; +} diff --git a/src/Primitives/Mindstorms/uart_sensor.h b/src/Primitives/Mindstorms/uart_sensor.h new file mode 100644 index 00000000..6ddacf49 --- /dev/null +++ b/src/Primitives/Mindstorms/uart_sensor.h @@ -0,0 +1,41 @@ +#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; + 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; + + // 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); + +int 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 d7273b0d..ec7d1024 100644 --- a/src/Primitives/zephyr.cpp +++ b/src/Primitives/zephyr.cpp @@ -12,28 +12,27 @@ * 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 "primitives.h" +#include "Mindstorms/Motor.h" +#include "Mindstorms/uart_sensor.h" -#define ALL_PRIMITIVES 5 +#define ALL_PRIMITIVES 11 // Global index for installing primitives int prim_index = 0; @@ -223,6 +222,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 +231,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 +264,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 +313,148 @@ 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"), + new MotorEncoder(specs[17], specs[13], "Port C"), + new MotorEncoder(specs[27], specs[26], "Port D")}; + +std::optional get_motor(uint32_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 = arg0.int32; + int32_t speed = arg1.int32; + uint32_t motor_index = arg2.uint32; + + printf("drive_motor(%d, %d, %d)\n", motor_index, speed, brake); + + if (motor_index > 3) { + printf("Invalid motor index %d\n", motor_index); + pop_args(3); + return true; + } + + Motor motor = get_motor(motor_index).value(); + motor.set_speed(speed / 10000.0f); + + if (speed == 0 && brake == 1) { + motor.halt(); + } + + pop_args(3); + return true; +} + +def_prim(drive_motor_ms, twoToNoneU32) { + int32_t motor_index = arg1.int32; + int32_t speed = arg0.int32; + printf("drive_motor_ms(%d, %d)\n", motor_index, speed); + + Motor motor = get_motor(motor_index).value(); + motor.set_speed(speed / 10000.0f); + k_msleep(arg1.uint32); + motor.halt(); + pop_args(2); + return true; +} + +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]; + 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.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); + +extern void read_debug_messages(); + +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 heartbeat_timer_func(struct k_timer *timer_id) { + uartHeartbeat(&sensor); + k_work_submit(&debug_work); +} + +def_prim(setup_uart_sensor, twoToNoneU32) { + bool result = configure_uart_sensor(&sensor, arg0.uint32); + pop_args(2); + return result; +} + +def_prim(color_sensor, oneToOneU32) { + if (!sensor_ready(&sensor)) { + printk("Input port is not ready!\n"); + return false; + } + + pop_args(1); + int value = get_sensor_value(&sensor); + pushUInt32(value); + return true; +} + +def_prim(abort, NoneToNoneU32) { + printf("abort\n"); + return false; +} + //------------------------------------------------------ // Installing all the primitives //------------------------------------------------------ @@ -270,8 +463,22 @@ 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(drive_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); + + install_primitive(abort); + + k_timer_init(&heartbeat_timer, heartbeat_timer_func, nullptr); + k_timer_start(&heartbeat_timer, K_MSEC(990), K_MSEC(990)); } //------------------------------------------------------ @@ -317,24 +524,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); } } }