Diff: main.cpp

Revision:

63:a3c7f31742c9

Parent:

62:851cf7b7aa7a

Child:

64:dc5b5c258579

diff -r 851cf7b7aa7a -r a3c7f31742c9 main.cpp
--- a/main.cpp	Tue May 12 06:23:22 2020 +0000
+++ b/main.cpp	Fri May 15 03:25:15 2020 +0000
@@ -76,6 +76,8 @@
 extern int CID_TX_VOUT;
 extern int CID_TX_VALVE_POSITION;
 
+float I_REF_fil_DZ = 0.0f;
+
 // =============================================================================
 // =============================================================================
 // =============================================================================
@@ -1300,7 +1302,8 @@
             case MODE_JOINT_CONTROL: {
                 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]
+                vel.err = (vel.ref - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
+//                vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
                 pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
                 
                 //K & D Low Pass Filter
@@ -1311,7 +1314,7 @@
                 torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N]
 
                 // torque feedback
-                torq.err = (torq_ref)/(float)(TORQUE_SENSOR_PULSE_PER_TORQUE)  - torq.sen; //[N]
+                torq.err = torq_ref - torq.sen; //[N]
                 torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
 
                 if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
@@ -1423,14 +1426,9 @@
             ////////////////////////////////////////////////////////////////////////////
             ////////////////////////////  CURRENT CONTROL //////////////////////////////
             ////////////////////////////////////////////////////////////////////////////
+            
             if (CURRENT_CONTROL_MODE) {
-                double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
-                I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
-
-                I_ERR = I_REF_fil - cur.sen;
-                I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
-
-
+                
                 // Moog Valve Current Control Gain
                 double R_model = 500.0f; // ohm
                 double L_model = 1.2f;
@@ -1446,15 +1444,34 @@
                     KP_I = 1.0f * L_model*w0;
                     KI_I = 0.08f * R_model*w0;
                 }
+                
+                double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
+                I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
+                
+                FLAG_VALVE_DEADZONE = 1;
+                
+                if (FLAG_VALVE_DEADZONE) {
+                    if (I_REF_fil > 0) I_REF_fil_DZ = I_REF_fil + VALVE_DEADZONE_PLUS / 1000.0f; // unit: mA
+                    else if (I_REF_fil < 0) I_REF_fil_DZ = I_REF_fil + VALVE_DEADZONE_MINUS / 1000.0f; // unit: mA
+                    else I_REF_fil_DZ = I_REF_fil + (VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f / 1000.0f; // unit: mA
+                } else {
+                    I_REF_fil_DZ = I_REF_fil;
+                }
+
+                I_ERR = I_REF_fil_DZ - cur.sen;
+                I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
+
+
+                
 
                 double FF_gain = 1.0f;
 
                 VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT;
                 //        VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model*I_REF); // Unit : mV
-                I_REF_fil_diff = I_REF_fil - I_REF_fil_old;
-                I_REF_fil_old = I_REF_fil;
-//                VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV
-                VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil); // Unit : mV
+                I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old;
+                I_REF_fil_old = I_REF_fil_DZ;
+//                VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV
+                VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV
                 double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
 
                 double Ka = 3.0f / KP_I;
@@ -1479,15 +1496,25 @@
             /////////////////  Dead Zone Cancellation & Linearization //////////////////
             ////////////////////////////////////////////////////////////////////////////
             // Dead Zone Cancellation (Mechanical Valve dead-zone)
-            if (FLAG_VALVE_DEADZONE) {
-                if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
-                else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
+//            if (FLAG_VALVE_DEADZONE) {
+//                if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
+//                else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
+//                VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
+//            } else {
+//                VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
+//            }
 
-                VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
-
-            } else {
+            // VALVE_DEADZONE_PLUS : Current[mA] to start extraction
+            // VALVE_DEADZONE_MINUS : Current[mA] to start contraction
+//            double R_model = 500.0f;
+//            FLAG_VALVE_DEADZONE = true;
+//            if (FLAG_VALVE_DEADZONE) {
+//                if (VALVE_PWM_RAW > 0) VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * R_model / 1000.0f; // unit: mV
+//                else if (VALVE_PWM_RAW < 0) VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * R_model / 1000.0f; // unit: mV
+//                else VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f* R_model / 1000.0f;
+//            } else {
                 VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
-            }
+//            }
 
             // Output Voltage Linearization
             double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV
@@ -1546,7 +1573,7 @@
                     }
                 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
                     if (SENSING_MODE == 0) {
-                        CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen));
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
                     } else if (SENSING_MODE == 1) {
                         CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
                     }
@@ -1567,8 +1594,8 @@
                 } else {
                     t_value = V_out;
                 }
-                CAN_TX_TORQUE((int16_t) (t_value)); //1300
-                //CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300
+//                CAN_TX_TORQUE((int16_t) (I_REF_fil_DZ * 1000.0f)); //1300
+                CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300
                 //CAN_TX_TORQUE((int16_t) (I_REF * 1000.0f)); //1300
             }