Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 239:8ac5c6162bc1, committed 2021-07-30
- Comitter:
- Lightvalve
- Date:
- Fri Jul 30 06:04:10 2021 +0000
- Parent:
- 238:4e660aa77eb7
- Child:
- 240:fb5b57e3f157
- Child:
- 257:c93d3eabff75
- Commit message:
- 210730
Changed in this revision
--- a/SPI_EEP_ENC/SPI_EEP_ENC.h Tue Jul 13 01:40:56 2021 +0000 +++ b/SPI_EEP_ENC/SPI_EEP_ENC.h Fri Jul 30 06:04:10 2021 +0000 @@ -91,7 +91,7 @@ #define RID_VALVE_POS_NUM 54 //#define RID_DDV_CENTER 55 #define RID_VALVE_CENTER_OFFSET 56 -#define RID_VALVE_ELECTRIC_CENTER 57 +#define RID_VALVE_ELECTRIC_CENTER 59 #define RID_VALVE_GAIN_PLUS_1 60 #define RID_VALVE_GAIN_MINUS_1 61
--- a/function_utilities/function_utilities.cpp Tue Jul 13 01:40:56 2021 +0000 +++ b/function_utilities/function_utilities.cpp Fri Jul 30 06:04:10 2021 +0000 @@ -41,8 +41,8 @@ float I_GAIN_JOINT_TORQUE_FF = 0.0f; float D_GAIN_JOINT_TORQUE_FF = 0.0f; -int16_t K_SPRING = 0.0; -int16_t D_DAMPER = 0.0; +int16_t K_SPRING = 0; +int16_t D_DAMPER = 0; int16_t flag_delay_test = 0; @@ -242,6 +242,7 @@ float joint_pos_err = 0.0f, joint_pos_err_old = 0.0f, joint_pos_err_diff = 0.0f, joint_pos_err_diff_fil = 0.0f, joint_pos_err_sum = 0.0f; float joint_torq_err = 0.0f, joint_torq_err_old = 0.0f, joint_torq_err_diff = 0.0f, joint_torq_err_sum = 0.0f; float VALVE_PWM_RAW_POS = 0.0f, VALVE_PWM_RAW_TORQ = 0.0f; +float force_ref_filter = 0.0f; float CUR_FLOWRATE = 0.0f; float VALVE_FF_VOLTAGE = 0.0f;
--- 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;
--- a/setting.h Tue Jul 13 01:40:56 2021 +0000 +++ b/setting.h Fri Jul 30 06:04:10 2021 +0000 @@ -335,6 +335,7 @@ extern float joint_pos_err, joint_pos_err_old, joint_pos_err_diff, joint_pos_err_diff_fil, joint_pos_err_sum; extern float joint_torq_err, joint_torq_err_old, joint_torq_err_diff, joint_torq_err_sum; extern float VALVE_PWM_RAW_POS, VALVE_PWM_RAW_TORQ; +extern float force_ref_filter; extern float CUR_FLOWRATE; extern float VALVE_FF_VOLTAGE;