maddiebaka
105 lines
2.6 KiB
C++
105 lines
2.6 KiB
C++
/*
|
|
* 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 = 100;
|
|
double real_rpm = 0;
|
|
|
|
AutoPID myPID(&real_rpm, &target, &throttle, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD);
|
|
myPID.setTimeStep(100);
|
|
myPID.setBangBang(400); // if you mess with this, you may melt your circuit
|
|
|
|
init_display();
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|