初始提交卡包 USB 工程
纳入固件源码、屏幕组件、nand_xfer 工具和文档。忽略本机构建产物、sdkconfig，以及含本机路径的依赖锁。 Co-authored-by: Cursor <[email protected]>
This commit is contained in:
zhengxin
@@ -0,0 +1,195 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "MOTOR/motor_control.h"
|
||||
#include "foc.h"
|
||||
#include "esp_check.h"
|
||||
#include "motor_timer.h"
|
||||
#include "step_motor_init.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include "motor_control.h"
|
||||
|
||||
volatile short Motor_Speed = 0;
|
||||
volatile short A_PULS_DUTY = 0;
|
||||
volatile short A_MIUNS_DUTY = 0;
|
||||
volatile short B_PULS_DUTY = 0;
|
||||
volatile short B_MIUNS_DUTY = 0;
|
||||
volatile float Ud = 0;
|
||||
volatile float Uq = 0;
|
||||
volatile float theta = 0;
|
||||
volatile float U_ALPHA = 0;
|
||||
volatile float U_BETA = 0;
|
||||
|
||||
#if MOTOR_WITH_MAGNET_RING
|
||||
static const float speed_adj = 0.1f; //0.22
|
||||
static const float voltage_adj = 0.22f; //0.32
|
||||
#else
|
||||
static const float speed_adj = 0.13f; //0.22
|
||||
static const float voltage_adj = 0.22f; //0.32
|
||||
#endif
|
||||
|
||||
|
||||
static TaskHandle_t s_foc_task_handle = NULL;
|
||||
static const char *TAG_FOC_CTRL = "foc_ctrl";
|
||||
static int64_t s_last_duty_log_us = 0;
|
||||
|
||||
static uint16_t clamp_duty(short duty)
|
||||
{
|
||||
if (duty < 0) {
|
||||
return 0;
|
||||
}
|
||||
if (duty > (short)STEPPER_DUTY_MAX) {
|
||||
return STEPPER_DUTY_MAX;
|
||||
}
|
||||
return (uint16_t)duty;
|
||||
}
|
||||
|
||||
static void foc_update_output_once(void)
|
||||
{
|
||||
float speed;
|
||||
Uq = voltage_adj;
|
||||
Ud = 0;
|
||||
if(motor_dir == MOTOR_CW)
|
||||
{
|
||||
speed = speed_adj;
|
||||
}
|
||||
if(motor_dir == MOTOR_CCW)
|
||||
{
|
||||
speed = -speed_adj;
|
||||
}
|
||||
theta += speed;
|
||||
if (theta >= (2.0f * (float)M_PI)) {
|
||||
theta -= (2.0f * (float)M_PI);
|
||||
}
|
||||
|
||||
inv_park(Ud, Uq, theta, &U_ALPHA, &U_BETA);
|
||||
PWM_DRIVER_OPENLOOP(U_ALPHA, U_BETA);
|
||||
|
||||
stepper_motor_set_duty_raw(clamp_duty(A_PULS_DUTY),
|
||||
clamp_duty(A_MIUNS_DUTY),
|
||||
clamp_duty(B_PULS_DUTY),
|
||||
clamp_duty(B_MIUNS_DUTY));
|
||||
|
||||
// int64_t now_us = esp_timer_get_time();
|
||||
// if (now_us - s_last_duty_log_us >= 1000000) {
|
||||
// s_last_duty_log_us = now_us;
|
||||
// // ESP_LOGI(TAG_FOC_CTRL,
|
||||
// // "duty A+:%d A-:%d B+:%d B-:%d theta:%.3f U_ALPHA:%.3f U_BETA:%.3f",
|
||||
// // A_PULS_DUTY,
|
||||
// // A_MIUNS_DUTY,
|
||||
// // B_PULS_DUTY,
|
||||
// // B_MIUNS_DUTY,
|
||||
// // (double)theta,
|
||||
// // (double)U_ALPHA,
|
||||
// // (double)U_BETA);
|
||||
// // }
|
||||
}
|
||||
|
||||
static void foc_task(void *arg)
|
||||
{
|
||||
(void)arg;
|
||||
while (1) {
|
||||
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
|
||||
foc_update_output_once();
|
||||
}
|
||||
}
|
||||
|
||||
void park(float ia, float ib, float theta, float *id, float *iq)
|
||||
{
|
||||
float cos_theta = cosf(theta);
|
||||
float sin_theta = sinf(theta);
|
||||
*id = ia * cos_theta + ib * sin_theta;
|
||||
*iq = -ia * sin_theta + ib * cos_theta;
|
||||
}
|
||||
|
||||
void inv_park(float vd, float vq, float theta, float *valpha, float *vbeta)
|
||||
{
|
||||
float cos_theta = cosf(theta);
|
||||
float sin_theta = sinf(theta);
|
||||
*valpha = vd * cos_theta - vq * sin_theta;
|
||||
*vbeta = vd * sin_theta + vq * cos_theta;
|
||||
}
|
||||
|
||||
void PWM_DRIVER_OPENLOOP(float U_ALPHA, float U_BETA)
|
||||
{
|
||||
int IA = (int)(U_ALPHA * 1572.0f);
|
||||
int IB = (int)(U_BETA * 1572.0f);
|
||||
|
||||
// A_PULS_DUTY = 0;
|
||||
// A_MIUNS_DUTY = 250;//电流为负数
|
||||
|
||||
// B_PULS_DUTY = 250;
|
||||
// B_MIUNS_DUTY = 0;//电流为负数90.00
|
||||
|
||||
// B_PULS_DUTY = (short)(IA + 510);
|
||||
// A_MIUNS_DUTY = (short)(-IA + 510);
|
||||
|
||||
if (U_ALPHA == 0) {
|
||||
A_PULS_DUTY = 500;
|
||||
A_MIUNS_DUTY = 500;
|
||||
}
|
||||
if (U_BETA == 0) {
|
||||
B_PULS_DUTY = 500;
|
||||
B_MIUNS_DUTY = 500;
|
||||
}
|
||||
if (U_ALPHA < 0) {
|
||||
A_PULS_DUTY = 0;
|
||||
A_MIUNS_DUTY = (short)(-IA + 510);
|
||||
}
|
||||
if (U_ALPHA > 0) {
|
||||
A_MIUNS_DUTY = 0;
|
||||
A_PULS_DUTY = (short)(IA + 510);
|
||||
}
|
||||
if (U_BETA > 0) {
|
||||
B_MIUNS_DUTY = 0;
|
||||
B_PULS_DUTY = (short)(IB + 510);
|
||||
}
|
||||
if (U_BETA < 0) {
|
||||
B_PULS_DUTY = 0;
|
||||
B_MIUNS_DUTY = (short)(-IB + 510);
|
||||
}
|
||||
}
|
||||
|
||||
bool IRAM_ATTR foc_cal_cb(void *user_data)
|
||||
{
|
||||
TaskHandle_t task_handle = (TaskHandle_t)user_data;
|
||||
BaseType_t high_task_wakeup = pdFALSE;
|
||||
|
||||
if (task_handle != NULL) {
|
||||
vTaskNotifyGiveFromISR(task_handle, &high_task_wakeup);
|
||||
}
|
||||
|
||||
return high_task_wakeup == pdTRUE;
|
||||
}
|
||||
|
||||
esp_err_t foc_openloop_start(void)
|
||||
{
|
||||
if (s_foc_task_handle == NULL) {
|
||||
BaseType_t task_ok = xTaskCreatePinnedToCore(foc_task,
|
||||
"foc_task",
|
||||
4096,
|
||||
NULL,
|
||||
configMAX_PRIORITIES - 2,
|
||||
&s_foc_task_handle,
|
||||
0);
|
||||
if (task_ok != pdPASS) {
|
||||
return ESP_ERR_NO_MEM;
|
||||
}
|
||||
}
|
||||
|
||||
// if (!stepper_motor_output_is_enabled()) {
|
||||
// stepper_motor_output_enable();
|
||||
// }
|
||||
|
||||
foc_update_output_once();
|
||||
|
||||
ESP_RETURN_ON_ERROR(foc_timer_init(FOC_TIMER_DEFAULT_HZ, foc_cal_cb, s_foc_task_handle),
|
||||
TAG_FOC_CTRL,
|
||||
"foc timer init failed");
|
||||
ESP_RETURN_ON_ERROR(foc_timer_start(),
|
||||
TAG_FOC_CTRL,
|
||||
"foc timer start failed");
|
||||
return ESP_OK;
|
||||
}
|
||||
Reference in New Issue
Block a user