diff --git a/dependencies.lock b/dependencies.lock index 764e12b..2c5ddfc 100644 --- a/dependencies.lock +++ b/dependencies.lock @@ -66,6 +66,14 @@ dependencies: registry_url: https://components.espressif.com/ type: service version: 2.7.0 + hayschan/autopid_for_esp_idf: + component_hash: + abb01ee57353d22df70c27840ddec00b90544540c9a78ec18932737d7fbb7878 + dependencies: [] + source: + registry_url: https://components.espressif.com/ + type: service + version: 1.0.2 idf: source: type: idf @@ -82,8 +90,9 @@ direct_dependencies: - atanisoft/esp_lcd_touch_xpt2046 - espressif/esp_lcd_ili9341 - espressif/esp_lvgl_port +- hayschan/autopid_for_esp_idf - idf - lvgl/lvgl -manifest_hash: d2b549f17124d3d38e4add4614fb10956ada1a576e4c5f6a623ea3756bcd3ba5 +manifest_hash: 5e3bb9d0c782ebaa9ecdc3b0310e8f13b698e0fd5071cab2a465725e2a33722b target: esp32 version: 2.0.0 diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index e37bfaa..7b56bec 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,4 +1,4 @@ -idf_component_register(SRCS "spincoat-plater-firmware.c" "dshot_esc_encoder.c" "display.c" "motor.c" "ui.c" +idf_component_register(SRCS "spincoat-plater-firmware.cpp" "dshot_esc_encoder.c" "display.c" "motor.c" "ui.c" PRIV_REQUIRES esp_driver_rmt esp_driver_gpio esp_driver_uart esp_driver_spi - esp_lcd unity lvgl esp_lvgl_port esp_lcd_touch_xpt2046 + esp_lcd unity lvgl esp_lvgl_port esp_lcd_touch_xpt2046 autopid_for_esp_idf INCLUDE_DIRS ".") diff --git a/main/idf_component.yml b/main/idf_component.yml index a3a1e9e..041403d 100644 --- a/main/idf_component.yml +++ b/main/idf_component.yml @@ -18,3 +18,4 @@ dependencies: lvgl/lvgl: ^9.4.0 espressif/esp_lvgl_port: ^2.3.0 atanisoft/esp_lcd_touch_xpt2046: ^1.0.2 + hayschan/autopid_for_esp_idf: ^1.0.2 diff --git a/main/motor.c b/main/motor.c index a48ce56..92be0c2 100644 --- a/main/motor.c +++ b/main/motor.c @@ -145,7 +145,7 @@ uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen){ * Parse the KISS telemetry frame and check the crc8 * TODO: Do more with the data than print it */ -void parse_telemetry(void) { +bool parse_telemetry(esc_telemetry_t * telemetry) { uint8_t frame_size = 10; uint8_t data[128]; // get data @@ -153,28 +153,30 @@ void parse_telemetry(void) { uart_flush(ESC_UART_NUM); - if(length < 10) return; - + if(length < 10) return false; + // chop out just the payload uint8_t payload[128]; uint8_t payload_length = (frame_size - 1); for(uint8_t i = 0; i < payload_length; i++) { payload[i] = data[i]; } - + // calculate the crc8 uint8_t expected_crc8 = get_crc8(payload, payload_length); uint8_t received_crc8 = (uint8_t) data[frame_size - 1]; - if(expected_crc8 != received_crc8) return; + if(expected_crc8 != received_crc8) return false; - for(uint8_t i = 0; i < length; i++) { - printf("%d - %d\n", i, data[i]); - } - printf("--------------------\n"); - printf("expected: %d\n", expected_crc8); - printf("received: %d\n", received_crc8); - printf("======================\n"); + + telemetry->temperature = payload[0]; + telemetry->voltage = (payload[1] << 8) + payload[2]; + telemetry->current = (payload[3] << 8) + payload[4]; + telemetry->consumption = (payload[5] << 8) + payload[6]; + telemetry->rpm = (payload[7] << 8) + payload[8]; + + + return true; } /** diff --git a/main/motor.h b/main/motor.h index 43dbf4e..1419423 100644 --- a/main/motor.h +++ b/main/motor.h @@ -1,6 +1,7 @@ #pragma once #include +#include #if CONFIG_IDF_TARGET_ESP32H2 #define DSHOT_ESC_RESOLUTION_HZ 32000000 // 32MHz resolution, DSHot protocol needs a relative high resolution @@ -10,11 +11,22 @@ #define ESC_UART_NUM UART_NUM_2 +/** + * Struct for returning telemetry data + */ +typedef struct { + uint16_t temperature; + uint16_t voltage; + uint16_t current; + uint16_t consumption; + uint16_t rpm; +} esc_telemetry_t; + void init_rmt_esc_tx(void); void send_dshot_packet(void); -void parse_telemetry(void); +bool parse_telemetry(esc_telemetry_t * telemetry); void init_motor(void); diff --git a/main/spincoat-plater-firmware.c b/main/spincoat-plater-firmware.c index 5c27cb9..0efe988 100644 --- a/main/spincoat-plater-firmware.c +++ b/main/spincoat-plater-firmware.c @@ -28,23 +28,30 @@ static const char *TAG = "spincoat-plater-firmware"; -static const uint16_t throttle = 200; +static const uint16_t throttle = 100; +static const uint16_t OUTPUT_MIN = 0; +static const uint16_t OUTPUT_MAX = 150; + +static const float KP = 0.12; // Proportional gain +static const float KI = 0.0003; // Integral gain +static const float KD = 0; // Derivative gain + +extern "C" void app_main(void) { + srand((unsigned int)esp_timer_get_time()); + + esc_telemetry_t telemetry; + uint16_t real_rpm = 0; + + AutoPID myPID(&real_rpm, &throttle, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD); -void app_main(void) { init_display(); build_ui(); init_motor(); - //init_rmt_esc_tx(); - //throttle.throttle = 300; update_throttle(throttle); - //xTaskCreate(&v_telemetry_packet_func, "v_telemetry_packet_func", 2048, NULL, 1, NULL); - - //init_telemetry_uart_rx(); - while(1) { send_dshot_packet(); @@ -52,7 +59,14 @@ void app_main(void) { ESP_ERROR_CHECK(uart_get_buffered_data_len(ESC_UART_NUM, (size_t*)&length)); if(length >= 10) { - parse_telemetry(); + if(parse_telemetry(&telemetry)) { + real_rpm = telemetry.rpm / (uint16_t) CONFIG_MOTOR_POLECOUNT; + + myPID.run(); + //ESP_LOGI(TAG, "eRPM returned is: %d\n", telemetry.rpm); + //ESP_LOGI(TAG, "real RPM returned is: %d\n", real_rpm); + update_rpm_readout(real_rpm); + } } } } diff --git a/main/spincoat-plater-firmware.cpp b/main/spincoat-plater-firmware.cpp new file mode 100644 index 0000000..cae0b64 --- /dev/null +++ b/main/spincoat-plater-firmware.cpp @@ -0,0 +1,104 @@ +/* + * SPDX-FileCopyrightText: 2021-2022 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Unlicense OR CC0-1.0 + */ + +#include + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/semphr.h" + +#include "esp_log.h" +#include "driver/uart.h" + +#include "esp_lcd_panel_ops.h" +#include "driver/spi_common.h" +#include "esp_lcd_panel_io.h" +#include "esp_lcd_panel_commands.h" +#include "esp_lcd_ili9341.h" +#include "lvgl.h" + +#include "AutoPID-for-ESP-IDF.h" + +extern "C" { + #include "dshot_esc_encoder.h" + #include "display.h" + #include "ui.h" + #include "motor.h" +} + + +static const char *TAG = "spincoat-plater-firmware"; + +static double throttle = 0; + +static double zero_offset = 100; // Value to offset the throttle by, to skip the + // command values +static double OUTPUT_MIN = 0; +static double OUTPUT_MAX = 1948; + +static double RPM_MAX = 250; +static double BANG_BANG_THRESHOLD = RPM_MAX + 200; + +static double KP = 0.015; +static double KI = 0.8; +static double KD = 0.0; + +static bool motor_running = false; + +extern "C" void app_main(void) { + srand((unsigned int)esp_timer_get_time()); + + esc_telemetry_t telemetry; + double target = 250; + double real_rpm = 0; + + AutoPID myPID(&real_rpm, &target, &throttle, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD); + myPID.setTimeStep(100); // don't set too low or you'll blow the DC-DC converter + myPID.setBangBang(400); + + init_display(); + + build_ui(); + + init_motor(); + update_throttle(throttle); + + while(1) { + send_dshot_packet(); + + uint8_t length = 0; + ESP_ERROR_CHECK(uart_get_buffered_data_len(ESC_UART_NUM, (size_t*)&length)); + + if(length >= 10) { + if(parse_telemetry(&telemetry)) { + + real_rpm = telemetry.rpm / (uint16_t) CONFIG_MOTOR_POLECOUNT; + + myPID.run(); + update_throttle(throttle + zero_offset); + + update_rpm_readout(real_rpm); + ESP_LOGI(TAG, "eRPM: %d, RPM: %.2f, SetPoint: %.2f, Output: %.2f", + telemetry.rpm, + real_rpm, + target, + throttle + zero_offset); // Log the values + + if(myPID.atSetPoint(10)) { + ESP_LOGI(TAG, "At setpoint."); + } + + vTaskDelay(pdMS_TO_TICKS(100)); + + if(real_rpm >= 30) { + motor_running = true; + } else { + motor_running = false; + } + } + } + } +} diff --git a/main/ui.c b/main/ui.c index 89b09e0..894974a 100644 --- a/main/ui.c +++ b/main/ui.c @@ -13,19 +13,28 @@ static const char *TAG = "spincoat-plater-firmware/ui"; static lv_obj_t * rpm_label = NULL; +/** + * Callback for pressing the top button + */ void top_cb(lv_event_t * e) { update_throttle(get_throttle() + BTN_INCREMENT); - lv_label_set_text_fmt(rpm_label, "RPM: %d", get_throttle()); + //lv_label_set_text_fmt(rpm_label, "RPM: %d", get_throttle()); } +/** + * Callback for pressing the bottom button + */ void bottom_cb(lv_event_t * e) { uint16_t throttle = get_throttle(); if(throttle >= BTN_INCREMENT) { update_throttle(get_throttle() - BTN_INCREMENT); - lv_label_set_text_fmt(rpm_label, "RPM: %d", get_throttle()); + //lv_label_set_text_fmt(rpm_label, "RPM: %d", get_throttle()); } } +/** + * Factory function that creates a "numberstack" widget and returns the label for it + */ void build_numberstack(lv_obj_t * parent, const char * label_text, lv_obj_t ** label_value, @@ -90,9 +99,14 @@ void build_numberstack(lv_obj_t * parent, lv_obj_center(lbl_bottom); } +void update_rpm_readout(uint16_t rpm) { + if(rpm_label != NULL) { + lv_label_set_text_fmt(rpm_label, "RPM: %d", rpm); + } +}; + void build_ui(void) { rpm_label = NULL; build_numberstack(lv_screen_active(), "RPM", &rpm_label, top_cb, bottom_cb); - lv_label_set_text_fmt(rpm_label, "RPM: %d", get_throttle()); } diff --git a/main/ui.h b/main/ui.h index 2394d41..45cb18b 100644 --- a/main/ui.h +++ b/main/ui.h @@ -18,4 +18,6 @@ void build_numberstack(lv_obj_t * parent, ns_btn_cb_t top_btn_cb, ns_btn_cb_t bottom_btn_cb); +void update_rpm_readout(uint16_t rpm); + void build_ui(void);