zhengxin/P4-USB-transfer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

初始提交卡包 USB 工程

Browse Source 
纳入固件源码、屏幕组件、nand_xfer 工具和文档。忽略本机构建产物、sdkconfig，以及含本机路径的依赖锁。

Co-authored-by: Cursor <[email protected]>
This commit is contained in:
zhengxin
2026-10-13 21:54:50 +08:00
co-authored by Cursor
commit 851e347cf5
260 changed files with 269349 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
main/MOTOR/foc.c
+195
View File
@@ -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;
}
main/MOTOR/foc.h
+26
View File
@@ -0,0 +1,26 @@
#ifndef __FOC_H__
#define __FOC_H__
#include <stdbool.h>
#include <stdint.h>
#include "esp_err.h"
#include "esp_check.h"
extern volatile short Motor_Speed;
extern volatile short A_PULS_DUTY;
extern volatile short A_MIUNS_DUTY;
extern volatile short B_PULS_DUTY;
extern volatile short B_MIUNS_DUTY;
extern volatile float Ud;
extern volatile float Uq;
extern volatile float theta;
extern volatile float U_ALPHA;
extern volatile float U_BETA;
void park(float ia, float ib, float theta, float *id, float *iq);
void inv_park(float vd, float vq, float theta, float *valpha, float *vbeta);
void PWM_DRIVER_OPENLOOP(float U_ALPHA, float U_BETA);
bool foc_cal_cb(void *user_data);
esp_err_t foc_openloop_start(void);
#endif
main/MOTOR/motor.c
+307
View File
@@ -0,0 +1,307 @@
// #include <MOTOR/motor.h>
// // #include "freertos/FreeRTOS.h"
// // #include "freertos/task.h"
// static const char *TAG_MOTOR = "MotorControl";
// /** 置位后 motor_run_for_ms 与哨兵停 2 秒会提前退出，用于随时停止哨兵并回正 */
// static volatile bool motor_stop_request = false;
// static bool flag = true;
// void angle_control_task(void *pvParameter);
// float current_angle = 0;
// float target_angle = 0;
// float motor_speed = 90;
// uint32_t motor_rpm = 10;
// uint32_t motor_pwm = 0;
// uint32_t PWM_QUEUE[2] = {MOTOR_60_PWM, MOTOR_90_PWM};
// uint32_t SPEED_QUEUE[2] = {MOTOR_60_SPEED, MOTOR_90_SPEED};
// TaskHandle_t angle_control_handle = NULL;
// // 电机控制结构体
// typedef struct
// {
// gpio_num_t ain1_pin;
// gpio_num_t ain2_pin;
// ledc_channel_t ain1_channel;
// ledc_channel_t ain2_channel;
// } motor_config_t;
// static motor_config_t motor = {
// .ain1_pin = MOTOR_AIN1_GPIO,
// .ain2_pin = MOTOR_AIN2_GPIO,
// .ain1_channel = LEDC_CHANNEL_AIN1,
// .ain2_channel = LEDC_CHANNEL_AIN2};
// // 初始化PWM
// void pwm_init(void)
// {
// // 配置PWM定时器
// ledc_timer_config_t ledc_timer = {
// .speed_mode = LEDC_MODE,
// .duty_resolution = LEDC_DUTY_RES,
// .timer_num = LEDC_TIMER,
// .freq_hz = LEDC_FREQ,
// .clk_cfg = LEDC_AUTO_CLK};
// ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
// // 配置PWM通道
// ledc_channel_config_t ledc_channel_ain1 = {
// .speed_mode = LEDC_MODE,
// .channel = motor.ain1_channel,
// .timer_sel = LEDC_TIMER,
// .intr_type = LEDC_INTR_DISABLE,
// .gpio_num = motor.ain1_pin,
// .duty = 0,
// .hpoint = 0};
// ledc_channel_config_t ledc_channel_ain2 = {
// .speed_mode = LEDC_MODE,
// .channel = motor.ain2_channel,
// .timer_sel = LEDC_TIMER,
// .intr_type = LEDC_INTR_DISABLE,
// .gpio_num = motor.ain2_pin,
// .duty = 0,
// .hpoint = 0};
// ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel_ain1));
// ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel_ain2));
// }
// void gpio_init(void)
// {
// // 配置GPIO为输出模式[2](@ref)
// gpio_config_t io_conf = {
// .pin_bit_mask = (1ULL << motor.ain1_pin | motor.ain2_pin),
// .mode = GPIO_MODE_OUTPUT,
// .pull_up_en = GPIO_PULLUP_DISABLE,
// .pull_down_en = GPIO_PULLDOWN_ENABLE,
// .intr_type = GPIO_INTR_DISABLE};
// gpio_config(&io_conf);
// }
// /**
// * 按固定时长运行电机（可被 motor_stop_request 中断，收到停止时立即停）
// * @param pwm 电机 PWM，正负表示方向
// * @param duration_ms 运行时长（毫秒）
// * @return 实际运行的毫秒数（若被中断则小于 duration_ms，用于按比例回正）
// */
// static uint32_t motor_run_for_ms(int32_t pwm, uint32_t duration_ms)
// {
// if (duration_ms == 0)
// return 0;
// TickType_t xWake = xTaskGetTickCount();
// TickType_t xStart = xWake;
// const TickType_t xPeriod = pdMS_TO_TICKS(1);
// TickType_t xEnd = xWake + pdMS_TO_TICKS(duration_ms);
// while (xTaskGetTickCount() < xEnd)
// {
// if (motor_stop_request)
// {
// motor_set_speed(0);
// return (uint32_t)pdTICKS_TO_MS(xTaskGetTickCount() - xStart);
// }
// motor_set_speed(pwm);
// vTaskDelayUntil(&xWake, xPeriod);
// }
// return duration_ms;
// }
// /** 可被 motor_stop_request 中断的延时（毫秒），用于哨兵停 2 秒等 */
// static void delay_ms_check_stop(uint32_t duration_ms)
// {
// TickType_t xWake = xTaskGetTickCount();
// TickType_t xEnd = xWake + pdMS_TO_TICKS(duration_ms);
// while (xTaskGetTickCount() < xEnd)
// {
// if (motor_stop_request)
// return;
// vTaskDelay(pdMS_TO_TICKS(1));
// }
// }
// void motor_request_stop(void)
// {
// motor_stop_request = true;
// }
// static void motor_clear_stop_request(void)
// {
// motor_stop_request = false;
// }
// // 设置电机速度和方向
// void motor_set_speed(int32_t speed)
// {
// speed = -speed;
// if (speed == 0)
// {
// // 停止电机
// ledc_set_duty(LEDC_MODE, motor.ain1_channel, 0);
// ledc_set_duty(LEDC_MODE, motor.ain2_channel, 0);
// ledc_update_duty(LEDC_MODE, motor.ain1_channel);
// ledc_update_duty(LEDC_MODE, motor.ain2_channel);
// }
// else if (speed > 0)
// {
// // 正转
// ledc_set_duty(LEDC_MODE, motor.ain1_channel, speed * 1023 / MOTOR_MAX_PWM);
// ledc_set_duty(LEDC_MODE, motor.ain2_channel, 0);
// ledc_update_duty(LEDC_MODE, motor.ain1_channel);
// ledc_update_duty(LEDC_MODE, motor.ain2_channel);
// }
// else
// {
// // 反转
// speed = -speed;
// ledc_set_duty(LEDC_MODE, motor.ain1_channel, 0);
// ledc_set_duty(LEDC_MODE, motor.ain2_channel, speed * 1023 / MOTOR_MAX_PWM);
// ledc_update_duty(LEDC_MODE, motor.ain1_channel);
// ledc_update_duty(LEDC_MODE, motor.ain2_channel);
// }
// /* motor_speed = (speed < 0) ? -speed : speed; */
// // ESP_LOGI(TAG_MOTOR, "Motor speed set to: %ld", speed);
// }
// // 设置目标角度
// void motor_set_angle(float angle)
// {
// set_flag(false);
// target_angle = angle;
// int angle_milli = (int)(target_angle * 1000.0f);
// ESP_LOGI(TAG_MOTOR, "Target angle set to: %d.%03d degrees", angle_milli / 1000, abs(angle_milli % 1000));
// }
// // 转速控制函数,单位为度每秒
// void motor_set_motor_speed(uint32_t speed)
// {
// motor_speed = speed;
// ESP_LOGI(TAG_MOTOR, "Motor_speed set to: %lu", speed);
// }
// // 电机初始化
// void motor_init(void)
// {
// gpio_init();
// pwm_init();
// target_angle = 0;
// current_angle = 0;
// motor_set_motor_speed(MOTOR_90_SPEED);
// /* motor_set_speed(MOTOR_30_PWM);
// vTaskDelay(pdMS_TO_TICKS(12000));
// motor_set_speed(0); */
// xTaskCreate(angle_control_task, "angle_control", 4096, NULL, 5, &angle_control_handle);
// }
// // 角度控制任务（需要编码器反馈，这里是开环控制示例）
// void angle_control_task(void *pvParameter)
// {
// TickType_t xLastWakeTime = xTaskGetTickCount();
// const TickType_t xControlPeriod = pdMS_TO_TICKS(1); // pdMS_TO_TICKS(20)
// ESP_LOGI(TAG_MOTOR, "Starting angle control task");
// uint32_t frist_back_time = 450;
// uint32_t pwm_value = 42;//53
// uint32_t back_time_period = 2000;//ms -- 5800
// for (size_t i = 0; i < 1; i++)
// // while(1)
// {
// motor_set_speed(pwm_value );
// vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(800));
// motor_set_speed(0);
// vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(1000));
// motor_set_speed(-pwm_value );
// vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(frist_back_time)); // 150
// ESP_LOGE("angle_task","back_time :%lu",frist_back_time);
// motor_set_speed(0);
// }
// uint32_t bits = 0;
// // int target_position = 0;
// bool motor_run_flag = false;
// MOTOR_STATE motor_state;
// while (1)
// {
// // 必须是有效的任务句柄
// BaseType_t notify_ret = xTaskNotifyWait(0x00000000, ULONG_MAX, &bits, 0); // pdMS_TO_TICKS(100)
// if (notify_ret == pdPASS)
// {
// motor_run_flag = true;
// if (bits == 0x00)
// {
// motor_clear_stop_request();
// motor_set_speed(0);
// }
// else if (bits == 0x01)
// {
// motor_run_for_ms(-pwm_value , 600);//200
// motor_set_speed(0);
// vTaskDelay(pdMS_TO_TICKS(back_time_period));
// motor_run_for_ms(pwm_value , 450);
// motor_set_speed(0);
// }
// else if (bits == 0x02)
// {
// motor_run_for_ms(pwm_value , 600);//110
// motor_set_speed(0);
// vTaskDelay(pdMS_TO_TICKS(back_time_period));
// motor_run_for_ms(-pwm_value,450);//310
// motor_set_speed(0);
// }
// else if (bits == 0x05)
// {
// motor_run_for_ms(-pwm_value, 150);
// motor_set_speed(0);
// vTaskDelay(pdMS_TO_TICKS(2000));
// motor_run_for_ms(pwm_value, 150);
// motor_set_speed(0);
// }
// else if (bits == 0x06)
// {
// motor_run_for_ms(pwm_value , 150);
// motor_set_speed(0);
// vTaskDelay(pdMS_TO_TICKS(2000));
// motor_run_for_ms(-pwm_value , 115);
// motor_set_speed(0);
// }
// else if (bits == 0x07)
// {
// motor_run_for_ms(-pwm_value , 1000);
// motor_set_speed(0);
// vTaskDelay(pdMS_TO_TICKS(2000));
// motor_run_for_ms(pwm_value , 900);
// motor_set_speed(0);
// }
// else if (bits == 0x11)
// {
// motor_run_for_ms(-pwm_value , 10);
// motor_set_speed(0);
// }
// else if (bits == 0x12)
// {
// motor_run_for_ms(pwm_value , 10);
// motor_set_speed(0);
// }
// else
// {
// frist_back_time = bits;
// // ESP_LOGE("angle_task","back_time :%lu",back_time);
// }
// }
// }
// }
// static void set_flag(bool state)
// {
// flag = state;
// }
// bool get_flag(void)
// {
// return flag;
// }
main/MOTOR/motor.h
+104
View File
@@ -0,0 +1,104 @@
// #ifndef _MOTOR_H_
// #define _MOTOR_H_
// #ifdef __cplusplus
// extern "C" {
// #endif
// #include <math.h>
// #include <stdio.h>
// #include "freertos/FreeRTOS.h"
// #include "freertos/task.h"
// #include "freertos/queue.h"
// #include "driver/ledc.h"
// #include "driver/gpio.h"
// #include "esp_log.h"
// #include "esp_timer.h"
// #include <string.h>
// #include "nvs.h"
// #include "nvs_flash.h"
// #include <stdint.h>
// void motor_init(void);
// void motor_set_speed(int32_t speed);
// void motor_set_angle(float angle); //外部接口 -60 主驾 +60副驾
// void motor_set_motor_speed(uint32_t speed); //调整目标转速
// static void set_flag(bool state);
// bool get_flag(void);
// // 用于标识角度数据的键名
// #define STORAGE_NAMESPACE "motor_data"
// #define ANGLE_KEY "current_angle"
// // DRV8837引脚定义
// #define MOTOR_AIN1_GPIO GPIO_NUM_1
// #define MOTOR_AIN2_GPIO GPIO_NUM_2
// // #define MOTOR_AIN1_GPIO GPIO_NUM_9
// // #define MOTOR_AIN2_GPIO GPIO_NUM_46
// //#define MOTOR_BIN1_GPIO GPIO_NUM_14 // 可选，用于第二个电机
// //#define MOTOR_BIN2_GPIO GPIO_NUM_15 // 可选，用于第二个电机
// // PWM配置
// #define LEDC_TIMER LEDC_TIMER_0
// #define LEDC_MODE LEDC_LOW_SPEED_MODE
// #define LEDC_CHANNEL_AIN1 LEDC_CHANNEL_0
// #define LEDC_CHANNEL_AIN2 LEDC_CHANNEL_1
// #define LEDC_DUTY_RES LEDC_TIMER_10_BIT // 10位分辨率(0-1023)
// #define LEDC_FREQ 1000 // 1kHz PWM频率
// // 电机参数
// #define MOTOR_MAX_PWM 100
// #define MOTOR_90_SPEED 90
// #define MOTOR_90_PWM 42
// #define MOTOR_60_SPEED 60
// #define MOTOR_60_PWM 53 //30 50
// // #define MOTOR_30_SPEED 30
// // #define MOTOR_50_SPEED 50
// // #define MOTOR_50_PWM 44
// // #define MOTOR_45_SPEED 45
// // #define MOTOR_45_PWM 34
// // #define MOTOR_40_SPEED 40
// // #define MOTOR_40_PWM 28
// // #define MOTOR_36_SPEED 36
// // #define MOTOR_36_PWM 25
// // #define MOTOR_30_SPEED 30
// // #define MOTOR_30_PWM 21
// #define ANGLE_LIMIT 3
// extern TaskHandle_t angle_control_handle;
// typedef enum
// {
// motor_stop = 0 ,
// motor_turn_to_right,
// motor_turn_to_left,
// }MOTOR_STATE;
// // typedef enum
// // {
// // position_zero = 0,
// // position_right_15,//右边偏移15度
// // position_right_30,//右边偏移30度
// // position_right_60,//右边偏移60度
// // position_left_15,//左边偏移15度
// // position_left_30,//左边偏移30度
// // position_left_60,//左边偏移60度
// // position_left_180,//左边偏移180度
// // position_left_120,//左边偏移180度
// // position_left_240,//左边偏移180度
// // }MOTOR_POSITION;
// void motor_turn_aroud(uint8_t cmd);
// /** 请求电机立即停止（用于哨兵模式等可中断场景） */
// void motor_request_stop(void);
// #ifdef __cplusplus
// }
// #endif
// #endif /* TEST_H_ */
main/MOTOR/motor_control.c
+413
View File
@@ -0,0 +1,413 @@
#include "motor_control.h"
#include "freertos/projdefs.h"
#include "freertos/queue.h"
#include "step_motor_init.h"
#include "motor_io.h"
#include "foc.h"
#include <limits.h>
#include <stdint.h>
#include <stdbool.h>
#include "esp_check.h"
#include "esp_log.h"
#include "BLE/ble.h"
static const char *TAG_MOTOR_CTL = "motor_control";
TaskHandle_t motor_control_handle = NULL;
#define MOTOR_CMD_QUEUE_LEN 16
static QueueHandle_t s_motor_cmd_q = NULL;
typedef enum
{
MOTOR_LAT_UNKNOWN = 0,
MOTOR_LAT_LEFT,
MOTOR_LAT_RIGHT,
MOTOR_LAT_CENTER,
} motor_lateral_t;
/** 仅电机任务写、其它任务读；用于左右切换时是否插 0x0A */
static volatile motor_lateral_t s_motor_last_lateral = MOTOR_LAT_CENTER;
/** 最近一次成功入队的 play_sticker 方向字节；同字节+同侧向则不再重复入队（如已是 0x01 又来 0x01） */
static uint8_t s_last_play_sticker_direction = 0xFF;
// MOTOR_POSITION motor_position ;//记录步进电机的位置
MOTOR_POSITION motor_current_position = MOTOR_POSITION_ZERO;
MOTOR_POSITION motor_target_position = MOTOR_POSITION_ZERO;
MOTOR_DIR motor_dir = MOTOR_CW;
SemaphoreHandle_t motor_action_down_sem = NULL;
static volatile bool s_sentinel_motor_running = false;
static volatile bool s_sentinel_stop_requested = false;
#if MOTOR_WITH_MAGNET_RING
const MOTOR_RUN_TIME motor_run_time =
{
.run_to_left_15 = 250,
.run_to_left_30 = 500,
.run_to_left_90 = 1000,
.run_to_left_180 = 1800,
.run_to_right_15 = 250,
.run_to_right_30 = 500,
.run_to_right_180 = 1800,
};
uint16_t OB_BACK_TIME = 130;
#elif OLD_MOTOR
const MOTOR_RUN_TIME motor_run_time =
{
.run_to_left_15 = 250,
.run_to_left_30 = 500,
.run_to_left_90 = 750,
.run_to_left_180 = 1300,
.run_to_right_15 = 250,
.run_to_right_30 = 500,
.run_to_right_180 = 1300,
};
uint16_t OB_BACK_TIME = 55;
#else
const MOTOR_RUN_TIME motor_run_time =
{
.run_to_left_15 = 250,
.run_to_left_30 = 450,
.run_to_left_90 = 1250,
.run_to_left_180 = 1950,
.run_to_right_15 = 250,
.run_to_right_30 = 450,
.run_to_right_180 = 1950,
};
uint16_t OB_BACK_TIME = 55;
#endif
static void motor_run_steps(uint32_t command);
/** 与上次已在电机任务里执行完的 notify 值相同则忽略，避免 BLE 连发 / completed_cb 重复补发导致同向连转 */
static uint32_t s_last_executed_motor_notify = UINT32_MAX;
static bool motor_cmd_is_left(uint32_t cmd)
{
return cmd == 0x01u || cmd == 0x05u || cmd == 0x07u;
}
static bool motor_cmd_is_right(uint32_t cmd)
{
return cmd == 0x02u || cmd == 0x06u || cmd == 0x08u;
}
static void motor_update_lateral_after_cmd(uint32_t cmd)
{
if (motor_cmd_is_left(cmd))
{
s_motor_last_lateral = MOTOR_LAT_LEFT;
}
else if (motor_cmd_is_right(cmd))
{
s_motor_last_lateral = MOTOR_LAT_RIGHT;
}
else if (cmd == 0x00u || cmd == 0x0Au || cmd == 0x0Bu)
{
s_motor_last_lateral = MOTOR_LAT_CENTER;
}
}
bool motor_post_cmd(uint32_t cmd)
{
if (s_motor_cmd_q == NULL)
{
ESP_LOGW(TAG_MOTOR_CTL, "motor_post_cmd: queue not ready cmd=0x%02lX", (unsigned long)cmd);
return false;
}
if (xQueueSend(s_motor_cmd_q, &cmd, 0) != pdPASS)
{
uint32_t dropped = 0;
(void)xQueueReceive(s_motor_cmd_q, &dropped, 0);
if (xQueueSend(s_motor_cmd_q, &cmd, 0) != pdPASS)
{
ESP_LOGW(TAG_MOTOR_CTL, "motor_post_cmd: enqueue failed cmd=0x%02lX", (unsigned long)cmd);
return false;
}
ESP_LOGW(TAG_MOTOR_CTL, "motor_post_cmd: queue full dropped=0x%02lX enq=0x%02lX",
(unsigned long)dropped, (unsigned long)cmd);
}
return true;
}
bool motor_post_play_sticker_direction(uint8_t direction)
{
const uint32_t cmd = (uint32_t)direction;
/* 0x00：协议表示「本条不追加转向」；若头仍在左/右侧，必须用 0x0A 物理回中，
* 不能发 0x00（openloop stop 与回中语义不同），否则左播途中接 0x10/0x00 会丢回中。 */
if (cmd == 0x00u)
{
const motor_lateral_t last = s_motor_last_lateral;
if (last == MOTOR_LAT_LEFT || last == MOTOR_LAT_RIGHT)
{
if (!motor_post_cmd(0x0Au))
{
return false;
}
s_last_play_sticker_direction = 0xFF;
return true;
}
return true;
}
if (cmd == 0x0Au || cmd == 0x0Bu)
{
if (!motor_post_cmd(cmd))
{
return false;
}
s_last_play_sticker_direction = 0xFF;
return true;
}
if (!motor_cmd_is_left(cmd) && !motor_cmd_is_right(cmd))
{
if (!motor_post_cmd(cmd))
{
return false;
}
s_last_play_sticker_direction = (uint8_t)cmd;
return true;
}
const motor_lateral_t last = s_motor_last_lateral;
/* 侧向未变且与上次下发的方向字节相同：不重复入队（如当前已是 0x01 姿态又来 0x01） */
if ((motor_cmd_is_left(cmd) && last == MOTOR_LAT_LEFT) || (motor_cmd_is_right(cmd) && last == MOTOR_LAT_RIGHT))
{
if ((uint8_t)cmd == s_last_play_sticker_direction)
{
ESP_LOGI(TAG_MOTOR_CTL, "skip redundant play_sticker direction 0x%02lX (same as last)", cmd);
return true;
}
}
const bool need_home =
(last == MOTOR_LAT_LEFT && motor_cmd_is_right(cmd)) || (last == MOTOR_LAT_RIGHT && motor_cmd_is_left(cmd));
if (need_home && !motor_post_cmd(0x0Au))
{
return false;
}
if (!motor_post_cmd(cmd))
{
return false;
}
s_last_play_sticker_direction = (uint8_t)cmd;
return true;
}
void motor_post_cmd_from_isr(uint32_t cmd, BaseType_t *pxHigherPriorityTaskWoken)
{
if (s_motor_cmd_q == NULL)
{
return;
}
if (xQueueSendFromISR(s_motor_cmd_q, &cmd, pxHigherPriorityTaskWoken) != pdPASS)
{
uint32_t dropped = 0;
(void)xQueueReceiveFromISR(s_motor_cmd_q, &dropped, pxHigherPriorityTaskWoken);
(void)xQueueSendFromISR(s_motor_cmd_q, &cmd, pxHigherPriorityTaskWoken);
}
}
void motor_control_init(void)
{
// PWM 初始化 任务创建
//motor_io_init();
stepper_motor_init();
s_motor_cmd_q = xQueueCreate(MOTOR_CMD_QUEUE_LEN, sizeof(uint32_t));
if (s_motor_cmd_q == NULL)
{
ESP_LOGE(TAG_MOTOR_CTL, "motor cmd queue create failed");
}
// 固定到 CPU1，避免与 CPU0 上的 LVGL task 竞争；优先级降为 5
xTaskCreatePinnedToCore(motor_control_task, "motor_control_task", 4096, NULL, 8, &motor_control_handle, 0);
}
void motor_control_task(void *pvParameter)
{
uint32_t bits = 0;
motor_action_down_sem = xSemaphoreCreateBinary();
xSemaphoreGive(motor_action_down_sem);
(void)pvParameter;
s_motor_last_lateral = MOTOR_LAT_CENTER;
motor_current_position =MOTOR_POSITION_ZERO;
motor_run_dir(MOTOR_CW);
stepper_motor_output_enable();
vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_left_180));
motor_current_position = MOTOR_POSITION_LEFT_180;
stepper_motor_output_disable();
ESP_LOGW("TAG_MOTOR_CTL", "motor_control_task: TURNED LEFT DONE");
s_motor_last_lateral = MOTOR_LAT_LEFT;
vTaskDelay(pdMS_TO_TICKS(2000));
motor_run_dir(MOTOR_CCW);
stepper_motor_output_enable();
vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_right_180));
stepper_motor_output_disable();
s_motor_last_lateral = MOTOR_LAT_CENTER;
motor_current_position = MOTOR_POSITION_ZERO;
ESP_LOGW("TAG_MOTOR_CTL", "motor_control_task: TURNED RIGHT DONE");
while (1)
{
if (s_motor_cmd_q == NULL)
{
vTaskDelay(pdMS_TO_TICKS(20));
continue;
}
if (xQueueReceive(s_motor_cmd_q, &bits, pdMS_TO_TICKS(20)) != pdPASS)
{
continue;
}
if (bits == s_last_executed_motor_notify)
{
ESP_LOGD(TAG_MOTOR_CTL, "skip duplicate motor cmd 0x%02lX", (unsigned long)bits);
continue;
}
motor_run_steps(bits);
s_last_executed_motor_notify = bits;
motor_update_lateral_after_cmd(bits);
}
}
static void motor_run_steps(uint32_t command)
{
uint32_t back_time_ms = 0;
if (command == 0x00)
{
stepper_motor_stop();
motor_current_position = MOTOR_POSITION_ZERO;
}
else if (command == 0x01)
{
motor_run_dir(MOTOR_CW);
stepper_motor_output_enable();
vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_left_30));
stepper_motor_output_disable();
motor_current_position = MOTOR_POSITION_LEFT_30;
}
else if (command == 0x02)
{
motor_run_dir(MOTOR_CCW);
stepper_motor_output_enable();
vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_right_30));
stepper_motor_output_disable();
motor_current_position = MOTOR_POSITION_RIGHT_30;
}
else if (command == 0x05)
{
// stepper_motor_run_steps(STEPPER_DIR_FORWARD, ANGLE_STEP_15 * 4);
motor_run_dir(MOTOR_CW);
stepper_motor_output_enable();
vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_left_15));
stepper_motor_output_disable();
motor_current_position = MOTOR_POSITION_LEFT_15;
}
else if (command == 0x06)
{
// stepper_motor_run_steps(STEPPER_DIR_REVERSE, ANGLE_STEP_15 * 4);
motor_run_dir(MOTOR_CCW);
stepper_motor_output_enable();
vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_right_15));
stepper_motor_output_disable();
motor_current_position = MOTOR_POSITION_RIGHT_15;
}
else if (command == 0x07)
{
// stepper_motor_run_steps(STEPPER_DIR_FORWARD, ANGLE_STEP_180 * 4);
motor_run_dir(MOTOR_CW);
stepper_motor_output_enable();
vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_left_180));
stepper_motor_output_disable();
motor_current_position = MOTOR_POSITION_LEFT_180;
if(sentinel_mode_is_active == true)
{
vTaskDelay(pdMS_TO_TICKS(2000));
xSemaphoreGive(motor_action_down_sem);
}
}
else if (command == 0x08)
{
// stepper_motor_run_steps(STEPPER_DIR_FORWARD, ANGLE_STEP_180 * 4);
motor_run_dir(MOTOR_CCW);
stepper_motor_output_enable();
vTaskDelay(pdMS_TO_TICKS(motor_run_time.run_to_right_180));
stepper_motor_output_disable();
motor_current_position = MOTOR_POSITION_RIGHT_180;
if(sentinel_mode_is_active == true)
{
vTaskDelay(pdMS_TO_TICKS(2000));
xSemaphoreGive(motor_action_down_sem);
}
}
else if (command == 0x0A)
{
// stepper_motor_run_steps(STEPPER_DIR_FORWARD, ANGLE_STEP_30 * 4);
if(motor_current_position == MOTOR_POSITION_ZERO)back_time_ms = 0;
else if(motor_current_position == MOTOR_POSITION_LEFT_15 || motor_current_position == MOTOR_POSITION_LEFT_30 || motor_current_position == MOTOR_POSITION_LEFT_180)
{
motor_run_dir(MOTOR_CCW);
if(motor_current_position == MOTOR_POSITION_LEFT_15)back_time_ms = motor_run_time.run_to_right_15;
else if(motor_current_position == MOTOR_POSITION_LEFT_30)back_time_ms = motor_run_time.run_to_right_30;
else if(motor_current_position == MOTOR_POSITION_LEFT_180)back_time_ms = motor_run_time.run_to_right_180;
}
else if(motor_current_position == MOTOR_POSITION_RIGHT_15 || motor_current_position == MOTOR_POSITION_RIGHT_30|| motor_current_position == MOTOR_POSITION_RIGHT_180)
{
motor_run_dir(MOTOR_CW);
if(motor_current_position == MOTOR_POSITION_RIGHT_15)back_time_ms = motor_run_time.run_to_left_15;
else if(motor_current_position == MOTOR_POSITION_RIGHT_30)back_time_ms = motor_run_time.run_to_left_30;
else if(motor_current_position == MOTOR_POSITION_RIGHT_180)back_time_ms = motor_run_time.run_to_left_180;
}
if(back_time_ms != 0)
{
ESP_LOGW(TAG_MOTOR_CTL, "0X0A back_time_ms START,motor_current_position=%d,back_time_ms=%d",motor_current_position,back_time_ms);
stepper_motor_output_enable();
vTaskDelay(pdMS_TO_TICKS(back_time_ms));
}
stepper_motor_output_disable();
ESP_LOGW(TAG_MOTOR_CTL, "0X0A back_time_ms END");
if(sentinel_mode_is_active == true)
{
vTaskDelay(pdMS_TO_TICKS(2000));
xSemaphoreGive(motor_action_down_sem);
}
motor_current_position = MOTOR_POSITION_ZERO;
}
else if (command == 0x0B)
{
ESP_LOGE("MOTOR_CONTROL", "MOTOR_CONTROL_STOP");
vTaskDelay(pdMS_TO_TICKS(100));
// motor_run_dir(MOTOR_CW);
// stepper_motor_output_enable();
// vTaskDelay(pdMS_TO_TICKS(35));
stepper_motor_output_disable();
motor_current_position = MOTOR_POSITION_ZERO;
}
}
void motor_run_dir(MOTOR_DIR dir)
{
if(dir == MOTOR_CW)
{
motor_dir = MOTOR_CW;
}
if(dir == MOTOR_CCW)
{
motor_dir = MOTOR_CCW;
}
}
main/MOTOR/motor_control.h
+68
View File
@@ -0,0 +1,68 @@
#ifndef __MOTOR_CONTROL_H__
#define __MOTOR_CONTROL_H__
#ifdef __cplisplus
extern "C" {
#endif
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#define MOTOR_WITH_MAGNET_RING 0 //带磁环的时候
#define OLD_MOTOR 0 //旧电机参数
typedef enum
{
MOTOR_POSITION_ZERO = 0,
MOTOR_POSITION_LEFT_15 ,
MOTOR_POSITION_RIGHT_15 ,
MOTOR_POSITION_LEFT_30 ,
MOTOR_POSITION_RIGHT_30 ,
MOTOR_POSITION_LEFT_90 ,
MOTOR_POSITION_RIGHT_90 ,
MOTOR_POSITION_LEFT_180 ,
MOTOR_POSITION_RIGHT_180,
MOTOR_POSITION_LEFT_2 ,//微步
}MOTOR_POSITION;
typedef enum
{
MOTOR_CW,
MOTOR_CCW,
}MOTOR_DIR;
typedef struct
{
uint32_t run_to_left_15;
uint32_t run_to_left_30;
uint32_t run_to_left_90;
uint32_t run_to_left_180;
uint32_t run_to_right_15;
uint32_t run_to_right_30;
uint32_t run_to_right_180;
}MOTOR_RUN_TIME;
extern TaskHandle_t motor_control_handle;
extern MOTOR_DIR motor_dir;
extern MOTOR_POSITION motor_current_position ;
extern MOTOR_POSITION motor_target_position ;
extern SemaphoreHandle_t motor_action_down_sem;
void motor_control_init(void);
void motor_control_task(void *pvParameter);
void motor_run_dir(MOTOR_DIR dir);
/** 串行入队执行；满则丢最旧再入队，避免 Overwrite 丢命令 */
bool motor_post_cmd(uint32_t cmd);
/** play_sticker 方向：左右切换时先排队 0x0A；0x00 在头仍左/右时排队 0x0A；与上次同字节且侧向未变则跳过 */
bool motor_post_play_sticker_direction(uint8_t direction);
void motor_post_cmd_from_isr(uint32_t cmd, BaseType_t *pxHigherPriorityTaskWoken);
#ifdef __cplusplus
}
#endif
#endif
main/MOTOR/motor_io.c
+194
View File
@@ -0,0 +1,194 @@
#include "motor_io.h"
#include <stdbool.h>
#include "MOTOR/motor_control.h"
#include "step_motor_init.h"
#include "driver/gpio.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
static const char *TAG = "motor_io";
static TaskHandle_t s_motor_io_task_handle = NULL;
static volatile bool s_stop_transition_enabled = false;
static volatile int s_stop_from_14 = 0;
static volatile int s_stop_from_47 = 0;
static volatile int s_stop_to_14 = 0;
static volatile int s_stop_to_47 = 0;
volatile int s_io14_last_level ;
volatile int s_io47_last_level ;
volatile int s_io14_now_level ;
volatile int s_io47_now_level ;
static void motor_io_log_levels(const char *reason, int level_14, int level_47)
{
ESP_LOGI(TAG, "%s GPIO14=%d GPIO47=%d", reason, level_14, level_47);
}
static void IRAM_ATTR motor_io_isr_handler(void *arg)
{
(void)arg;
BaseType_t hpw = pdFALSE;
s_io14_now_level = motor_io_get_level_14();
s_io47_now_level = motor_io_get_level_47();
if (motor_control_handle != NULL)
{
if(s_io14_last_level == 0 && s_io47_last_level == 1)
{
if(s_io14_now_level == 1 && s_io47_now_level == 1)
{
//发送停止信号
if(motor_current_position == MOTOR_POSITION_LEFT_2)
{
stepper_motor_output_disable();
motor_post_cmd_from_isr((uint32_t)0x0Bu, &hpw);
}
}
}
if(s_io14_last_level == 0 && s_io47_last_level == 1)
{
if(s_io14_now_level == 0 && s_io47_now_level == 0)
{
//发送停止信号
// xTaskNotifyFromISR(motor_control_handle,(uint32_t)0x0B,eSetValueWithOverwrite,NULL);
}
}
}
s_io14_last_level = s_io14_now_level;
s_io47_last_level = s_io47_now_level;
if (s_motor_io_task_handle != NULL)
{
vTaskNotifyGiveFromISR(s_motor_io_task_handle, &hpw);
}
if (hpw == pdTRUE)
{
portYIELD_FROM_ISR();
}
}
int motor_io_get_level_14(void)
{
return gpio_get_level(MOTOR_IO_PIN_14);
}
int motor_io_get_level_47(void)
{
return gpio_get_level(MOTOR_IO_PIN_47);
}
void motor_io_get_levels(int *level_14, int *level_47)
{
if (level_14 != NULL)
{
*level_14 = motor_io_get_level_14();
}
if (level_47 != NULL)
{
*level_47 = motor_io_get_level_47();
}
}
void motor_io_set_stop_transition(int from_14, int from_47, int to_14, int to_47)
{
s_stop_from_14 = from_14;
s_stop_from_47 = from_47;
s_stop_to_14 = to_14;
s_stop_to_47 = to_47;
s_stop_transition_enabled = true;
}
void motor_io_clear_stop_transition(void)
{
s_stop_transition_enabled = false;
}
static void motor_io_monitor_task(void *pvParameter)
{
(void)pvParameter;
int last_level_14 = motor_io_get_level_14();
int last_level_47 = motor_io_get_level_47();
motor_io_log_levels("初始电平:", last_level_14, last_level_47);
while (1)
{
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
int level_14 = motor_io_get_level_14();
int level_47 = motor_io_get_level_47();
if ((level_14 != last_level_14) || (level_47 != last_level_47))
{
motor_io_log_levels("检测到电平变化:", level_14, level_47);
if (s_stop_transition_enabled &&
(last_level_14 == s_stop_from_14) &&
(last_level_47 == s_stop_from_47) &&
(level_14 == s_stop_to_14) &&
(level_47 == s_stop_to_47))
{
ESP_LOGI(TAG, "命中停机跳变: GPIO14=%d GPIO47=%d -> GPIO14=%d GPIO47=%d",
last_level_14,
last_level_47,
level_14,
level_47);
stepper_motor_stop();
}
last_level_14 = level_14;
last_level_47 = level_47;
}
}
}
void motor_io_init(void)
{
const gpio_config_t io_conf = {
.pin_bit_mask = (1ULL << MOTOR_IO_PIN_14) | (1ULL << MOTOR_IO_PIN_47),
.mode = GPIO_MODE_INPUT,
.pull_up_en = GPIO_PULLUP_DISABLE,
.pull_down_en = GPIO_PULLDOWN_DISABLE,
.intr_type = GPIO_INTR_ANYEDGE,
};
ESP_ERROR_CHECK(gpio_config(&io_conf));
if (s_motor_io_task_handle == NULL)
{
xTaskCreatePinnedToCore(motor_io_monitor_task,
"motor_io_task",
MOTOR_IO_TASK_STACK_SIZE,
NULL,
MOTOR_IO_TASK_PRIORITY,
&s_motor_io_task_handle,
1);
}
esp_err_t ret = gpio_install_isr_service(0);
if ((ret != ESP_OK) && (ret != ESP_ERR_INVALID_STATE))
{
ESP_ERROR_CHECK(ret);
}
ESP_ERROR_CHECK(gpio_isr_handler_add(MOTOR_IO_PIN_14, motor_io_isr_handler, NULL));
ESP_ERROR_CHECK(gpio_isr_handler_add(MOTOR_IO_PIN_47, motor_io_isr_handler, NULL));
s_io14_last_level = motor_io_get_level_14();
s_io47_last_level = motor_io_get_level_47();
}
main/MOTOR/motor_io.h
+26
View File
@@ -0,0 +1,26 @@
#ifndef __MOTOR_IO_H__
#define __MOTOR_IO_H__
#include "driver/gpio.h"
#ifdef __cplusplus
extern "C" {
#endif
#define MOTOR_IO_PIN_14 GPIO_NUM_14
#define MOTOR_IO_PIN_47 GPIO_NUM_47
#define MOTOR_IO_TASK_STACK_SIZE 4096
#define MOTOR_IO_TASK_PRIORITY 8 // 优先级 ---
void motor_io_init(void);
int motor_io_get_level_14(void);
int motor_io_get_level_47(void);
void motor_io_get_levels(int *level_14, int *level_47);
void motor_io_set_stop_transition(int from_14, int from_47, int to_14, int to_47);
void motor_io_clear_stop_transition(void);
#ifdef __cplusplus
}
#endif
#endif
main/MOTOR/motor_timer.c
+153
View File
@@ -0,0 +1,153 @@
#include "motor_timer.h"
#include <math.h>
#include "driver/gptimer.h"
#include "esp_attr.h"
#include "esp_check.h"
#include "esp_log.h"
static const char *TAG_FOC = "foc_timer"; //打印标签
static gptimer_handle_t s_foc_timer = NULL; //定时器timer句柄
static foc_timer_isr_cb_t s_foc_timer_user_cb = NULL; //用户回调函数--FOC计算
static void *s_foc_timer_user_data = NULL; //用户回调传入一个私有参数
static uint32_t s_foc_timer_freq_hz = FOC_TIMER_DEFAULT_HZ; //定时器频率 3K hz
static bool s_foc_timer_started = false; //记录定时器启动标记
static bool IRAM_ATTR foc_timer_on_alarm_cb(gptimer_handle_t timer,
const gptimer_alarm_event_data_t *edata,
void *user_data)
{
(void)timer;
(void)edata;
(void)user_data;
if (s_foc_timer_user_cb) {
return s_foc_timer_user_cb(s_foc_timer_user_data);
}
return false;
}
static esp_err_t foc_timer_apply_alarm_locked(uint32_t freq_hz)
{
if (freq_hz == 0 || s_foc_timer == NULL) {
return ESP_ERR_INVALID_ARG;
}
uint64_t alarm_count = 1000000ULL / freq_hz;
if (alarm_count == 0) {
alarm_count = 1;
}
gptimer_alarm_config_t alarm_config = {
.reload_count = 0,
.alarm_count = alarm_count,
.flags.auto_reload_on_alarm = true,
};
return gptimer_set_alarm_action(s_foc_timer, &alarm_config);
}
esp_err_t foc_timer_init(uint32_t freq_hz, foc_timer_isr_cb_t cb, void *user_data)
{
if (freq_hz == 0) {
freq_hz = FOC_TIMER_DEFAULT_HZ;
}
if (s_foc_timer != NULL) {
s_foc_timer_user_cb = cb;
s_foc_timer_user_data = user_data;
s_foc_timer_freq_hz = freq_hz;
return foc_timer_apply_alarm_locked(freq_hz);
}
gptimer_config_t timer_config = {
.clk_src = GPTIMER_CLK_SRC_DEFAULT,
.direction = GPTIMER_COUNT_UP,
.resolution_hz = 1000000,
};
ESP_RETURN_ON_ERROR(gptimer_new_timer(&timer_config, &s_foc_timer), TAG_FOC, "create gptimer failed");
gptimer_event_callbacks_t cbs = {
.on_alarm = foc_timer_on_alarm_cb,
};
ESP_RETURN_ON_ERROR(gptimer_register_event_callbacks(s_foc_timer, &cbs, NULL), TAG_FOC, "register gptimer callback failed");
s_foc_timer_user_cb = cb;
s_foc_timer_user_data = user_data;
s_foc_timer_freq_hz = freq_hz;
s_foc_timer_started = false;
ESP_RETURN_ON_ERROR(foc_timer_apply_alarm_locked(freq_hz), TAG_FOC, "set gptimer alarm failed");
esp_err_t err = gptimer_enable(s_foc_timer);
if ((err != ESP_OK) && (err != ESP_ERR_INVALID_STATE)) {
return err;
}
ESP_LOGI(TAG_FOC, "FOC timer init done, freq=%lu Hz", (unsigned long)s_foc_timer_freq_hz);
return ESP_OK;
}
esp_err_t foc_timer_set_frequency(uint32_t freq_hz)
{
if (freq_hz == 0 || s_foc_timer == NULL) {
return ESP_ERR_INVALID_ARG;
}
bool restart = s_foc_timer_started;
if (restart) {
ESP_RETURN_ON_ERROR(gptimer_stop(s_foc_timer), TAG_FOC, "stop gptimer before retune failed");
s_foc_timer_started = false;
}
ESP_RETURN_ON_ERROR(foc_timer_apply_alarm_locked(freq_hz), TAG_FOC, "retune gptimer alarm failed");
s_foc_timer_freq_hz = freq_hz;
if (restart) {
ESP_RETURN_ON_ERROR(gptimer_start(s_foc_timer), TAG_FOC, "restart gptimer failed");
s_foc_timer_started = true;
}
ESP_LOGI(TAG_FOC, "FOC timer frequency set to %lu Hz", (unsigned long)s_foc_timer_freq_hz);
return ESP_OK;
}
esp_err_t foc_timer_start(void)
{
if (s_foc_timer == NULL) {
return ESP_ERR_INVALID_STATE;
}
if (s_foc_timer_started) {
return ESP_OK;
}
ESP_RETURN_ON_ERROR(gptimer_set_raw_count(s_foc_timer, 0), TAG_FOC, "reset gptimer count failed");
esp_err_t err = gptimer_start(s_foc_timer);
if ((err != ESP_OK) && (err != ESP_ERR_INVALID_STATE)) {
return err;
}
s_foc_timer_started = true;
ESP_LOGI(TAG_FOC, "FOC timer started");
return ESP_OK;
}
esp_err_t foc_timer_stop(void)
{
if (s_foc_timer == NULL) {
return ESP_ERR_INVALID_STATE;
}
if (!s_foc_timer_started) {
return ESP_OK;
}
ESP_RETURN_ON_ERROR(gptimer_stop(s_foc_timer), TAG_FOC, "stop gptimer failed");
s_foc_timer_started = false;
ESP_LOGI(TAG_FOC, "FOC timer stopped");
return ESP_OK;
}
uint32_t foc_timer_get_frequency(void)
{
return s_foc_timer_freq_hz;
}
main/MOTOR/motor_timer.h
+19
View File
@@ -0,0 +1,19 @@
#ifndef __MOTOR_TIMER_H__
#define __MOTOR_TIMER_H__
#include <stdbool.h>
#include <stdint.h>
#include "esp_err.h"
#include "config.h"
typedef bool (*foc_timer_isr_cb_t)(void *user_data);
#define FOC_TIMER_DEFAULT_HZ 8000U
esp_err_t foc_timer_init(uint32_t freq_hz, foc_timer_isr_cb_t cb, void *user_data);
esp_err_t foc_timer_set_frequency(uint32_t freq_hz);
esp_err_t foc_timer_start(void);
esp_err_t foc_timer_stop(void);
uint32_t foc_timer_get_frequency(void);
#endif
main/MOTOR/step_motor_init.c
+276
View File
@@ -0,0 +1,276 @@
#include "step_motor_init.h"
#include <stdbool.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_check.h"
#include "esp_timer.h"
static mcpwm_timer_handle_t s_timer;
static mcpwm_oper_handle_t s_oper[2];
static mcpwm_cmpr_handle_t s_cmpr[4]; /* [0]=A+ [1]=A- [2]=B+ [3]=B- */
static mcpwm_gen_handle_t s_gen[4];
static esp_timer_handle_t s_openloop_timer;
static uint32_t s_pwm_period_ticks = STEPPER_PWM_RESOL_HZ / STEPPER_PWM_FREQ_HZ;
static bool s_running = false;
static bool s_output_enabled = false;
static bool s_openloop_running = false;
static uint8_t s_openloop_step_idx = 0;
static stepper_motor_direction_t s_direction = STEPPER_DIR_FORWARD;
static const char *TAG = "stepper_motor";
static const uint16_t s_openloop_phase_table[4][4] = {
{ STEPPER_STARTUP_DUTY_RAW, 0, STEPPER_STARTUP_DUTY_RAW, 0 },
{ 0, STEPPER_STARTUP_DUTY_RAW, STEPPER_STARTUP_DUTY_RAW, 0 },
{ 0, STEPPER_STARTUP_DUTY_RAW, 0, STEPPER_STARTUP_DUTY_RAW },
{ STEPPER_STARTUP_DUTY_RAW, 0, 0, STEPPER_STARTUP_DUTY_RAW },
};
static uint32_t duty_raw_to_ticks(uint16_t duty_raw)
{
if (duty_raw > STEPPER_DUTY_MAX) {
duty_raw = STEPPER_DUTY_MAX;
}
return ((uint64_t)duty_raw * s_pwm_period_ticks) / STEPPER_DUTY_MAX;
// return ((uint64_t)duty_raw * s_pwm_period_ticks);
// return ((uint64_t)duty_raw );
}
static void apply_openloop_phase(uint8_t step_idx)
{
const uint16_t *phase = s_openloop_phase_table[step_idx & 0x03];
stepper_motor_set_duty_raw(phase[0], phase[1], phase[2], phase[3]);
}
static void stepper_motor_step_once(stepper_motor_direction_t direction)
{
if (direction == STEPPER_DIR_FORWARD) {
s_openloop_step_idx = (uint8_t)((s_openloop_step_idx + 1) & 0x03);
} else {
s_openloop_step_idx = (uint8_t)((s_openloop_step_idx + 3) & 0x03);
}
apply_openloop_phase(s_openloop_step_idx);
}
static void openloop_timer_cb(void *arg)
{
(void)arg;
stepper_motor_step_once(s_direction);
}
void stepper_motor_output_enable(void)
{
ESP_ERROR_CHECK(mcpwm_generator_set_force_level(s_gen[0], -1, true));
ESP_ERROR_CHECK(mcpwm_generator_set_force_level(s_gen[1], -1, true));
ESP_ERROR_CHECK(mcpwm_generator_set_force_level(s_gen[2], -1, true));
ESP_ERROR_CHECK(mcpwm_generator_set_force_level(s_gen[3], -1, true));
s_output_enabled = true;
}
void stepper_motor_output_disable(void)
{
ESP_ERROR_CHECK(mcpwm_generator_set_force_level(s_gen[0], 0, true));
ESP_ERROR_CHECK(mcpwm_generator_set_force_level(s_gen[1], 0, true));
ESP_ERROR_CHECK(mcpwm_generator_set_force_level(s_gen[2], 0, true));
ESP_ERROR_CHECK(mcpwm_generator_set_force_level(s_gen[3], 0, true));
s_output_enabled = false;
}
bool stepper_motor_output_is_enabled(void)
{
return s_output_enabled;
}
stepper_motor_direction_t stepper_motor_get_direction(void)
{
return s_direction;
}
void stepper_motor_set_duty_raw(uint16_t a_plus, uint16_t a_minus, uint16_t b_plus, uint16_t b_minus)
{
if (!s_running) {
return;
}
ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(s_cmpr[0], duty_raw_to_ticks(a_plus)));
ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(s_cmpr[1], duty_raw_to_ticks(a_minus)));
ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(s_cmpr[2], duty_raw_to_ticks(b_plus)));
ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(s_cmpr[3], duty_raw_to_ticks(b_minus)));
}
void stepper_motor_openloop_start_dir(stepper_motor_direction_t direction)
{
if (s_openloop_timer == NULL) {
esp_timer_create_args_t timer_args = {
.callback = openloop_timer_cb,
.arg = NULL,
.dispatch_method = ESP_TIMER_TASK,
.name = "step_ol",
.skip_unhandled_events = true,
};
ESP_ERROR_CHECK(esp_timer_create(&timer_args, &s_openloop_timer));
}
if (!s_running) {
return;
}
s_direction = (direction == STEPPER_DIR_REVERSE) ? STEPPER_DIR_REVERSE : STEPPER_DIR_FORWARD;
if (s_openloop_running) {
ESP_ERROR_CHECK(esp_timer_stop(s_openloop_timer));
s_openloop_running = false;
}
if (!s_output_enabled) {
stepper_motor_output_enable();
}
s_openloop_step_idx = 0;
apply_openloop_phase(s_openloop_step_idx);
const uint64_t period_us = 1000000ULL / (uint64_t)(STEPPER_OPENLOOP_STEP_HZ * 4U);
ESP_ERROR_CHECK(esp_timer_start_periodic(s_openloop_timer, period_us));
s_openloop_running = true;
ESP_LOGI(TAG,
"openloop start: dir=%s duty=%u step_hz=%u",
s_direction == STEPPER_DIR_FORWARD ? "forward" : "reverse",
STEPPER_STARTUP_DUTY_RAW,
STEPPER_OPENLOOP_STEP_HZ);
}
void stepper_motor_openloop_start(void)
{
stepper_motor_openloop_start_dir(s_direction);
}
void stepper_motor_forward(void)
{
stepper_motor_openloop_start_dir(STEPPER_DIR_FORWARD);
}
void stepper_motor_reverse(void)
{
stepper_motor_openloop_start_dir(STEPPER_DIR_REVERSE);
}
void stepper_motor_run_steps(stepper_motor_direction_t direction, uint32_t steps)
{
if (!s_running || steps == 0) {
return;
}
if (s_openloop_timer != NULL && s_openloop_running) {
ESP_ERROR_CHECK(esp_timer_stop(s_openloop_timer));
s_openloop_running = false;
}
s_direction = (direction == STEPPER_DIR_REVERSE) ? STEPPER_DIR_REVERSE : STEPPER_DIR_FORWARD;
if (!s_output_enabled) {
stepper_motor_output_enable();
}
for (uint32_t i = 0; i < steps; ++i) {
stepper_motor_step_once(s_direction);
vTaskDelay(pdMS_TO_TICKS(1000 / (STEPPER_OPENLOOP_STEP_HZ * 4U)));
}
stepper_motor_stop();
}
void stepper_motor_openloop_stop(void)
{
if (s_openloop_timer != NULL && s_openloop_running) {
ESP_ERROR_CHECK(esp_timer_stop(s_openloop_timer));
s_openloop_running = false;
}
stepper_motor_set_duty_raw(0, 0, 0, 0);
stepper_motor_output_disable();
ESP_LOGI(TAG, "openloop stop");
}
void stepper_motor_stop(void)
{
stepper_motor_openloop_stop();
}
void stepper_motor_init(void)
{
gpio_config_t gpio_cfg = {
.pin_bit_mask = (1ULL << STEPPER_PIN_A1) | (1ULL << STEPPER_PIN_A2) |
(1ULL << STEPPER_PIN_B1) | (1ULL << STEPPER_PIN_B2),
.mode = GPIO_MODE_OUTPUT,
.pull_up_en = GPIO_PULLUP_DISABLE,
.pull_down_en = GPIO_PULLDOWN_DISABLE,
.intr_type = GPIO_INTR_DISABLE,
};
ESP_ERROR_CHECK(gpio_config(&gpio_cfg));
gpio_set_level(STEPPER_PIN_A1, 0);
gpio_set_level(STEPPER_PIN_A2, 0);
gpio_set_level(STEPPER_PIN_B1, 0);
gpio_set_level(STEPPER_PIN_B2, 0);
ESP_LOGI(TAG, "Step1: GPIO done");
mcpwm_timer_config_t timer_cfg = {
.group_id = STEPPER_MCPWM_GROUP,
.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT,
.resolution_hz = STEPPER_PWM_RESOL_HZ,
.period_ticks = s_pwm_period_ticks,
.count_mode = MCPWM_TIMER_COUNT_MODE_UP,
.flags = { .update_period_on_empty = true },
};
ESP_ERROR_CHECK(mcpwm_new_timer(&timer_cfg, &s_timer));
ESP_LOGI(TAG, "Step2: timer created | pwm=%lu Hz | period=%lu",
(unsigned long)STEPPER_PWM_FREQ_HZ,
(unsigned long)s_pwm_period_ticks);
mcpwm_operator_config_t oper_cfg = { .group_id = STEPPER_MCPWM_GROUP };
ESP_ERROR_CHECK(mcpwm_new_operator(&oper_cfg, &s_oper[0]));
ESP_ERROR_CHECK(mcpwm_operator_connect_timer(s_oper[0], s_timer));
ESP_ERROR_CHECK(mcpwm_new_operator(&oper_cfg, &s_oper[1]));
ESP_ERROR_CHECK(mcpwm_operator_connect_timer(s_oper[1], s_timer));
ESP_LOGI(TAG, "Step3: operators connected");
mcpwm_comparator_config_t cmpr_cfg = { .flags.update_cmp_on_tez = true };
ESP_ERROR_CHECK(mcpwm_new_comparator(s_oper[0], &cmpr_cfg, &s_cmpr[0]));
ESP_ERROR_CHECK(mcpwm_new_comparator(s_oper[0], &cmpr_cfg, &s_cmpr[1]));
ESP_ERROR_CHECK(mcpwm_new_comparator(s_oper[1], &cmpr_cfg, &s_cmpr[2]));
ESP_ERROR_CHECK(mcpwm_new_comparator(s_oper[1], &cmpr_cfg, &s_cmpr[3]));
ESP_LOGI(TAG, "Step4: comparators created");
mcpwm_generator_config_t gen_cfg = { .gen_gpio_num = STEPPER_PIN_A1 };
ESP_ERROR_CHECK(mcpwm_new_generator(s_oper[0], &gen_cfg, &s_gen[0]));
gen_cfg.gen_gpio_num = STEPPER_PIN_A2;
ESP_ERROR_CHECK(mcpwm_new_generator(s_oper[0], &gen_cfg, &s_gen[1]));
gen_cfg.gen_gpio_num = STEPPER_PIN_B1;
ESP_ERROR_CHECK(mcpwm_new_generator(s_oper[1], &gen_cfg, &s_gen[2]));
gen_cfg.gen_gpio_num = STEPPER_PIN_B2;
ESP_ERROR_CHECK(mcpwm_new_generator(s_oper[1], &gen_cfg, &s_gen[3]));
ESP_LOGI(TAG, "Step5: generators created");
for (int i = 0; i < 4; ++i) {
ESP_ERROR_CHECK(mcpwm_generator_set_actions_on_timer_event(s_gen[i],
MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP,
MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_LOW),//MCPWM_GEN_ACTION_LOW
MCPWM_GEN_TIMER_EVENT_ACTION_END()));
ESP_ERROR_CHECK(mcpwm_generator_set_actions_on_compare_event(s_gen[i],
MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP,
s_cmpr[i], MCPWM_GEN_ACTION_HIGH),//
MCPWM_GEN_COMPARE_EVENT_ACTION_END()));
}
ESP_LOGI(TAG, "Step6: pwm actions configured");
stepper_motor_output_disable();
ESP_ERROR_CHECK(mcpwm_timer_enable(s_timer));
ESP_ERROR_CHECK(mcpwm_timer_start_stop(s_timer, MCPWM_TIMER_START_NO_STOP));
s_running = true;
stepper_motor_set_duty_raw(0, 0, 0, 0);
ESP_LOGI(TAG, "MCPWM init done, motor standby on boot");
}
main/MOTOR/step_motor_init.h
+50
View File
@@ -0,0 +1,50 @@
#ifndef __STEP_MOTOT_INIT_H__
#define __STEP_MOTOT_INIT_H__
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
#include "driver/gpio.h"
#include "esp_log.h"
#include "driver/mcpwm_prelude.h"
/* ---- 步进电机引脚 ---- */
#define STEPPER_PIN_A1 GPIO_NUM_20 /* A 相正 */
#define STEPPER_PIN_A2 GPIO_NUM_21 /* A 相负 */
#define STEPPER_PIN_B1 GPIO_NUM_23 /* B 相正 */
#define STEPPER_PIN_B2 GPIO_NUM_22 /* B 相负 */
/* ---- MCPWM 配置 ---- */
#define STEPPER_MCPWM_GROUP 0
#define STEPPER_PWM_RESOL_HZ 4000000U /* 定时器时钟分辨率 4 MHz */
#define STEPPER_PWM_FREQ_HZ 8000U /* PWM 载波频率 8 kHz */
#define STEPPER_DUTY_MAX 1000U /* 占空比满量程，和 A_PULS_DUTY 等变量一致 */
#define STEPPER_STARTUP_DUTY_RAW (STEPPER_DUTY_MAX / 2)
#define STEPPER_OPENLOOP_STEP_HZ 100U /* 上电演示转动的步进频率 */
#define STEPPER_DEFAULT_RUN_STEPS (STEPPER_OPENLOOP_STEP_HZ * 4U)
typedef enum {
STEPPER_DIR_FORWARD = 1,//原为 1
STEPPER_DIR_REVERSE = -1,//原为 -1
} stepper_motor_direction_t;
void stepper_motor_init(void);
void stepper_motor_set_duty_raw(uint16_t a_plus, uint16_t a_minus, uint16_t b_plus, uint16_t b_minus);
void stepper_motor_output_enable(void);
void stepper_motor_output_disable(void);
void stepper_motor_openloop_start(void);
void stepper_motor_openloop_start_dir(stepper_motor_direction_t direction);
void stepper_motor_forward(void);
void stepper_motor_reverse(void);
void stepper_motor_run_steps(stepper_motor_direction_t direction, uint32_t steps);
void stepper_motor_stop(void);
void stepper_motor_openloop_stop(void);
bool stepper_motor_output_is_enabled(void);
stepper_motor_direction_t stepper_motor_get_direction(void);
#ifdef __cplusplus
}
#endif
#endif /* __STEP_MOTOR_INIT_H__ */