Implement PID and telemetry

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

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 ".")

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

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;
 }
 
 /**

main/motor.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <inttypes.h>
+#include <stdbool.h>
 
 #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);
 

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);
+            }
         }
     }
 }

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 <inttypes.h>
+
+#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;
+                }
+            }
+        }
+    }
+}

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());
 }
 

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);