Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 255:1ba0cc8fee52, committed 2022-07-29
- Comitter:
- Lightvalve
- Date:
- Fri Jul 29 07:11:58 2022 +0000
- Parent:
- 254:9f487eaa87b5
- Child:
- 256:8586c11b4c30
- Commit message:
- force control vibration reduciing
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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 }