Diff: main.cpp

Revision:

207:c70c5a9f17dd

Parent:

206:2e4d0c287578

Child:

208: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;