Diff: main.cpp

Revision:

133:22ab22818e01

Parent:

132:06e670a4f416

Child:

134: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;
             }