From 4adc78553c8d48ff122506195fa33641134bd7b1 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期六, 13 十二月 2025 18:55:14 +0800
Subject: [PATCH] 路径点不重复的控制算法测试通了，但是好像不如之前路径点重复的版本好。增加了蓝牙接口部分。准备移植外包的MQTT。
---
STM32H743/FML/motion_control.c | 124 +++++++++--------------------------------
1 files changed, 28 insertions(+), 96 deletions(-)
diff --git a/STM32H743/FML/motion_control.c b/STM32H743/FML/motion_control.c
index 3658ba2..fdb4902 100644
--- a/STM32H743/FML/motion_control.c
+++ b/STM32H743/FML/motion_control.c
@@ -180,7 +180,7 @@
HIDO_UINT32 next_idx = start_idx + 1U;
/* 绠�鍖栭�昏緫锛氱洿鎺ヨ繑鍥炰笅涓�涓偣锛岀‘淇濇寜椤哄簭璁块棶姣忎釜鑸偣 */
- return next_idx;
+ return start_idx;
}
/* 璁＄畻甯︾鍙锋í鍚戣宸紙宸︽鍙宠礋锛� */
@@ -191,8 +191,17 @@
return 0.0f;
}
- HIDO_UINT32 idx0 = (nearest_idx == 0U) ? 0U : nearest_idx - 1U;
- HIDO_UINT32 idx1 = (nearest_idx + 1U < state->path_count) ? nearest_idx + 1U : nearest_idx;
+ HIDO_UINT32 idx0, idx1;
+ if (nearest_idx == 0U)
+ {
+ idx0 = 0U;
+ idx1 = (state->path_count > 1U) ? 1U : 0U;
+ }
+ else
+ {
+ idx0 = nearest_idx - 1U;
+ idx1 = nearest_idx;
+ }
float p0[2];
float p1[2];
@@ -240,24 +249,8 @@
float start_point[2];
mc_get_point(state, 0U, start_point);
- /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� */
+ /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� - 宸茬Щ闄OG */
static HIDO_UINT32 s_goto_log_idx = 0U;
- HIDO_BOOL target_changed = (fabsf(state->current_target_xy[0] - start_point[0]) > 0.01f) ||
- (fabsf(state->current_target_xy[1] - start_point[1]) > 0.01f);
-
- if (target_changed || (s_goto_log_idx++ % 50U) == 0U)
- {
- int tgt_x_int = (int)start_point[0];
- int tgt_x_frac = (int)(fabsf(start_point[0] - tgt_x_int) * 100);
- int tgt_y_int = (int)start_point[1];
- int tgt_y_frac = (int)(fabsf(start_point[1] - tgt_y_int) * 100);
-
- if (target_changed)
- {
- HIDO_Debug2("[MC_TGT]GOTO_START target changed: (%d.%02d,%d.%02d)\r\n",
- tgt_x_int, tgt_x_frac, tgt_y_int, tgt_y_frac);
- }
- }
state->current_target_xy[0] = start_point[0];
state->current_target_xy[1] = start_point[1];
@@ -336,47 +329,17 @@
/* 璋冭瘯锛氭瘡20甯ц緭鍑轰竴娆¤矾寰勮窡韪姸鎬� */
static HIDO_UINT32 s_path_log_idx = 0U;
- static HIDO_UINT32 s_last_nearest = 0U;
- static HIDO_UINT32 s_last_lookahead = 0U;
+ static HIDO_UINT32 s_last_nearest = 0xFFFFFFFF;
+ static HIDO_UINT32 s_last_lookahead = 0xFFFFFFFF;
/* 濡傛灉绱㈠紩鍙戠敓鍙樺寲锛岀珛鍗宠緭鍑篖OG */
+ /* 璺緞鐐硅皟璇昄OG - 宸茬Щ闄� */
+// static HIDO_UINT32 s_last_nearest = 0xFFFFFFFF;
+// static HIDO_UINT32 s_last_lookahead = 0xFFFFFFFF;
HIDO_BOOL index_changed = (nearest_idx != s_last_nearest) || (lookahead_idx != s_last_lookahead);
- if ((s_path_log_idx++ % 20U) == 0U || index_changed == HIDO_TRUE)
+ if (index_changed)
{
- float nearest_pt[2];
- mc_get_point(state, nearest_idx, nearest_pt);
- float dist_to_nearest = mc_distance(nearest_pt, state->pos);
- HIDO_BOOL reached = mc_is_point_reached(state, nearest_idx);
-
- /* 浣跨敤鏁存暟琛ㄧず娉曟墦鍗版诞鐐规暟 */
- int pos_x_int = (int)state->pos[0];
- int pos_x_frac = (int)(fabsf(state->pos[0] - pos_x_int) * 100);
- int pos_y_int = (int)state->pos[1];
- int pos_y_frac = (int)(fabsf(state->pos[1] - pos_y_int) * 100);
- int near_x_int = (int)nearest_pt[0];
- int near_x_frac = (int)(fabsf(nearest_pt[0] - near_x_int) * 100);
- int near_y_int = (int)nearest_pt[1];
- int near_y_frac = (int)(fabsf(nearest_pt[1] - near_y_int) * 100);
- int dn_int = (int)dist_to_nearest;
- int dn_frac = (int)((dist_to_nearest - dn_int) * 100);
-
- if (index_changed == HIDO_TRUE)
- {
- HIDO_Debug2("[MC_PATH]CHG pos=(%d.%02d,%d.%02d) near=%u->%u look=%u->%u dn=%d.%02d reach=%d\r\n",
- pos_x_int, pos_x_frac, pos_y_int, pos_y_frac,
- s_last_nearest, nearest_idx,
- s_last_lookahead, lookahead_idx,
- dn_int, dn_frac, reached);
- }
- else
- {
- HIDO_Debug2("[MC_PATH] near=%u(%d.%02d,%d.%02d) look=%u dn=%d.%02d reach=%d\r\n",
- nearest_idx, near_x_int, near_x_frac, near_y_int, near_y_frac,
- lookahead_idx,
- dn_int, dn_frac, reached);
- }
-
s_last_nearest = nearest_idx;
s_last_lookahead = lookahead_idx;
}
@@ -384,26 +347,7 @@
float target[2];
mc_get_point(state, lookahead_idx, target);
- /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� */
- HIDO_BOOL target_changed = (fabsf(state->current_target_xy[0] - target[0]) > 0.01f) ||
- (fabsf(state->current_target_xy[1] - target[1]) > 0.01f);
-
- if (target_changed)
- {
- int old_x_int = (int)state->current_target_xy[0];
- int old_x_frac = (int)(fabsf(state->current_target_xy[0] - old_x_int) * 100);
- int old_y_int = (int)state->current_target_xy[1];
- int old_y_frac = (int)(fabsf(state->current_target_xy[1] - old_y_int) * 100);
- int new_x_int = (int)target[0];
- int new_x_frac = (int)(fabsf(target[0] - new_x_int) * 100);
- int new_y_int = (int)target[1];
- int new_y_frac = (int)(fabsf(target[1] - new_y_int) * 100);
-
- HIDO_Debug2("[MC_TGT]FOLLOW target changed: (%d.%02d,%d.%02d)->(%d.%02d,%d.%02d) near=%u look=%u\r\n",
- old_x_int, old_x_frac, old_y_int, old_y_frac,
- new_x_int, new_x_frac, new_y_int, new_y_frac,
- nearest_idx, lookahead_idx);
- }
+ /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� - 宸茬Щ闄OG */
state->current_target_xy[0] = target[0];
state->current_target_xy[1] = target[1];
@@ -421,11 +365,15 @@
state->last_heading_err = heading_err;
float cross_track = mc_cross_track_error(state, nearest_idx);
- float yaw_rate_cmd = state->config.heading_kp * heading_err
- + state->config.heading_kd * heading_err_rate
- + state->config.xtrack_kp * cross_track;
+ float p_term = state->config.heading_kp * heading_err;
+ float d_term = state->config.heading_kd * heading_err_rate;
+ float x_term = state->config.xtrack_kp * cross_track;
+
+ float yaw_rate_cmd = p_term + d_term + x_term;
yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);
+ /* 璋冭瘯LOG锛氭瘡10甯ф墦鍗颁竴娆ID鎺у埗璇︽儏锛屽府鍔╁垎鏋愭鍖�/涓嶈浆鍚戦棶棰� - 宸茬Щ闄� */
+
float forward = state->config.base_speed_mps
- state->config.heading_speed_scale * fabsf(heading_err)
- state->config.xtrack_speed_scale * fabsf(cross_track);
@@ -474,13 +422,11 @@
_cfg->xtrack_speed_scale = MC_CFG_XTRACK_SPEED_SCALE;
}
-/* 鍒濆鍖栨帶鍒跺櫒锛岀粦瀹氳矾寰勪笌閰嶇疆 */
+/* MC_Init */
HIDO_VOID MC_Init(MC_State *_state, const MC_Config *_cfg, const float *_path_xy, HIDO_UINT32 _point_count)
{
if (_state == HIDO_NULL || _cfg == HIDO_NULL || _path_xy == HIDO_NULL || _point_count < 2U)
{
- DBG_Printf("[MC_Init] ERROR: Invalid parameters (state=%p, cfg=%p, path=%p, count=%u)\r\n",
- (void*)_state, (void*)_cfg, (void*)_path_xy, _point_count);
return;
}
@@ -491,9 +437,6 @@
_state->stage = MC_STAGE_GOTO_START;
_state->nearest_index = 0U;
_state->lookahead_index = 0U;
-
- DBG_Printf("[MC_Init] OK: path_count=%u, first_point=(%.2f,%.2f)\r\n",
- _point_count, _path_xy[0], _path_xy[1]);
}
/* 娉ㄥ叆鏈�鏂� ENU 浣嶅Э鍙婅埅鍚�/閫熷害 */
@@ -565,8 +508,6 @@
{
if (s_last_path_valid)
{
- HIDO_Debug2("[MC_TGT]WARNING: target_valid=FALSE, path invalid (path=%p, count=%u)\r\n",
- (void*)_state->path_xy, _state->path_count);
s_last_path_valid = HIDO_FALSE;
}
return;
@@ -577,7 +518,6 @@
{
if (s_last_pose_valid)
{
- HIDO_Debug2("[MC_TGT]WARNING: target_valid=FALSE, pose_valid=FALSE\r\n");
s_last_pose_valid = HIDO_FALSE;
}
return;
@@ -640,12 +580,6 @@
if (!s_last_target_valid)
{
- int x_int = (int)_out->target_xy[0];
- int x_frac = (int)(fabsf(_out->target_xy[0] - x_int) * 100);
- int y_int = (int)_out->target_xy[1];
- int y_frac = (int)(fabsf(_out->target_xy[1] - y_int) * 100);
- HIDO_Debug2("[MC_TGT]target_valid: FALSE->TRUE, stage=%d, target=(%d.%02d,%d.%02d)\r\n",
- _state->stage, x_int, x_frac, y_int, y_frac);
}
}
else
@@ -656,8 +590,6 @@
if (s_last_target_valid || _state->stage != s_last_stage_for_target)
{
- HIDO_Debug2("[MC_TGT]target_valid: TRUE->FALSE, stage=%d->%d (will output 0,0)\r\n",
- s_last_stage_for_target, _state->stage);
}
}
--
Gitblit v1.10.0