Implement PID and telemetry

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