Diff: main.cpp

Revision:

132:32e808423e70

Parent:

131:22ab22818e01

Child:

133: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;