Diff: main.cpp

Revision:

239:8ac5c6162bc1

Parent:

238:4e660aa77eb7

Child:

240:fb5b57e3f157

Child:

257:c93d3eabff75

diff -r 4e660aa77eb7 -r 8ac5c6162bc1 main.cpp
--- a/main.cpp	Tue Jul 13 01:40:56 2021 +0000
+++ b/main.cpp	Fri Jul 30 06:04:10 2021 +0000
@@ -526,6 +526,7 @@
 int cnt_trans = 0;
 double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
 int can_rest =0;
+float force_ref_act_can = 0.0f;
 
 extern "C" void TIM3_IRQHandler(void)
 {
@@ -1118,7 +1119,7 @@
                     VALVE_POS_CONTROL_DZ(valve_pos.ref);
                     V_out = Vout.ref;
                 } else if (CURRENT_CONTROL_MODE == 0) { //PWM
-                    V_out = valve_pos.ref;
+                    I_REF = valve_pos.ref;
                 } else {
                     I_REF = valve_pos.ref * 0.001f; // Unit : pulse >> mA
                     float I_MAX = 10.0f; // Max : 10mA
@@ -1164,8 +1165,12 @@
                     temp_vel_FT = 0.001f * (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.err = force_ref_act - force.sen;
-//                    force.err = 0.0f - force.sen;
+                    //////////////////////////////////////////////////force_reference_filter////////////////////////////////////////////////////////////////////
+                    float alpha_torque_ref = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*1.0f));
+                    force_ref_filter = (1.0f-alpha_torque_ref) * force_ref_filter + alpha_torque_ref * force_ref_act;
+                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+                    force_ref_act_can = force_ref_filter;
+                    force.err = force_ref_filter - 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
                 }
@@ -1367,7 +1372,7 @@
                 // Moog Valve Current Control Gain
                 double R_model = 500.0f; // ohm
                 double L_model = 1.2f;
-                double w0 = 2.0f * 3.14f * 150.0f;
+                double w0 = 2.0f * 3.14f * 100.0f;
                 double KP_I = 0.1f * L_model*w0;
                 double KI_I = 0.1f * R_model*w0;
 
@@ -1478,8 +1483,8 @@
 
             // Valve Position (ID:1300)
             if (flag_data_request[1] == HIGH) {
-//                CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
-                CAN_TX_PWM((int16_t)(alpha3));
+                CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
+//                CAN_TX_PWM((int16_t)(alpha3));
             }
 
             // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
@@ -1499,7 +1504,7 @@
 
                 valve_pos_ref_can = (float)valve_pos.ref;
 
-                CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_ref_can);
+                CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) force_ref_act_can);
             }
 
             TMR2_COUNT_CAN_TX = 0;