Revision 133:22ab22818e01, committed 2020-10-05

Comitter:

Lightvalve

Date:

Mon Oct 05 13:33:46 2020 +0000

Parent:

132:06e670a4f416

Child:

134:32e808423e70

Commit message:

201005

Changed in this revision

--- a/CAN/function_CAN.cpp	Sun Sep 27 07:42:10 2020 +0000
+++ b/CAN/function_CAN.cpp	Mon Oct 05 13:33:46 2020 +0000
@@ -739,7 +739,8 @@
         }
         
         torq.ref = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;   //N
-        
+        torq.ref_diff = torq.ref - torq.ref_old;
+        torq.ref_old = torq.ref;
         
         ///////////////Make Data///////////////////
         for(int i=0; i<num_array_u_past-1;i++){

--- a/function_utilities/function_utilities.cpp	Sun Sep 27 07:42:10 2020 +0000
+++ b/function_utilities/function_utilities.cpp	Mon Oct 05 13:33:46 2020 +0000
@@ -86,6 +86,14 @@
 
 int16_t STROKE;
 
+float Amm = 236.4f;
+float beta = 1300000000.0f;
+float Ps = 10000000.0f; //100bar = 100*10^5 Pa
+float Pt = 0.0f;    // 0bar = 0Pa
+//float Kv = 0.00000002635f;          // Q = Kv*xv*sqrt(Ps-Pa)    => 100bar full opening 5LPM    (full opening : xv = 1)  [unit] m^3.5/kg^0.5
+float gamma_hat = 1075.0f; // Kv*beta*A/(sqrt(2)*V)   0.00000002635f * 1300000000.0f *  / (sqrt(2.0f)*(1256.6f + 236.4f * 39.75f) * 0.000000001f / 2)     [unit] m^3.5/kg^0.5
+float V = 0.0000053f;           // (1256.6f + 236.4f * 39.75f) * 0.000000001f / 2
+float x_4_des_old = 0.0f;
 
 //int16_t VALVE_LIMIT_PLUS;
 //int16_t VALVE_LIMIT_MINUS;
@@ -465,6 +473,7 @@
 void ROM_CALL_DATA(void)
 {
     BNO = flashReadInt(Rom_Sector, RID_BNO);
+    BNO = 1;
     OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
 //    OPERATING_MODE = 5;
     SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE);

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

--- a/setting.h	Sun Sep 27 07:42:10 2020 +0000
+++ b/setting.h	Mon Oct 05 13:33:46 2020 +0000
@@ -180,6 +180,14 @@
 extern int16_t STROKE;
 
 
+extern float Amm;
+extern float beta;
+extern float Ps;
+extern float Pt;
+extern float gamma_hat;
+extern float V;
+extern float x_4_des_old;
+
 //extern int16_t VALVE_LIMIT_PLUS;
 //extern int16_t VALVE_LIMIT_MINUS;