![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: main.cpp
- Revision:
- 233:2ec0de8590d3
- Parent:
- 232:2976cf1b4252
- Child:
- 234:bd00f763acb1
--- a/main.cpp Mon May 31 07:19:49 2021 +0000 +++ b/main.cpp Wed Jun 02 03:14:34 2021 +0000 @@ -495,14 +495,16 @@ if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2; alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; cnt_trans++; - torq.err_sum = 0; + torq.err_int = 0.0f; + force.err_int = 0.0f; if((float)cnt_trans * DT_TMR3 > 3.0f) MODE_POS_FT_TRANS = 2; } else if(MODE_POS_FT_TRANS == 3) { if (alpha_trans == 0.0f) MODE_POS_FT_TRANS = 0; alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f; cnt_trans++; - torq.err_sum = 0; + torq.err_int = 0.0f; + force.err_int = 0.0f; if((float) cnt_trans * DT_TMR3 > 3.0f ) MODE_POS_FT_TRANS = 0; } else if(MODE_POS_FT_TRANS == 2) { @@ -799,12 +801,14 @@ float I_rem = I_REF - I_MAX; I_REF = I_MAX; float temp_vel_rem = K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary] - torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check! +// torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check! + torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE); } else if (I_REF < -I_MAX) { double I_rem = I_REF - (-I_MAX); I_REF = -I_MAX; float temp_vel_rem = K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary] - torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check! +/* torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check!*/ + torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE); } } break;