Diff: main.cpp

Revision:

255:1ba0cc8fee52

Parent:

254:9f487eaa87b5

Child:

256:8586c11b4c30

--- a/main.cpp	Thu Jul 28 09:22:00 2022 +0000
+++ b/main.cpp	Fri Jul 29 07:11:58 2022 +0000
@@ -270,7 +270,7 @@
         dac_1 = PRES_A_VREF / 3.3f;
         dac_2 = PRES_B_VREF / 3.3f;
     }
-    
+
     make_delay();
 
     for (int i=0; i<50; i++) {
@@ -485,7 +485,7 @@
             HAL_ADC_PollForConversion(&hadc1, 1);
             PSEN2 = (float) HAL_ADC_GetValue(&hadc1);
 
-            force.UpdateSen((((float)PSEN1) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR3, 100.0f); // unit : N
+            force.UpdateSen((((float)PSEN1) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR3, 100.0f); // unit : N //100Hz
 
         } else if (SENSING_MODE == 1) { // Pressure sensing
 
@@ -506,10 +506,10 @@
 
             if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
                 float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
-                torq.UpdateSen(torq_new,FREQ_TMR3,1000.0f);  // unit : Nm
+                torq.UpdateSen(torq_new,FREQ_TMR3,100.0f);  // unit : Nm   //1000Hz
             } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
                 float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
-                force.UpdateSen(force_new,FREQ_TMR3,1000.0f);  // unit : N
+                force.UpdateSen(force_new,FREQ_TMR3,100.0f);  // unit : N  //1000Hz
             }
         }
         CNT_TMR3++;
@@ -1215,8 +1215,10 @@
                 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode
                     float torq_ref_act = torq.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : Nm
                     torq.err = torq_ref_act - torq.sen;
-                    torq.err_int += torq.err/((float)TMR_FREQ_5k);
-                    temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s
+                    if (torq.err > 10.0f || torq.err < -10.0f) {
+                        torq.err_int += torq.err/((float)TMR_FREQ_5k);
+                    }
+                    temp_vel_FT = 0.01f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s
                 } else {
                     float force_ref_act = force.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : N
                     //////////////////////////////////////////////////force_reference_filter////////////////////////////////////////////////////////////////////
@@ -1224,8 +1226,10 @@
 //                    force_ref_filter = (1.0f-alpha_torque_ref) * force_ref_filter + alpha_torque_ref * force_ref_act;
                     ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                     force.err = force_ref_act - force.sen;
-                    force.err_int += force.err/((float)TMR_FREQ_5k);
-                    temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s
+                    if (force.err > 10.0f || force.err < -10.0f) {
+                        force.err_int += force.err/((float)TMR_FREQ_5k);
+                    }
+                    temp_vel_FT = 0.01f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s
                 }