/*******************************************************************************
|
* File Name : motion_config.h
|
* Description : Tunable parameters for mower motion controller
|
* Created on : 2025-11-22
|
*******************************************************************************/
|
|
#ifndef FML_MOTION_CONFIG_H_
|
#define FML_MOTION_CONFIG_H_
|
|
#include "HIDO_TypeDef.h"
|
|
/* 控制回路期望频率 (Hz) */
|
#define MC_CFG_CONTROL_HZ (75.0f)
|
/* 最大前进 / 后退速度 (m/s) */
|
#define MC_CFG_MAX_FORWARD_MPS (0.60f)
|
#define MC_CFG_MAX_REVERSE_MPS (0.20f)
|
/* 转向角速度上限 (rad/s) */
|
#define MC_CFG_MAX_TURN_RATE_RAD (0.80f)
|
/* 巡航基础速度与最低跟随速度 (m/s) */
|
#define MC_CFG_BASE_SPEED_MPS (0.45f)
|
#define MC_CFG_MIN_FOLLOW_SPEED_MPS (0.08f)
|
/* Pure pursuit lookahead 范围 (m) */
|
#define MC_CFG_LOOKAHEAD_MIN_M (0.35f)
|
#define MC_CFG_LOOKAHEAD_MAX_M (1.20f)
|
/* 起点/终点距离与航向容差 (m / rad) */
|
#define MC_CFG_START_POS_TOL_M (0.25f)
|
#define MC_CFG_START_HEADING_TOL_RAD (0.20f)
|
#define MC_CFG_GOAL_TOL_M (0.30f)
|
/* 控制增益：航向 P/D、横向 P，以及误差→降速的比例 */
|
#define MC_CFG_HEADING_KP (1.6f)
|
#define MC_CFG_HEADING_KD (0.25f)
|
#define MC_CFG_XTRACK_KP (-1.0f)
|
#define MC_CFG_HEADING_SPEED_SCALE (0.60f)
|
#define MC_CFG_XTRACK_SPEED_SCALE (0.40f)
|
|
/* GGA 原点经纬高（deg/m），供 ENU 转换使用 */
|
#define MC_CFG_ORIGIN_LAT_DEG (39.831706)
|
#define MC_CFG_ORIGIN_LON_DEG (116.279282)
|
#define MC_CFG_ORIGIN_ALT_M (4)
|
|
/* PWM 中值与 span，用于 forward/turn → 脉宽映射 */
|
#define MC_CFG_PWM_CENTER_US (1500U)
|
#define MC_CFG_PWM_SPAN_US (500U)
|
|
/*******************************************************************************
|
* 校准参数（基于实测数据，日期：2025-12-05）
|
* 前向速度模型：v = MC_CFG_FORWARD_K × (1500 - pwm) + MC_CFG_FORWARD_BIAS
|
* 说明：1000 PWM = 最大前进，1500 PWM = 停止，2000 PWM = 最大后退
|
*******************************************************************************/
|
#define MC_CFG_FORWARD_K (0.00196f) // 速度系数 (m/s per PWM)
|
#define MC_CFG_FORWARD_BIAS (-0.077f) // 速度偏置 (m/s)
|
#define MC_CFG_FORWARD_DEADZONE_PWM (40) // 前进死区阈值
|
|
/* 转向角速度模型：ω = MC_CFG_STEERING_K × (1500 - pwm) for left/right */
|
#define MC_CFG_STEERING_K_LEFT (0.00206f) // 左转系数 (rad/s per PWM, 1000 PWM -> ~1.03 rad/s -> ~59 deg/s)
|
#define MC_CFG_STEERING_K_RIGHT (0.00201f) // 右转系数 (rad/s per PWM, 2000 PWM -> ~1.00 rad/s -> ~57 deg/s)
|
#define MC_CFG_STEERING_TRIM (10) // 转向补偿值（修正左偏）
|
|
/* 基于校准数据更新速度限制 */
|
#undef MC_CFG_MAX_FORWARD_MPS
|
#define MC_CFG_MAX_FORWARD_MPS (0.56f) // 实测最大前进速度 (1000 PWM)
|
|
#endif /* FML_MOTION_CONFIG_H */
|
