Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_copy1
2011
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;