EDC/edc-mcu
3
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

主流程

Browse Source
This commit is contained in:
kirl committed 2026-08-01 02:41:29 +08:00
1 parent f96e72dbb8
commit 0045c0c390
10 files changed
+216 -74
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

components/actuator/CMakeLists.txt
+1 -1
View File
@@ -1,3 +1,3 @@
idf_component_register(SRCS "actuator.c"
INCLUDE_DIRS "."
REQUIRES emm_v5 servo protocol driver freertos)
REQUIRES emm_v5 servo magnet rgb protocol driver freertos)
components/actuator/actuator.c
+138 -28
View File
@@ -6,6 +6,8 @@
#include "actuator.h"
#include "emm_v5.h"
#include "servo.h"
#include "magnet.h"
#include "rgb.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
@@ -36,6 +38,19 @@ static inline void _frame_gap(void)
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));
}
/* 内部: 带 RGB 闪烁的延时 (周期 ms, 亮灭各半) */
static void _wait_flash(uint32_t ms, uint8_t r, uint8_t g, uint8_t b, uint32_t period_ms)
{
uint32_t half = pdMS_TO_TICKS(period_ms / 2);
int cycles = (int)(ms / period_ms);
for (int i = 0; i < cycles; i++) {
rgb_set(r, g, b);
vTaskDelay(half);
rgb_off();
vTaskDelay(half);
}
}
/* ================================================================
* 初始化
* ================================================================ */
@@ -49,7 +64,7 @@ void actuator_init(void)
servo_init(ACTUATOR_SERVO_GPIO);
/* 配置舵机限位: 抬起=80°, 放下=160° */
servo_set_limits(80.0f, 160.0f);
servo_set_limits(0.0f, 95.0f);
/* 上电默认归位到抬起状态 */
servo_set_up();
@@ -105,11 +120,8 @@ static void _actuator_move_abs(uint8_t addr, uint8_t dir, uint32_t pulses,
* 内部: Y 轴双电机同步运动
* ================================================================ */
static void _actuator_move_y_sync(uint32_t pulses_y)
static void _actuator_move_y_sync(uint32_t pulses_y, uint16_t vel, uint8_t acc)
{
uint16_t vel = ACTUATOR_DEFAULT_VEL;
uint8_t acc = ACTUATOR_DEFAULT_ACC;
/* Y 轴正向 = CCW (dir=1), 双电机同步 */
_actuator_move_abs(ACTUATOR_ADDR_Y1, ACTUATOR_DIR_Y, pulses_y, vel, acc, true);
_frame_gap();
@@ -119,9 +131,9 @@ static void _actuator_move_y_sync(uint32_t pulses_y)
/* 广播地址触发一次, 两台电机同时开始运动 */
Emm_V5_Synchronous_motion(0);
ESP_LOGD(TAG, "Y 同步(CCW): addr%d+%d -> %lu pulses (%.2fmm)",
ESP_LOGD(TAG, "Y 同步(CCW): addr%d+%d -> %lu pulses (%.2fmm) vel=%d acc=%d",
ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2, pulses_y,
(double)pulses_y / (double)ACTUATOR_SCALE_Y);
(double)pulses_y / (double)ACTUATOR_SCALE_Y, vel, acc);
}
/* ================================================================
@@ -130,28 +142,83 @@ static void _actuator_move_y_sync(uint32_t pulses_y)
static void _actuator_move_fragment(const edc_fragment_t *frag)
{
uint16_t vel = ACTUATOR_DEFAULT_VEL;
uint8_t acc = ACTUATOR_DEFAULT_ACC;
/* 固定速度/加速度 */
uint16_t vel_xy = 1500;
uint8_t acc_xy = 200;
uint16_t vel_angle = 50;
uint8_t acc_angle = 100;
/* source 坐标 → 脉冲数 */
uint32_t src_pulses_x = (uint32_t)(frag->source.x * s_scale_x);
uint32_t src_pulses_y = (uint32_t)(frag->source.y * s_scale_y);
uint32_t src_pulses_angle = (uint32_t)(frag->source.angle_x10 * s_scale_angle);
/* target 坐标 → 脉冲数 */
uint32_t pulses_x = (uint32_t)(frag->target.x * s_scale_x);
uint32_t pulses_y = (uint32_t)(frag->target.y * s_scale_y);
uint32_t pulses_angle = (uint32_t)(frag->target.angle_x10 * s_scale_angle);
uint32_t dst_pulses_x = (uint32_t)(frag->target.x * s_scale_x);
uint32_t dst_pulses_y = (uint32_t)(frag->target.y * s_scale_y);
uint32_t dst_pulses_angle = (uint32_t)(frag->target.angle_x10 * s_scale_angle);
ESP_LOGI(TAG, "Fragment: X=%lu Y=%lu Angle=%lu (raw x=%u y=%u a=%u)",
pulses_x, pulses_y, pulses_angle,
frag->target.x, frag->target.y, frag->target.angle_x10);
ESP_LOGI(TAG, "Fragment: SRC(X=%lu Y=%lu A=%lu) → DST(X=%lu Y=%lu A=%lu)",
src_pulses_x, src_pulses_y, src_pulses_angle,
dst_pulses_x, dst_pulses_y, dst_pulses_angle);
/* Angle / Z 旋转轴 (独立, CW) */
_actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE, pulses_angle, vel, acc, false);
/* ---- 1. 磁铁掉电 ---- */
magnet_set(false);
/* ---- 2. 运动到 source 位置 ---- */
/* Angle / Z 旋转轴 */
_actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE,
src_pulses_angle, vel_angle, acc_angle, false);
_frame_gap();
/* X 轴 (独立, CW) */
_actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X, pulses_x, vel, acc, false);
/* X 轴 */
_actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X,
src_pulses_x, vel_xy, acc_xy, false);
_frame_gap();
/* Y 轴 (双电机同步) */
_actuator_move_y_sync(pulses_y);
_actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy);
/* ---- 3. 等待 2s 运动到位 (红灯快闪) ---- */
_wait_flash(2000, 128, 0, 0, 200);
/* ---- 4. 舵机放下 ---- */
servo_set_down();
_wait_flash(500, 128, 0, 0, 200);
/* ---- 5. 磁铁吸合 ---- */
magnet_set(true);
_wait_flash(500, 128, 0, 0, 200);
/* ---- 6. 舵机抬起 ---- */
servo_set_up();
_wait_flash(500, 128, 0, 0, 200);
/* ---- 7. 运动到 target 位置 ---- */
_actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE,
dst_pulses_angle, vel_angle, acc_angle, false);
_frame_gap();
_actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X,
dst_pulses_x, vel_xy, acc_xy, false);
_frame_gap();
_actuator_move_y_sync(dst_pulses_y, vel_xy, acc_xy);
/* ---- 8. 等待 2s 运动到位 (红灯快闪) ---- */
_wait_flash(2000, 128, 0, 0, 200);
/* ---- 9. 舵机放下 ---- */
servo_set_down();
_wait_flash(500, 128, 0, 0, 200);
/* ---- 10. 磁铁掉电 ---- */
magnet_set(false);
_wait_flash(500, 128, 0, 0, 200);
/* ---- 11. 舵机抬起 ---- */
servo_set_up();
_wait_flash(500, 128, 0, 0, 200);
}
/* ================================================================
@@ -180,16 +247,8 @@ int actuator_execute_cycle(const edc_cycle_t *cycle)
for (int i = 0; i < cycle->frag_count; i++) {
ESP_LOGI(TAG, "-- Fragment %d/%d --", i + 1, cycle->frag_count);
_actuator_move_fragment(&cycle->fragments[i]);
/* 等待当前片段运动完成 (TODO: 改为轮询到位标志) */
if (i < cycle->frag_count - 1) {
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAG_DELAY_MS));
}
}
/* 最后一个 fragment 也等待 */
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAG_DELAY_MS));
ESP_LOGI(TAG, "=== Cycle #%u 完成 ===", cycle->cycle_id);
return 0;
}
@@ -368,3 +427,54 @@ void actuator_motor_stop(uint8_t addr)
ESP_LOGD(TAG, "Y 同步: 电机%d 同停", peer);
}
}
/* ================================================================
* 协议回调: 收到 Cycle → 执行 → DONE
* ================================================================ */
static edc_cycle_t s_last_cycle;
static bool s_last_cycle_valid;
static portMUX_TYPE s_cycle_lock = portMUX_INITIALIZER_UNLOCKED;
const edc_cycle_t *actuator_get_last_cycle(void)
{
return s_last_cycle_valid ? &s_last_cycle : NULL;
}
bool actuator_is_last_cycle_valid(void)
{
return s_last_cycle_valid;
}
void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
{
static uint16_t s_last_exec_id = 0xFFFF;
/* 保存快照 */
portENTER_CRITICAL(&s_cycle_lock);
memcpy(&s_last_cycle, cycle, sizeof(edc_cycle_t));
s_last_cycle_valid = true;
portEXIT_CRITICAL(&s_cycle_lock);
ESP_LOGI(TAG, "Cycle #%u via %s, %u frags",
cycle->cycle_id, ch == EDC_CH_I2C ? "I2C" : "UART",
cycle->frag_count);
/* I2C / UART 冗余下发, 同一 cycle_id 只执行一次 */
if (cycle->cycle_id == s_last_exec_id) {
ESP_LOGD(TAG, "Cycle #%u 重复, 跳过执行", cycle->cycle_id);
return;
}
s_last_exec_id = cycle->cycle_id;
/* 执行电机运动 (阻塞, 含拾放全流程) */
ESP_LOGI(TAG, "开始执行 Cycle #%u ...", cycle->cycle_id);
actuator_execute_cycle(cycle);
/* 全部 Fragment 执行完成后回复 DONE */
edc_proto_send_done(cycle->cycle_id);
ESP_LOGI(TAG, "Cycle #%u 执行完毕, DONE 已发送", cycle->cycle_id);
/* 设置 RGB 状态为 DONE, 主循环将持续绿灯慢闪 */
rgb_set_state(RGB_STATE_DONE);
}
components/actuator/actuator.h
+9
View File
@@ -231,7 +231,16 @@ void actuator_motor_mm(uint8_t addr, float distance_mm, uint16_t vel,
* @param addr 电机地址 (1~4)
*/
void actuator_motor_stop(uint8_t addr);
/**
* @brief 获取最近一次执行的 Cycle 快照 (供外部读取, 如 Web 面板)
* @return 有效时返回指针, 否则 NULL
*/
const edc_cycle_t *actuator_get_last_cycle(void);
/**
* @brief 最近 Cycle 是否有效
*/
bool actuator_is_last_cycle_valid(void);
#ifdef __cplusplus
}
#endif