rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
Lightvalve
Date:
Tue Dec 22 03:13:38 2020 +0000
Parent:
206:2e4d0c287578
Child:
208:408f9f15c486
Commit message:
201222-6-D gain add

Changed in this revision

CAN/function_CAN.h Show annotated file Show diff for this revision Revisions of this file
function_utilities/function_utilities.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
setting.h Show annotated file Show diff for this revision Revisions of this file
--- a/CAN/function_CAN.h	Tue Dec 22 00:44:04 2020 +0000
+++ b/CAN/function_CAN.h	Tue Dec 22 03:13:38 2020 +0000
@@ -225,6 +225,7 @@
 extern State vel;
 extern State Vout;
 extern State torq;
+extern State torq_dot;
 extern State pres_A;
 extern State pres_B;
 extern State cur;
--- a/function_utilities/function_utilities.cpp	Tue Dec 22 00:44:04 2020 +0000
+++ b/function_utilities/function_utilities.cpp	Tue Dec 22 03:13:38 2020 +0000
@@ -350,6 +350,7 @@
 float K_LPF = 0.0f;
 float D_LPF = 0.0f;
 
+float totq_sen_past = 0.0f;
 float torq_ref_past = 0.0f;
 float output_normalized = 0.0f;
 
--- 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;
--- a/setting.h	Tue Dec 22 00:44:04 2020 +0000
+++ b/setting.h	Tue Dec 22 03:13:38 2020 +0000
@@ -441,7 +441,8 @@
 extern float K_LPF;
 extern float D_LPF;
 
-extern float torq_ref_past ;
+extern float totq_sen_past;
+extern float torq_ref_past;
 extern float output_normalized;