Diff: main.cpp

Revision:

137:ccf70b9b1705

Parent:

136:7e66fc71af40

Child:

138:a843f32ced33

--- a/main.cpp	Tue Oct 06 11:29:47 2020 +0000
+++ b/main.cpp	Tue Oct 06 12:06:45 2020 +0000
@@ -1559,16 +1559,16 @@
             }
 
             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
+                
+//                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
+                
+                float Va = (1256.6f + Amm * 39.5f) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x,      unit : m^3
+                float Vb = (1256.6f + Amm  * 39.5f) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x),      unit : m^3
                 V = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f
     
                 
-                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 f3 = -Amm*Amm*beta*0.000001f*0.000001f/V * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s    //xdot=10mm/s일때 137076
                 
                 float g3_prime = 0.0f;
                 if (torq.sen > Amm*(Ps-Pt)*0.000001f){
@@ -1583,8 +1583,8 @@
                     }
                 }
                 float tau = 0.01f;
-//                float K_valve = 0.00004f;
-                float K_valve = 0.4f;
+                float K_valve = 0.00004f;
+//                float K_valve = 0.4f;
                 
                 float x_v = 0.0f;   //x_v : -1~1
                 if(value>=VALVE_CENTER) {
@@ -1595,7 +1595,7 @@
                 float f4 = -x_v/tau;
                 float g4 = K_valve/tau;
                 
-                float torq_ref_dot = torq.ref_diff / (float) TMR_FREQ_5k;
+                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]
@@ -1604,21 +1604,21 @@
                 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 k3 = 10.0f;
+                float k4 = 10000.0f;
                 float rho3 = 1.0f;
-                float rho4 = 1.0f;
+                float rho4 = 0.0001f;
                 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;
+                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;
                 
                 float rho_gamma = 1.0f;
                 float gamma_hat_dot = rho3*torq.err/rho_gamma*((-f3+torq_ref_dot-k3*torq.err)/gamma_hat + g3_prime*(x_v-x_4_des));
-                gamma_hat = gamma_hat + gamma_hat_dot * (float) TMR_FREQ_5k;
+                gamma_hat = gamma_hat + gamma_hat_dot / (float) TMR_FREQ_5k;
                 
                 
                 /*