Diff: main.cpp

Revision:

46:2694daea349b

Parent:

45:35fa6884d0c6

Child:

47:fdcb8bd86fd6

--- a/main.cpp	Thu Jan 16 12:05:15 2020 +0000
+++ b/main.cpp	Mon Jan 20 08:05:21 2020 +0000
@@ -453,12 +453,14 @@
         if(MODE_POS_FT_TRANS == 1){
             alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f;
             cnt_trans++;
+            torq.err_sum = 0;
             if(cnt_trans>3*TMR_FREQ_5k)
                 MODE_POS_FT_TRANS = 2;
         }
         if(MODE_POS_FT_TRANS == 3){
             alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans/(float)(3*TMR_FREQ_5k)))/2.0f;
             cnt_trans++;
+            torq.err_sum = 0;
             if(cnt_trans>3*TMR_FREQ_5k)
                 MODE_POS_FT_TRANS = 0;
         }
@@ -497,9 +499,9 @@
                 
                 pos.err = pos.ref - pos.sen; //[pulse]
                 vel.err = vel.ref - vel.sen; //[pulse/s]
-                double K_spring = 1.0f; //[N/mm]
-                double D_damper = 0.0001f;
-                torq.ref = torq.ref + (K_spring * pos.err + D_damper * vel.err) / ENC_PULSE_PER_POSITION; //[N]
+                //double K_spring = 1.0f; //[N/mm]
+                //double D_damper = 0.0001f;
+                torq.ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
                 
                 // torque feedback
                 torq.err = torq.ref - torq.sen; //[pulse]