Sungwoo Kim
/
HydraulicControlBoard_PostLIGHT_210420
LIGHT2
Diff: main.cpp
- Revision:
- 259:5da820cc9a1a
- Parent:
- 258:c16774af92ed
- Child:
- 260:bbb74caca589
diff -r c16774af92ed -r 5da820cc9a1a main.cpp --- a/main.cpp Wed Jul 07 10:09:11 2021 +0000 +++ b/main.cpp Fri Jul 09 02:07:56 2021 +0000 @@ -759,8 +759,8 @@ // torque control command =============================================================================================================================================== float alpha_SpringDamper = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*30.0f)); - K_LPF = alpha_SpringDamper * K_LPF + (1.0f-alpha_SpringDamper) * K_SPRING; - D_LPF = alpha_SpringDamper * D_LPF + (1.0f-alpha_SpringDamper) * D_DAMPER; + K_LPF = (1.0f-alpha_SpringDamper) * K_LPF + alpha_SpringDamper * K_SPRING; + D_LPF = (1.0f-alpha_SpringDamper) * D_LPF + alpha_SpringDamper * D_DAMPER; if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode float torq_ref_act = torq.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : Nm