#!/usr/bin/env python3
|
"""基于可信数据段提取参数"""
|
import numpy as np
|
|
# 手动提取的可信数据点（前进测试，GPS正常）
|
pwm_speed_pairs = [
|
(1400, 0.065), # FWD_VLOW_C: 0.32m/5.0s
|
(1300, 0.298), # FWD_LOW_C: 1.48m/5.0s
|
(1100, 0.493), # FWD_HIGH_C: 2.44m/5.0s (跳过1200和1050因为有GPS跳变)
|
]
|
|
print("========== 基于可信数据的线性拟合 ==========\n")
|
print("使用数据点:")
|
for pwm, v in pwm_speed_pairs:
|
print(f" PWM {pwm}: {v:.3f} m/s")
|
|
# 线性拟合：v = k × (1500 - pwm) + bias
|
pwms = np.array([p[0] for p in pwm_speed_pairs])
|
speeds = np.array([p[1] for p in pwm_speed_pairs])
|
|
x = 1500 - pwms
|
y = speeds
|
|
coef = np.polyfit(x, y, 1)
|
k = coef[0]
|
bias = coef[1]
|
|
print(f"\n========== 拟合结果 ==========")
|
print(f"模型：v = {k:.6f} × (1500 - pwm) + ({bias:.6f})")
|
print(f"\n验证:")
|
for pwm, v_measured in pwm_speed_pairs:
|
v_pred = k * (1500 - pwm) + bias
|
print(f" PWM {pwm}: 实测 {v_measured:.3f} m/s, 预测 {v_pred:.3f} m/s, 误差 {abs(v_measured-v_pred):.3f} m/s")
|
|
# 预测极限
|
v_1000 = k * (1500 - 1000) + bias
|
v_1500 = bias
|
|
print(f"\n========== 预测 ==========")
|
print(f"PWM 1500 (停止): {v_1500:.3f} m/s (应接近0)")
|
print(f"PWM 1000 (最大前进): {v_1000:.3f} m/s")
|
|
print(f"\n========== 推荐的C代码参数 ==========")
|
print(f"#define MC_CFG_FORWARD_K ({k:.6f}f)")
|
print(f"#define MC_CFG_FORWARD_BIAS ({bias:.6f}f)")
|
print(f"#define MC_CFG_MAX_FORWARD_MPS ({v_1000:.3f}f)")
|
|
# 转向数据（从之前的分析结果手动提取，只用前期可信数据）
|
print(f"\n\n========== 转向参数（基于部分数据）==========")
|
print("由于后期GPS失锁，转向数据不完整")
|
print("建议先用保守值：")
|
print("#define MC_CFG_STEERING_K_LEFT (0.00010f) // 待更精确校准")
|
print("#define MC_CFG_STEERING_K_RIGHT (0.00010f) // 待更精确校准")
|
print("#define MC_CFG_STEERING_TRIM (10) // 向右补偿10PWM")
|
