![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
distribution-201229
Diff: main.cpp
- Revision:
- 134:32e808423e70
- Parent:
- 133:22ab22818e01
- Child:
- 135:79885a39c161
diff -r 22ab22818e01 -r 32e808423e70 main.cpp --- a/main.cpp Mon Oct 05 13:33:46 2020 +0000 +++ b/main.cpp Tue Oct 06 05:25:47 2020 +0000 @@ -1565,10 +1565,11 @@ // 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); + V = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f - float f3 = -Amm*Amm*beta*0.000001f/V * vel.sen/(float)(ENC_PULSE_PER_POSITION); // unit : N/s + float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -1.05*10^-4 + float g3_prime = 0.0f; if (torq.sen > Amm*(Ps-Pt)*0.000001f){ g3_prime = 1.0f; @@ -1576,19 +1577,19 @@ g3_prime = -1.0f; } else { if ((value-VALVE_CENTER) > 0) { - g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f); + 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 K_valve = 0.00004f; - float x_v = 0.0f; + float x_v = 0.0f; //x_v : -1~1 if(value>=VALVE_CENTER) { - x_v = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); + x_v = 1.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); + x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); } float f4 = -x_v/tau; float g4 = K_valve/tau; @@ -1607,6 +1608,8 @@ float rho3 = 1.0f; float rho4 = 1.0f; float x_4_des = (-f3 + torq_ref_dot - k3*torq.err)/(gamma_hat*g3_prime); + if (x_4_des > 1) x_4_des = 1; + else if (x_4_des < -1) x_4_des = -1; float x_4_des_dot = (x_4_des - x_4_des_old)/(float) TMR_FREQ_5k; x_4_des_old = x_4_des;