From 4cd0803c4189707a5eff81251d36ad3feccb1b3b Mon Sep 17 00:00:00 2001 From: spdis Date: Mon, 25 May 2026 14:50:03 +0800 Subject: [PATCH] Replace PID with proportional steering, asymmetric servo limits 1.20~1.80ms, center 1.50ms --- control/servo.cpp | 71 ++++++++-------------------------------------- control/servo.hpp | 5 +--- ctl.sh | 2 +- model/test_model | Bin 0 -> 21872 bytes types.hpp | 4 +-- 5 files changed, 16 insertions(+), 66 deletions(-) create mode 100644 model/test_model diff --git a/control/servo.cpp b/control/servo.cpp index 5a89b4b..08ef507 100644 --- a/control/servo.cpp +++ b/control/servo.cpp @@ -4,73 +4,30 @@ #include #include -// ── 全局对象 ────────────────────────────────────────── -static PWM* servo_pwm = nullptr; // pwmchip1/pwm0, 3ms 周期 -static PID* servo_pid = nullptr; // 位置式 PID, 偏差→脉宽偏移量 (ns) +static PWM* servo_pwm = nullptr; static bool serv_initialized = false; -// ── 运行时热调参数 ────────────────────────────────── -// 这些值会被 scheduler.cpp 每 1 秒从文件热读覆盖 -static float g_deadband = 0.03f; // 死区 (归一化偏差, 默认 ±0.03) -static float g_steer_gain = 1.5f; // 转向增益 (偏差放大系数) +static float g_deadband = 0.03f; // 死区 (归一化偏差) +static float g_steer_gain = 1.5f; // 转向增益 +static constexpr float HALF_RANGE = 300000.0f; // 半行程 (ns), ±0.30ms -// ============================================================ -// servo_init — 初始化舵机 PWM + PID -// -// 调用时机: scheduler_init() 启动时调用一次 -// -// 硬件: pwmchip1/pwm0 -// 周期 = 3ms (333Hz) — 舵机标准周期 -// 中位 = 1.52ms — 前轮摆正 -// 行程 = 1.52 ± 0.21ms — 约 ±45° 物理摆角 -// -// PID: 位置式 -// Kp=400000, Ki=200000, Kd=0 -// 输出限幅 = ±SERVO_MAX_DELTA_NS (±210000ns = ±0.21ms) -// 这组参数等效于线性增益 (因为 Ki 很大, 积分瞬间到位) -// ============================================================ void servo_init() { if (serv_initialized) return; - servo_pwm = new PWM(SERVO_PWMCHIP, SERVO_PWMID, SERVO_PERIOD_NS); servo_pwm->setDutyCycle(SERVO_CENTER_NS); servo_pwm->enable(); - - servo_pid = new PID(400000, 200000, 0, 0, PID_POSITIONAL, - static_cast(SERVO_MAX_DELTA_NS), - static_cast(SERVO_MAX_DELTA_NS)); - serv_initialized = true; } -// ── 热调接口 ───────────────────────────────────────── void servo_set_deadband(float v) { g_deadband = std::max(0.0f, v); } void servo_set_steer_gain(float v) { g_steer_gain = std::max(0.1f, v); } -void servo_set_pid(double kp, double ki, double kd) { if (servo_pid) servo_pid->setPID(kp, ki, kd); } +void servo_set_pid(double, double, double) {} // 保留接口兼容, 不再使用 PID -// ============================================================ -// servo_set_angle — 偏差 → 舵机脉宽 (核心控制回路) -// -// 调用频率: scheduler.cpp sched_control() 每 5ms 一次 -// -// 输入: deviation — 归一化中线偏差 (-1=偏左极限, 0=正中, +1=偏右极限) -// 由 strategy/deviation.cpp 的 calc_deviation() 计算 -// -// 处理流程: -// 1. 钳位偏差至 [-1, +1] -// 2. 死区: |deviation| < g_deadband → 直接回中, 跳过 PID -// 典型值 g_deadband=0.03, 即偏差 < 3% 时不转向 -// 3. 增益缩放: deviation *= g_steer_gain -// 典型值 g_steer_gain=1.5, 把微小偏差放大供 PID 处理 -// 4. PID 计算: out = PID(deviation) -// 位置式 PID, 输出为脉宽偏移量 (ns) -// 5. 叠加中位: duty_ns = 1520000 + out -// 正偏差(偏右) → out>0 → 脉宽>中位 → 舵机右转 -// 负偏差(偏左) → out<0 → 脉宽<中位 → 舵机左转 -// 6. 限幅: 1.52 ± 0.21ms (1310000ns ~ 1730000ns) -// 7. 写入 PWM sysfs -// ============================================================ +// 纯比例转向: deviation(-1~+1) → 线性映射到舵机脉宽范围 +// deviation=0 → 中位 1.50ms +// deviation=±1 → 极限 1.20ms / 1.80ms +// steer_gain 控制灵敏度 (1.0=标准, 1.5=灵敏, 0.5=柔和) void servo_set_angle(float deviation) { if (!serv_initialized) return; @@ -82,16 +39,12 @@ void servo_set_angle(float deviation) return; } - float scaled = deviation * g_steer_gain; - float out = -servo_pid->update(scaled); // 负号: 偏差正(右偏)→输出负→左转回正 - float duty_ns = SERVO_CENTER_NS + out; - duty_ns = std::clamp(duty_ns, - static_cast(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS), - static_cast(SERVO_CENTER_NS + SERVO_MAX_DELTA_NS)); + float offset = deviation * g_steer_gain * HALF_RANGE; + float duty_ns = SERVO_CENTER_NS + offset; + duty_ns = std::clamp(duty_ns, 1200000.0f, 1800000.0f); servo_pwm->setDutyCycle(static_cast(duty_ns)); } -// servo_center — 紧急回中 (停止时调用) void servo_center() { if (serv_initialized) diff --git a/control/servo.hpp b/control/servo.hpp index 41b3bc5..5f420c2 100644 --- a/control/servo.hpp +++ b/control/servo.hpp @@ -1,12 +1,9 @@ #pragma once #include "types.hpp" -#include "control/pid.hpp" void servo_init(); void servo_set_angle(float deviation); void servo_center(); void servo_set_deadband(float v); void servo_set_steer_gain(float v); -void servo_set_pid(double kp, double ki, double kd); - -inline float servo_get_duty_ns() { return SERVO_CENTER_NS; } +void servo_set_pid(double kp, double ki, double kd); // 保留兼容, 空实现 diff --git a/ctl.sh b/ctl.sh index cbf7509..700018b 100644 --- a/ctl.sh +++ b/ctl.sh @@ -34,7 +34,7 @@ init_pins() { echo 0 > /sys/class/pwm/pwmchip1/export 2>/dev/null echo 3000000 > /sys/class/pwm/pwmchip1/pwm0/period 2>/dev/null echo 1 > /sys/class/pwm/pwmchip1/pwm0/enable 2>/dev/null - echo 1520000 > /sys/class/pwm/pwmchip1/pwm0/duty_cycle 2>/dev/null + echo 1500000 > /sys/class/pwm/pwmchip1/pwm0/duty_cycle 2>/dev/null echo 12 > "$DIR/speed" 2>/dev/null echo 0.03 > "$DIR/deadband" 2>/dev/null diff --git a/model/test_model b/model/test_model new file mode 100644 index 0000000000000000000000000000000000000000..faa2b6cc1a5e2cdadcdd48eb5e91449b9aeb976f GIT binary patch literal 21872 zcmeHvdw3L8n)m7I5JG6613?)KPUE%8j**}ujN@8?Ag~0Dz-%+9)7^CECf!L)8WIRf z1%t9{(BOresO_%o>^B=`n9&{48OQ1nqB8@CI-fTjX-2>s2#6!dt-jxTs!pf6i8K3s z&$G|>59i70x6XUs+j-CZROKw4Ty~4(a4-=hW~SUO<{_+VWo*9Hd~kBycs2kz7qO8n z8|f^LGxJrJ;^X`$EYSkym}HFP5Z`#oL;!Mn0f$(00tyQW_OtRV!1?gZ;fiZatbn2b z#7DNWe=zgeeI~ttNy`py7?G$Z9ifTOZSlD+J^_zdh6yOjM`fcp)shRy?Gw<0JPIEE zYtqtV`Tkg#`0Bl81|a7XFlM5efJ95-cJT4?naRKV>$WInTKc}rFHHEwf>v?aHIbTm z13OzzdSr&fH;)rm_JmH_WWF=hau)83j0XL07MFd z5Qp=p-XE9sIItJRpQs*$=^~5#(;t!Ao(O}j2T?A{BRb+GTE_05JTW&^@?e%a@e((? zg7QcgmFMKl&j!0&HY|~^9ei-imSuycI2H`?yfNU+l7||HoG6_0yJY->sJii**Ix3@ zAO4foj}=^Z$nWl0ux!wXrK^|x@{5a~f{5=79mpXanc)l&b{~rT|NR|!@}ox(*pRCEY7Y8i)eUt`{&~T$ zKiJR^T;#8TW9jdfSVP&!Ag>C zQPZr(dm3VutfINTl8{xQkiXF!2#^YyB$ajb6}7B>L9CGl=hZdDShOPASiwQ4ei5q- zMd}(WSY<;+Fw7cb4WVc~^ZSY2-xv!v#Qf1>I8{R&i={7u#og(bA;qCLM+rkr$#x@Z~*zopo$YaCX$6VQl z7h^KX8e_xL_$|VC8{R&isW!Yh{+b+9ZFn(eQ%<=JPjiO|b8L7qCsSU)h8Ob+;j3+U zF((ne-iD|7NrYw_K3^aKH5*>E1m~J4Br)`($8k3TQkzZjC4sxdUQtGk&)i}Ub?eYk&4@X|02KM*?>j(yz=q zCjA(2!ehszf_zP4Yw{i0$yTYKwoJ-VHK_`8VJB*?eAw)@czytJGSbf>8aErh5p!60 zgz>86a_(Vk`<1W}&%0S9pi3jJy_z+F_Ag_Zckd<4n;go#`z1}Dc-i+HqY)Y6)Ghgr zD-p91kI&Pk%Sh%IXMHDj8veQcHR;YK!%w=62haLgVB~D>kB*~~G3Bf;3$o!C8GezQ zlRfexy+>Xg?~z-y9(hTiN4{6>k()gacth%Il83P}&Z{1ljDg_I(j?9 z?qMp|anDX!8=kI%?99{?(qONuD@~9S48U$pmySc%@n7wb4od7Q*jZ{^WB6RC3#5rg z2E7b=IRx5Fd+p^Hy3qbobr@1lN=@1}^pgkVXmYu{*t1QE+~r}(Ty|%Fb`^WbF`Sj# z(zQqzUhkpx9?u^s4QFm!y4KUB+zOhvt}bAA1I}k#6_PRQ%&Ss!V4Kpywkh}E`6A$& zr9G^fbt~fm>-6PvlfG41lI&KF_nXE}Q@+}*lwDcCX5Z~*L+{LEbD%2+F&y8jRKYjR zdO~S)>(W|(9{U7vVSJl%uimZP7w=YVFN$XOY zlL@8D)2#&5gc9%|!sh`77*8lUm*ufnp?@6W?I=e?-KrE$1k|=Ft|CCaOL5&*z*aWm zInbpXgpI#*>|t%=h!$z|JwM`l@Nb$>z`~xb%Dte8#=De=r%Tq}bf^(es~lzBa;>LZ z-tRgUO@EFnicl$W_UN?E0Z29ARCuivcDz&T}{N!pK_(o@jEbJYVaQJE1ok-zrad5xN82 zrd|ztU;}$D-=Pg+D(sNaAH2yYnHuwGa-*urIq0Kql&=ouJBs?i2CTd|Taymo<7V!; zZg%BQ*qlP(wL?be$rcu_UTf?AJKH#<5ay9hUwD$nmpE4?PmX2 z<7S078~&);CD#VF$`LIgkGO%#4c~$vX=T#{o`$~@@+aUqjyBf05Fgr0c9CRkzAf)zXJH#bv3GC4 zw_376rxBgSrzVuBzE$r0^BF(dvW$L-dfg_Y{ZhY_Ti7-k{X<3@m(j*$v~fA4Z%ga* zA?=LRgf{3yA3KhI<6@ zzmRQ!@2O86M7#)XpxM)2V0TlVBvxs?dVeP!2^b$`u*{*+-wBu|I`Y@ z4;y8aA9Glj47+8NT}HWO*e^E+T48%DY;X0puvYJqq~;B2UEF_nqh6~uU8xM{N(Jhv z%A+er|KmR3W#DDtW#DDtWy5b9KQUhO@jZB4Y6j0*hX%do*xdv<>)@9WVRNoO1fOqu z^BX?rzv$3qG4GGKu!J34mXK%-z+9~SIG#_8Wpge^AKQn1WZe9X@0o+f6dLRI%}bzs zI?AV$p46C6w3k7CoBJ)v*z=8#Xzg?~@Aox73{o&bS!4On(I*RS=@9!$+5z0dh}#jH z;yaXPZHKZ2W7=YMhtlHNp;TzPvKz8#y>tci_tfu_)*yWnF$~{UfiIZcp;X7wUv$i8 z`VM7LV284V0V3~C#Cwz5l~HvC>^^Y?thzU@w3-Z_Tl zztj4Hv;BQHmyK~zG44O>qwx{)W!NGQa%s|h_~LHF&k&AcjS~Sy7 z`=an|EqqM={df45?0pT8d^-)1+x+}V-!I_T!|>-}`182PaW{7 z$L7;V;8SX|yWltXGSo@J&GO)jd7zNkZcz&xeUyl*spW%}ir zQ$EqY-H;*LcD<@A^_IWM&(yBFVK=pP!~0FIpR{c$`NpM|%;~3k^8;o+?RR)vG>)2j zue8y)O_~jYX17J-w9zEHxt=|+;b}qlyhXSBq>ub8eES+`9zd)>+=pmP|JH}HV-2d) znkVJAcK9tRF>fn;d>p;{U`QBzZ%8vB0SbL zuOROg%xjU3ZfPa-JpETU_UPS`@{jKkgfzwzF_C1dI`SE3i z3^L@$pzA2?n+uy7A9b@1$Z60L5?_bqFqOdv5!ih-+8eErguR zXS`w296#ZsvOJHuc&YZf^d`#Fg7Q#Vs7zlWonD^Aca~>;W_i9uy`elOA^Wc5ZuTu? z-H3Uj<=U;12WtkZ2U-KN{Co%56Etx$Yh#YlTDD{Kv8CRZb{qb^z_(+4L4DS&?<@zc zCEAY}{tX|y`MUk(A#BcE$THqFW>H^P-RX4_h2WqfqNa}JO#xSfSm7XyL ztY?MFjNg3eqcte4Es-Z1$4>J-TSz@CU5;{Z>^LDc1+XTIC$L9MD9vg@S>#D5i&;Wx zNn(!Eb>-f;uG|N|EDh)i&B?}vU-~pAsSo0r)+N~(OZvGrsT=az0)LW>2fy@vi02L0 z{z>Zm+0XLZfAoZNrBjpfd_ntE&7)+@65~BKNCo<@Z>)6XU&cp)rffW&G4!BVI;jdE}t)h9BU33G=`E1LMh_gTCeP>vrJVkM58LK^|Y*sYj)i@PT0*^r7Bm z)Vqu}AfxVO)V++lmr?gJ>Rv|O%cy%9buVXuKcpvk``eE)a9Nvirt{+er5xBNCk19LAXm?Ed5nA207SU--}#WDVn%;WTx`(Ki0u_S*CqAAjLvSht86VrL|-d)4oy zMc9vEZ*f&H*@bmwm(mo+{zU6i7OP!KiwAoa)}`E=Oepv131w*Q`Jej=sXm`Q!p{|H{OQJ-V@FP#^SZOf@V3+UJ7;0^ zD&Oj6rH{E;1KQ2w(r=xO(q`wNx7_SD)EoJr6Y_r9`kM45AlkxJ?Sph_C)(|^8}l8b zF{ja3m%w?DzxP%W&4Q!|->UF{GJ0-7M?ZhM)KGx4X@{bAKkSr*?ZI)Ikz+-%_<7p4*PkrU9M4NM=Zw*F!8#cTP7;Xjb(HlJz#hbni2vw#$TVigdI&mxGS$ zXx%ThfuHM%Yf?@#^Z;gq&v?w_`QS>@v$c+IdIV1rJa{(zCeKg2EdL|tmR>!h z_nBoWhmIdY2WyqG!P9mEW2*NnAFrFuA4^YB{oa9fSkkKB_{Wkl{b+CfQToQCeC^G7 zDD4HkZqe&VUv$*>!8I(uGO5dRoy(jq`0(W~us=fGigiD=pN*%`eo-$21E^=%GazLf z>Uhl;zFM@AsIJSrp3ydcib&(?e(>aOZj%Op&$#)puaov0j1L~xZBO{gKzHw&9_*?bOLgJ&^jjZ zmc+sry1o}?gSLNV_(!5|{8r?HcEeWW?~@$HiwAt#q^Ni9{mk(p*3Kk@uY-sm=Pg=E z)Vunq;eQ2dpVb&s{`{ojzi{8lDvVqCH@m1l9&&!;T;-%SjIs6KoHT9@Kdiu*)?5~j__dABvR|2N^?t~Ui#+%;JDKO&^OP}t81yyXQouSP^LLOF zMf(~Bz0a4q+0Pdgus^}BA;{Y~1Lu&iHD|mg{R`UPpdhP${U$fdK|K|KFXs@>wPvt6 z1!c(Z?+7^H=l;-{Ji(?82LGU$Y|bW>bI&GYN~&x)8B&0Ap)VHx^4mM+PD)5?K0O;U ze){dK!)_KhjJ(HjPHH@2%o;S<wvWL+9c`r@x}W`ze8+>(N$>GC z?R}Td9p#NkUqWO2|H^leA9&uI??^Q6KY+Z6k7KQi=%RUM^t;m4Ur04@uCFWaLFb6j7t$cK-5}aWlcwX`T~`)+bR~W}Q{PAagttGJ z3bC$nwV}1bhaEP$1*zs&#qO<7sU_z!d4mx)>{`j$vWEFw_;4+mx{Js&2&tRnt z>xg$D>-bxrN&6&Sz8veSjoJ%Nqvd0)iEvKA%l?^^lyrsGi?9RpHO|UZjO`wrm+9s^ zlINhusQTDvJfB`3lmAnx4Rfmp^LmIGQj?Cdpgb><-k~2#FHu>BeJD-XlZUe{yoZRR zoB>@CXG;@?Oka!qZp7BE&cucj9I8RJ-g zO>IrgQyHv@RD?N}v4#b;p3B2OK=gpTA{48st8L6>%;O0~8gBwNR99Ph)8$3iR(hHj z-6UULG^VnUeiYD9xNSOP>({OOhdXB6Ryw)tF3*fwlpE4(iaz{J3+|l#S6Eyd6Bx%{90VCFkHROVA7a)%f&CI6RdW@@f`Wq!F0zXN@VmNjP)M>WS;u zHBbCTo-*Cu2(|f&FI0630c(8_`{8tY>5P z7{)HZc+xOBD81J(HUkF!Vi+C=(gzHq9x(n1=l}zs8ipIgf%}MIR1-XA7$*SZCkR*XV~NAz8f0b^08deT@7re4vHnnqKzTnz#N13K zu`EP;z+lDhM2uVCv4u@G#!3h8azvNmeWMG#C}w|2ZG7A(}?jrQ(kqieQ^imm9Nh&d(w61Z$|ufWP~i;Ft}CP zGW4bVmoJdkW>|Wn^xM*trKd_y+wCRa+zFZPVW#{SkmjLuu%~QocKNy-@_|&Io0w{r zn#wxcasgHzvq6k^bZnQ!-zh$N3Jliz&t8+gHm7{O>&d~*t6eOPUW z>R})Ci7QO`-IiXon=z$pv+tx@pmr!lauc&f`?IyX0?=QEwwAQ`X?J1|B)=5-N38tE zMP}(>bt;LjZk5(%o9$du)0_BU(2hWC1AoVrrhS?HNQgTR&wJp!2hMxoya&#E;JgRU zd*HkW&U@gz2hMxoya&#E;D5pco2_$}fCa_zNG7EJjiVs&zcQ1lv~H!pXF>6Q)fZXm z91D&VY3u)}Y2QsjUf@^sH`AgVCoF#PKW0MzS;H((jzw=+ zX<-MB_IPC#)kQ+p`1evzL#ViDoJSrzX6$vv#l_bvif25|;KvB&?|}55 zp5mSYI|==d=~S!TrsJJVwC8kuR;pd6gd)$Y^r{aIh_#fe@e+F3gN04CaN zIv%~pYS-!bflRcAbi6Bd?wgJu#NyUDS2}($-D|-6bvk|s`?OCyPT|Bo4K@ZR{xd$O zPyA5USNn0&tpZm6wuxa*oQik!i6741>l1$gOZJIRjS4C8CFuWI?5L4KSPwKT^o6JM z#=h|QW}r{}RlrNk#Wq{#@<{Sfh+o3v1%t;Lf;gUDJ`i4v^BjqvA0aw$GlM9X;0bg5 zxqf~Kc&;}?{u)kyZhg^s+gG`MZOO?z=YQM6XP(!8ZQ(P|+3~*x-2Q`By<{Oxp&trD zF#klMOp6t&&Z@3q#%0Lw z9hXEq@mciA4EYfYpLx&2eH`CA?uqwZ_mg~lcak}7bXanP9RmKVz-Qe5@I2uq$hZ2n zu=5Tq5Qd`NX1=HUqa`QvzKgxU=Sk<*;~|UQlc5)bA@}o`41B(YS2OVU@p`;~vDWl< zd!h&9F*R8}IV_c9QFEb7NC8nqRTxH%(S=dX&?%?n2S;aFWmqd&Ny znT6`2^$~pi5iS~c{kZG;Ch}L};}PqEkc!$^18(MM2u3UX;RVs?BCyy}{4;e1PX#y=7iiHH({&|f*H`%)!Y~gKX3k<2k;<#-6Bu~M@4sbc$&AVV$+wpJ{YYWG z=bt>)LQXB6$^4~1zO`h=v`NS#mQ-=V=?H!Y3G+`WyKQ1gng6z1ZkaWCwtsfX#Inhd zN|&doD!-D%x^2Wiw|K1iDNeY8#4fb1G_k16i%a|!;b1HX_TDR1(xsSpqNMTgTU638 z=UkGKo;&R}+@BJzsr4^t#K%D5{**KtetAn8hTpYuMXCRakmV=vL%g%ZgNRO z_`?xDZ?3^u1>M<#=IgJo!L>Bztum@=yQpOXyoZy7uAB zU95=9!vl(8N<$5@B8+5Fj83+J`v38f&Lc2&VGU>ov5y1f!G8A(A&Ax7c@aL&|=! zZw2Im_AeB~IgijI;2h-98Hz}Y{VgC5nhCi5WT&v-LbED7kS_(l*arjh&}@Q~e!(xw ze-HABf1*W~+Amv#npI%3AG^+!e!;&GI3h`%TUqN)?5}z7WH^lWg%kvA!Lyw|ZmlB# z%`nP>77%;_+lmL`7v&fGbU+?(1koA5&D^Ct)EpO5jsqe`W5^F{t-EL{tk=3Lp52ZrZ~+9!6)D@pzZu( zzw(}%PH(1q{U_4z;(=rd`;(Ubq-s)lOpy8&dIjP`Jdpk|0%1YHKQ04Lv=r=6v|TgD z{2ftZ!g@;~okNo>pSn@rlf(R7G0lV%Ga9VWD`;tt+86&HW|$QF1)&8q`Tq~y CXtaX> literal 0 HcmV?d00001 diff --git a/types.hpp b/types.hpp index 1e228d8..71eb13b 100644 --- a/types.hpp +++ b/types.hpp @@ -65,8 +65,8 @@ constexpr int ENCODER_RIGHT_DIR_GPIO= 72; // 右编码器方向检测脚 constexpr int SERVO_PWMCHIP = 1; // 舵机 PWM 芯片 constexpr int SERVO_PWMID = 0; // 舵机 PWM 通道 constexpr int SERVO_PERIOD_NS = 3000000; // PWM 周期 3ms (333Hz) -constexpr int SERVO_CENTER_NS = 1520000; // 舵机中位 1.52ms -constexpr int SERVO_MAX_DELTA_NS = 210000; // 舵机偏离中位的最大行程 ±0.21ms +constexpr int SERVO_CENTER_NS = 1500000; // 舵机中位 1.50ms +constexpr int SERVO_MAX_DELTA_NS = 280000; // 舵机 PID 输出限幅 (不直接对应物理限位) // ========================================================= // IMU — JY62 六轴陀螺仪