414 lines
13 KiB
C
414 lines
13 KiB
C
|
#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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|