![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2011
Diff: main.cpp
- Revision:
- 198:c70c5a9f17dd
- Parent:
- 197:2e4d0c287578
- Child:
- 199:408f9f15c486
--- a/main.cpp Tue Dec 22 00:44:04 2020 +0000 +++ b/main.cpp Tue Dec 22 03:13:38 2020 +0000 @@ -61,6 +61,7 @@ State vel; State Vout; State torq; +State torq_dot; State pres_A; State pres_B; State cur; @@ -2374,6 +2375,7 @@ // torque feedback torq.err = torq_ref - torq.sen; //[N] torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] + torq_dot.sen = (torq.sen-totq_sen_past)*(float) TMR_FREQ_5k; if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { @@ -2437,8 +2439,7 @@ float VALVE_POS_RAW_FORCE_FF = 0.0f; float VALVE_POS_RAW = 0.0f; - VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * 0.01f - + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref)); + VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * (torq.ref_diff - torq_dot.sen) / (float) TMR_FREQ_5k) * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref)); VALVE_POS_RAW_FORCE_FF = P_GAIN_JOINT_TORQUE_FF * torq_ref * 0.001f + D_GAIN_JOINT_TORQUE_FF * (torq_ref - torq_ref_past) * 0.0001f; @@ -2474,6 +2475,7 @@ } torq_ref_past = torq_ref; + totq_sen_past = torq.sen; break;