![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2011
Diff: main.cpp
- Revision:
- 131:22ab22818e01
- Parent:
- 130:06e670a4f416
- Child:
- 132:32e808423e70
diff -r 06e670a4f416 -r 22ab22818e01 main.cpp --- a/main.cpp Sun Sep 27 07:42:10 2020 +0000 +++ b/main.cpp Mon Oct 05 13:33:46 2020 +0000 @@ -1559,6 +1559,62 @@ } case MODE_JOINT_CONTROL: { +// float Amm = 236.4f; +// float beta = 1300000000.0f; +// float Ps = 10000000.0f; //100bar = 100*10^5 Pa +// float Pt = 0.0f; // 0bar = 0Pa + float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3 + float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3 + V = 1.0f / (1.0f/Va + 1.0f/Vb); + + + float f3 = -Amm*Amm*beta*0.000001f/V * vel.sen/(float)(ENC_PULSE_PER_POSITION); // unit : N/s + float g3_prime = 0.0f; + if (torq.sen > Amm*(Ps-Pt)*0.000001f){ + g3_prime = 1.0f; + } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) { + g3_prime = -1.0f; + } else { + if ((value-VALVE_CENTER) > 0) { + g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f); + } else { + g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f); + } + } + float tau = 0.01f; + float K_valve = 0.4f; + + float x_v = 0.0f; + if(value>=VALVE_CENTER) { + x_v = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); + } else { + x_v = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); + } + float f4 = -x_v/tau; + float g4 = K_valve/tau; + + float torq_ref_dot = torq.ref_diff / (float) TMR_FREQ_5k; + + pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] + vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] + pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] + + torq.err = torq.ref - torq.sen; //[N] + torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] + + float k3 = 1.0f; + float k4 = 1.0f; + float rho3 = 1.0f; + float rho4 = 1.0f; + float x_4_des = (-f3 + torq_ref_dot - k3*torq.err)/(gamma_hat*g3_prime); + float x_4_des_dot = (x_4_des - x_4_des_old)/(float) TMR_FREQ_5k; + x_4_des_old = x_4_des; + + V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*torq.err)/g4; + + + /* + double torq_ref = 0.0f; pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] @@ -1675,6 +1731,11 @@ torq_ref_past = torq_ref; + + */ + + + break; }