rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
hyhoon
Date:
Thu Mar 11 07:27:23 2021 +0000
Parent:
226:82a3ca333004
Child:
228:763bbefcb79f
Commit message:
210311

Changed in this revision

CAN/function_CAN.cpp Show annotated file Show diff for this revision Revisions of this file
CAN/function_CAN.h Show annotated file Show diff for this revision Revisions of this file
function_utilities/function_utilities.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
setting.h Show annotated file Show diff for this revision Revisions of this file
--- a/CAN/function_CAN.cpp	Wed Mar 03 10:56:25 2021 +0000
+++ b/CAN/function_CAN.cpp	Thu Mar 11 07:27:23 2021 +0000
@@ -8,13 +8,13 @@
 // CAN ID Setting Variables
 int CID_RX_CMD = 100;
 int CID_RX_REF_POSITION = 200;
-int CID_RX_REF_VALVE_POS = 300;
+int CID_RX_REF_OPENLOOP = 300;
 int CID_RX_REF_PWM = 400;
 
 int CID_TX_INFO = 1100;
-int CID_TX_POSITION = 1200;
-int CID_TX_TORQUE = 1300;
-int CID_TX_PRES = 1400;
+int CID_TX_POS_VEL_TORQ = 1200;
+int CID_TX_PWM = 1300;
+int CID_TX_CURRENT = 1400;
 int CID_TX_VOUT = 1500;
 int CID_TX_VALVE_POSITION = 1600;
 int CID_TX_SOMETHING = 1700;
@@ -39,13 +39,13 @@
 
     CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD);
     CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION);
-    CID_RX_REF_VALVE_POS = (int) (BNO + INIT_CID_RX_REF_VALVE_POS);
+    CID_RX_REF_OPENLOOP = (int) (BNO + INIT_CID_RX_REF_OPENLOOP);
     CID_RX_REF_PWM = (int) (BNO + INIT_CID_RX_REF_PWM);
 
     CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO);
-    CID_TX_POSITION = (int) (BNO + INIT_CID_TX_POSITION);
-    CID_TX_TORQUE = (int) (BNO + INIT_CID_TX_TORQUE);
-    CID_TX_PRES = (int) (BNO + INIT_CID_TX_PRES);
+    CID_TX_POS_VEL_TORQ = (int) (BNO + INIT_CID_TX_POS_VEL_TORQ);
+    CID_TX_PWM = (int) (BNO + INIT_CID_TX_PWM);
+    CID_TX_CURRENT = (int) (BNO + INIT_CID_TX_CURRENT);
     CID_TX_VOUT = (int) (BNO + INIT_CID_TX_VOUT);
     CID_TX_VALVE_POSITION = (int) (BNO + INIT_CID_TX_VALVE_POSITION);
     CID_TX_SOMETHING = (int) (BNO + INIT_CID_TX_SOMETHING);
@@ -140,6 +140,7 @@
 //            if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
             //if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3;
 
+            
             break;
         }
 
@@ -638,84 +639,54 @@
         int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
 
         if((OPERATING_MODE&0b001)==0) { // Rotary Actuator
-            pos.ref = (double)temp_pos * 10.0f;
-            vel.ref = (double)temp_vel * 256.0f;
+            REF_POSITION = (double)temp_pos * 10.0f;
+            REF_VELOCITY = (double)temp_vel * 256.0f;
         } else { //Linear Actuator
-            pos.ref = (double)temp_pos * 10.0f;
-            vel.ref = (double)temp_vel * 256.0f;
+            REF_POSITION = (double)temp_pos * 10.0f;
+            REF_VELOCITY = (double)temp_vel * 256.0f;
         }
 
-        torq.ref = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;   //N
-        torq.ref_diff = torq.ref - torq.ref_old;
-        torq_dot.sen = torq.sen-torq_sen_past;
-        torq.ref_old = torq.ref;
-        torq_sen_past = torq.sen;
+        REF_TORQUE = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;   //N
+        torq.ref_diff = REF_TORQUE - REF_TORQUE_OLD;
+        REF_TORQUE_OLD = REF_TORQUE;
 
         if(CAN_FREQ == -1) {
             // Position, Velocity, and Torque (ID:1200)
             if (flag_data_request[0] == HIGH) {
                 if ((OPERATING_MODE & 0b01) == 0) { // Rotary 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*10.0f));
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
                     } 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)*10.0f), (int16_t) ((pres_B.sen)*10.0f));
+                        CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
                     }
                 } 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 * 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)*10.0f), (int16_t) ((pres_B.sen)*10.0f));
+                        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));
                     }
                 }
             }
 
-            // ID:1300
+            // PWM, ID:1300
             if (flag_data_request[1] == HIGH) {
-                CAN_TX_TORQUE((int16_t) (pos.ref/10.0f)); //1300
+                CAN_TX_PWM((int16_t) (V_out)); //1300
             }
 
-            // Reference Current, Current Current (ID:1700)
+            // Reference Current, Current Current (ID:1400)
             if (flag_data_request[2] == HIGH) {
-
-                CAN_TX_SOMETHING((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t)HOMEPOS_OFFSET, (int16_t) (I_ERR / mA_PER_pulse));
-                //double t_value = 0.0f;
-//            if(value>=(float) VALVE_CENTER) {
-//                t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-//            } else {
-//                t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-//            }
-//            double t_value_ref = 0.0f;
-//            if(valve_pos.ref>=(float) VALVE_CENTER) {
-//                t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-//            } else {
-//                t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-//            }
-//
-//
-//            CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
+                CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400
             }
-
-            //If it doesn't rest, below can can not work.
-            for (int can_rest2 = 0; can_rest2 < 10000; can_rest2++) {
-                ;
-            }
-
-            // ID:1500
+            
+            // Reference Current, Current Current (ID:1700)
             if (flag_data_request[3] == HIGH) {
-                //PWM
-                CAN_TX_PWM((int16_t) (torq.ref)); //1500
-            }
-
-            // ID:1600
-            if (flag_data_request[4] == HIGH) {
-                //valve position
-                CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
+                CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700
             }
 
         }
 
 
-    } else if(address==CID_RX_REF_VALVE_POS) {
+    } else if(address==CID_RX_REF_OPENLOOP) {
         int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
 
         if (((OPERATING_MODE&0b110)>>1) == 0) { //Moog Valve
@@ -1336,7 +1307,7 @@
 {
     CANMessage temp_msg;
 
-    temp_msg.id = CID_TX_POSITION;
+    temp_msg.id = CID_TX_POS_VEL_TORQ;
     temp_msg.len = 6;
     temp_msg.data[0] = (uint8_t) t_pos;
     temp_msg.data[1] = (uint8_t) (t_pos >> 8);
@@ -1353,7 +1324,7 @@
 
     CANMessage temp_msg;
 
-    temp_msg.id = CID_TX_POSITION;
+    temp_msg.id = CID_TX_POS_VEL_TORQ;
     temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) t_pos;
     temp_msg.data[1] = (uint8_t) (t_pos >> 8);
@@ -1367,24 +1338,11 @@
     can.write(temp_msg);
 }
 
-
-void CAN_TX_TORQUE(int16_t t_torque)
+void CAN_TX_CURRENT(int16_t t_pres_a, int16_t t_pres_b)
 {
     CANMessage temp_msg;
 
-    temp_msg.id = CID_TX_TORQUE;
-    temp_msg.len = 2;
-    temp_msg.data[0] = (uint8_t) t_torque;
-    temp_msg.data[1] = (uint8_t) (t_torque >> 8);
-
-    can.write(temp_msg);
-}
-
-void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b)
-{
-    CANMessage temp_msg;
-
-    temp_msg.id = CID_TX_PRES;
+    temp_msg.id = CID_TX_CURRENT;
     temp_msg.len = 4;
     temp_msg.data[0] = (uint8_t) t_pres_a;
     temp_msg.data[1] = (uint8_t) (t_pres_a >> 8);
@@ -1398,7 +1356,7 @@
 {
     CANMessage temp_msg;
 
-    temp_msg.id = CID_TX_VOUT;
+    temp_msg.id = CID_TX_PWM;
     temp_msg.len = 2;
     temp_msg.data[0] = (uint8_t) t_pwm;
     temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
--- a/CAN/function_CAN.h	Wed Mar 03 10:56:25 2021 +0000
+++ b/CAN/function_CAN.h	Thu Mar 11 07:27:23 2021 +0000
@@ -9,13 +9,13 @@
 // INIT_CID
 #define INIT_CID_RX_CMD                   100
 #define INIT_CID_RX_REF_POSITION          200
-#define INIT_CID_RX_REF_VALVE_POS       300
+#define INIT_CID_RX_REF_OPENLOOP       300
 #define INIT_CID_RX_REF_PWM            400
 
 #define INIT_CID_TX_INFO              1100
-#define INIT_CID_TX_POSITION          1200
-#define INIT_CID_TX_TORQUE            1300
-#define INIT_CID_TX_PRES              1400
+#define INIT_CID_TX_POS_VEL_TORQ      1200
+#define INIT_CID_TX_PWM            1300
+#define INIT_CID_TX_CURRENT              1400
 #define INIT_CID_TX_VOUT              1500
 #define INIT_CID_TX_VALVE_POSITION    1600
 #define INIT_CID_TX_SOMETHING         1700
@@ -150,9 +150,9 @@
 // Sensor & State Transmission
 void CAN_TX_POSITION_FT(int16_t t_pos, int16_t t_vel, int16_t t_torq);
 void CAN_TX_POSITION_PRESSURE(int16_t t_pos, int16_t t_vel, int16_t t_pa, int16_t t_pb);
-//void CAN_TX_TORQUE(int16_t t_valve_pos, int16_t t_vout);
-void CAN_TX_TORQUE(int16_t t_valve_pos);
-void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b);
+//void CAN_TX_PWM(int16_t t_valve_pos, int16_t t_vout);
+void CAN_TX_PWM(int16_t t_valve_pos);
+void CAN_TX_CURRENT(int16_t t_pres_a, int16_t t_pres_b);
 void CAN_TX_PWM(int16_t t_pwm);
 void CAN_TX_VALVE_POSITION(int16_t t_valve_pos_1, int16_t t_valve_pos_2, int16_t t_ref_valve_pos, int16_t t_pwm);
 void CAN_TX_SOMETHING (int16_t t_a, int16_t t_b, int16_t t_c, int16_t t_d);
--- a/function_utilities/function_utilities.cpp	Wed Mar 03 10:56:25 2021 +0000
+++ b/function_utilities/function_utilities.cpp	Thu Mar 11 07:27:23 2021 +0000
@@ -17,7 +17,7 @@
 uint8_t CONTROL_UTILITY_MODE = 0;
 uint8_t CURRENT_CONTROL_MODE = 0; // (0 : pwm, 1 : current control)
 uint8_t FLAG_VALVE_DEADZONE = 0;
-uint8_t REFERENCE_MODE = 0;
+uint8_t REFERENCE_MODE = 1;
 int16_t CAN_FREQ = 500;
 int16_t DIR_JOINT_ENC = 0;
 int16_t DIR_VALVE = 0;
@@ -113,10 +113,11 @@
 float DAC_REF;
 float DAC_RESOL;
 
-int REF_POSITION;
-int REF_VELOCITY;
-int16_t REF_TORQUE;
-int16_t REF_PRES_DIFF;
+float REF_POSITION;
+float REF_VELOCITY;
+float REF_TORQUE;
+float REF_TORQUE_OLD;
+float REF_PRES_DIFF;
 int16_t REF_PWM;
 int16_t REF_VALVE_POSITION;
 int16_t REF_CURRENT;
@@ -287,6 +288,20 @@
 int FINDHOME_POSITION = 0;
 int FINDHOME_POSITION_OLD = 0;
 
+int cnt_finddz = 0;
+int cnt_vel_finddz = 0;
+int flag_finddz = 0;
+int FINDDZ_VELOCITY = 0;
+int FINDDZ_VELOCITY_OLD = 0;
+int FINDDZ_POSITION = 0;
+int FINDDZ_POSITION_OLD = 0;
+
+double temp_VALVE_DEADZONE_PLUS = 0.0f;
+double temp_VALVE_DEADZONE_MINUS = 0.0f;
+float temp_pos_ref = 0.0f;
+float temp_pos_ref_offset = 0.0f;
+
+
 // valve gain
 int check_vel_pos_init = 0;
 int check_vel_pos_fin = 0;
@@ -328,6 +343,12 @@
 int FINDHOME_GOTOLIMIT = 1;
 int FINDHOME_ZEROPOSE = 2;
 
+int FINDDZ_STAGE = 0;
+int FINDDZ_INIT = 0;
+int FINDDZ_START1 = 1;
+int FINDDZ_START2 = 2;
+int FINDDZ_STOP = 3;
+
 float alpha_trans = 0.0f;
 
 float V_out=0.0f;
@@ -389,7 +410,6 @@
     for (i = 0; i < 1000000; i++) {
         ;
     }
-
 }
 
 
--- a/main.cpp	Wed Mar 03 10:56:25 2021 +0000
+++ b/main.cpp	Thu Mar 11 07:27:23 2021 +0000
@@ -79,13 +79,13 @@
 
 extern int CID_RX_CMD;
 extern int CID_RX_REF_POSITION;
-extern int CID_RX_REF_VALVE_POS;
+extern int CID_RX_REF_OPENLOOP;
 extern int CID_RX_REF_PWM;
 
 extern int CID_TX_INFO;
-extern int CID_TX_POSITION;
-extern int CID_TX_TORQUE;
-extern int CID_TX_PRES;
+extern int CID_TX_POS_VEL_TORQ;
+extern int CID_TX_PWM;
+extern int CID_TX_CURRENT;
 extern int CID_TX_VOUT;
 extern int CID_TX_VALVE_POSITION;
 extern int CID_TX_SOMETHING;
@@ -93,6 +93,13 @@
 float pres_A_new = 0.0f;
 float pres_B_new = 0.0f;
 
+
+
+float temp_P_GAIN = 0.0f;
+float temp_I_GAIN = 0.0f;
+int temp_VELOCITY_COMP_GAIN = 0;
+
+
 /*******************************************************************************
  *  REFERENCE MODE
  ******************************************************************************/
@@ -213,12 +220,17 @@
 //    make_delay();
 
     // spi init
-    enc_cs = 1;
+    eeprom_cs = 1;
     eeprom.format(8,3);
     eeprom.frequency(5000000); //5M
+    eeprom_cs = 0;
+    make_delay();
+
+    enc_cs = 1;     //sw add
     enc.format(8,0);
-    enc.frequency(5000000); //5M
-    enc_cs = 0;
+    enc.frequency(5000000); //10M
+    enc_cs = 0;     //sw add
+
     make_delay();
 
 
@@ -302,10 +314,10 @@
 //        timer_while ++;
 
         //i2c for SW valve
-        if(OPERATING_MODE == 5) {
-            read_field(i2c_slave_addr1);
-            if(DIR_VALVE_ENC < 0) value = 1023 - value;
-        }
+        //if(OPERATING_MODE == 5) {
+//            read_field(i2c_slave_addr1);
+//            if(DIR_VALVE_ENC < 0) value = 1023 - value;
+//        }
     }
 }
 
@@ -526,6 +538,22 @@
         }
 
 
+// Reference Loop ==========================================================
+        switch (REFERENCE_MODE) {
+            case MODE_REF_NO_ACT: {
+                break;
+            }
+
+            case MODE_REF_DIRECT: {
+                pos.ref = REF_POSITION;
+                vel.ref = REF_VELOCITY;
+                torq.ref = REF_TORQUE;
+                break;
+            }
+            default:
+                break;
+        }
+
         int UTILITY_MODE = 0;
         int CONTROL_MODE = 0;
 
@@ -577,6 +605,7 @@
 
             case MODE_FIND_HOME: {
                 if (FINDHOME_STAGE == FINDHOME_INIT) {
+                    REFERENCE_MODE=MODE_REF_NO_ACT;
                     cnt_findhome = 0;
                     cnt_vel_findhome = 0;
                     pos.ref = pos.sen;
@@ -633,50 +662,11 @@
                     vel.ref = 0.0f;
                     alpha_trans = 0.0f;
 
-                    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]
                     pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
                     CONTROL_MODE = MODE_JOINT_CONTROL;
 
-                    if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
-
-                        double I_REF_POS = 0.0f;
-                        double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
-                        double I_REF_VC = 0.0f; // I_REF for velocity compensation
-
-                        double temp_vel_pos = 0.0f;
-                        double temp_vel_torq = 0.0f;
-                        double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
-
-                        if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
-                            temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * 3.14159f / 180.0f; // rad/s
-                            //                            L when P-gain = 100, f_cut = 10Hz                                 L feedforward velocity
-                        } else if ((OPERATING_MODE & 0x01) == 1) {
-                            temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
-                            //                            L when P-gain = 100, f_cut = 10Hz                                 L feedforward velocity
-                        }
-                        if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
-                        else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
-
-                        I_REF = I_REF_POS;
-
-                    } else {
-                        float VALVE_POS_RAW_FORCE_FB = 0.0f;
-                        VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
-
-                        if (VALVE_POS_RAW_FORCE_FB >= 0) {
-                            valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
-                        } else {
-                            valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
-                        }
-
-                        VALVE_POS_CONTROL(valve_pos.ref);
-
-                        V_out = (float) Vout.ref;
-
-                    }
-
                     cnt_findhome++;
                     if (cnt_findhome >= T_move) {
                         cnt_findhome = 0;
@@ -686,83 +676,13 @@
                         vel.ref_home_pos = 0.0f;
                         FINDHOME_STAGE = FINDHOME_INIT;
                         CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
+                        REFERENCE_MODE=MODE_REF_DIRECT;
                     }
                 }
 
                 break;
             }
 
-//            case MODE_VALVE_GAIN_SETTING: {
-//                if (TMR3_COUNT_FLOWRATE == 0) {
-//                    if (pos_plus_end == pos_minus_end) need_enc_init = true;
-//                    else {
-//                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-//                        temp_time = (int) (0.5f * (float) TMR_FREQ_5k);
-//                    }
-//                }
-//                if (need_enc_init) {
-//                    if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) {
-//                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
-//                        pos_plus_end = pos.sen;
-//                    } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
-//                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-//                        pos_minus_end = pos.sen;
-//                    } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
-//                        need_enc_init = false;
-//                        check_vel_pos_init = (int) (0.9f * (float) (pos_plus_end - pos_minus_end));
-//                        check_vel_pos_fin = (int) (0.95f * (float) (pos_plus_end - pos_minus_end));
-//                        check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
-//                    }
-//                    temp_time = TMR_FREQ_5k;
-//                }
-//                TMR3_COUNT_FLOWRATE++;
-//                if (TMR3_COUNT_FLOWRATE > temp_time) {
-//                    if (flag_flowrate % 2 == 0) { // (+)
-//                        VALVE_VOLTAGE = 1000.0f * (float) (flag_flowrate / 2 + 1);
-//                        V_out = VALVE_VOLTAGE;
-//                        if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) {
-//                            fl_temp_cnt++;
-//                        } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
-//                            VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0 * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // 0.9587=6*pi/65536*10000 0.5757=0.02525*0.02*0.0095*2*60*1000
-//                            //                        VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
-//                            fl_temp_cnt2++;
-//                        }
-//                    } else if (flag_flowrate % 2 == 1) { // (-)
-//                        VALVE_VOLTAGE = -1. * (float) (flag_flowrate / 2 + 1);
-//                        V_out = VALVE_VOLTAGE;
-//                        if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) {
-//                            fl_temp_cnt++;
-//                        } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
-//                            VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0f * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE);
-//                            //                        VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
-//                            fl_temp_cnt2++;
-//                        }
-//                    }
-//                    if (fl_temp_cnt2 == 100) {
-//
-//
-//
-//                        //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f));
-//                        cur_vel_sum = 0;
-//                        fl_temp_cnt = 0;
-//                        fl_temp_cnt2 = 0;
-//                        flag_flowrate++;
-//                    }
-//                    if (flag_flowrate == 10) {
-//                        V_out = 0;
-//                        flag_flowrate = 0;
-//                        TMR3_COUNT_FLOWRATE = 0;
-//                        valve_gain_repeat_cnt++;
-//                        if (valve_gain_repeat_cnt >= 1) {
-//                            CONTROL_MODE = MODE_NO_ACT;
-//                            valve_gain_repeat_cnt = 0;
-//                        }
-//
-//                    }
-//                    break;
-//                }
-//
-//            }
             case MODE_PRESSURE_SENSOR_NULLING: {
                 // DAC Voltage reference set
                 if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
@@ -815,397 +735,92 @@
                 break;
             }
 
-//            case MODE_PRESSURE_SENSOR_CALIB: {
-//                if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
-//                    V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-//                    if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
-//                        CUR_PRES_A_sum += CUR_PRES_A;
-//                    }
-//                } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
-//                    V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
-//                    if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
-//                        CUR_PRES_B_sum += CUR_PRES_B;
-//                    }
-//                } else {
-//                    CONTROL_MODE = MODE_NO_ACT;
-//                    TMR3_COUNT_PRES_CALIB = 0;
-//                    V_out = 0;
-//                    PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_A_NULL;
-//                    PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
-//                    PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_B_NULL;
-//                    PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
-//                    CUR_PRES_A_sum = 0;
-//                    CUR_PRES_B_sum = 0;
-//                    CUR_PRES_A_mean = 0;
-//                    CUR_PRES_B_mean = 0;
-//
-//
-//
-//                    //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
-//                    //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
-//                }
-//                TMR3_COUNT_PRES_CALIB++;
-//                break;
-//            }
+            case MODE_DDV_DEADZONE_AND_CENTER: {
+                if (FINDDZ_STAGE == FINDDZ_INIT) {
+                    cnt_finddz = 0;
+                    cnt_vel_finddz = 0;
+                    temp_pos_ref = pos.sen;
+                    temp_pos_ref_offset = 0.0f;
+                    vel.ref = 0.0f;
+                    FINDDZ_STAGE = FINDDZ_START1;
+                    flag_finddz = 1;
+                    FINDDZ_POSITION = pos.sen;
+                    FINDDZ_POSITION_OLD = FINDDZ_POSITION;
+
+                    temp_P_GAIN = P_GAIN_JOINT_POSITION;
+                    temp_I_GAIN = I_GAIN_JOINT_POSITION;
+                    temp_VELOCITY_COMP_GAIN = VELOCITY_COMP_GAIN;
 
-//            case MODE_ROTARY_FRICTION_TUNING: {
-//                if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f);
-//                V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f);
-//                if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
-//                else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-//                TMR3_COUNT_ROTARY_FRIC_TUNE++;
-//                if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
-//                    TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
-//                    V_out = 0.0f;
-//                    CONTROL_MODE = MODE_NO_ACT;
-//                }
-//                break;
-//            }
-
-            case MODE_DDV_POS_VS_PWM_ID: {
-                CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
-                VALVE_ID_timer = VALVE_ID_timer + 1;
+                    P_GAIN_JOINT_POSITION = 1.0f;
+                    I_GAIN_JOINT_POSITION = 0.0f;
+                    VELOCITY_COMP_GAIN = 0;
+                    FLAG_VALVE_DEADZONE = 0;
+                    alpha_trans = 0.0f;
+                    REFERENCE_MODE=MODE_REF_NO_ACT;
 
-                if(VALVE_ID_timer < TMR_FREQ_5k*1) {
-                    Vout.ref = 3000.0f * sin(2.0f*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f);
-                } else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
-                    Vout.ref = 1000.0f*(ID_index_array[ID_index]);
-                } else if(VALVE_ID_timer == TMR_FREQ_5k*2) {
-                    VALVE_POS_TMP = 0;
-                    data_num = 0;
-                } else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
-                    data_num = data_num + 1;
-                    VALVE_POS_TMP = VALVE_POS_TMP + value;
-                } else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
-                    Vout.ref = 0.0f;
-                } else {
-                    VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num;
-                    VALVE_ID_timer = 0;
-                    ID_index= ID_index +1;
-                }
+                } else if (FINDDZ_STAGE == FINDDZ_START1) {
+                    cnt_finddz++;
+                    CONTROL_MODE = MODE_JOINT_CONTROL;
+                    FINDDZ_POSITION = pos.sen;
+                    FINDDZ_VELOCITY = FINDDZ_POSITION - FINDDZ_POSITION_OLD;
+                    FINDDZ_POSITION_OLD = FINDDZ_POSITION;
 
-                if(ID_index>=25) {
-                    int i;
-                    VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
-                    for(i=0; i<25; i++) {
-                        VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
-                        if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) {
-                            VALVE_MAX_POS = VALVE_POS_AVG[i];
-                            VALVE_POS_AVG_OLD = VALVE_MAX_POS;
-                        } else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) {
-                            VALVE_MIN_POS = VALVE_POS_AVG[i];
-                            VALVE_POS_AVG_OLD = VALVE_MIN_POS;
-                        }
-                    }
-                    spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
-                    spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
-                    for(int i=0; i<25; i++) {
-                        spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) VALVE_POS_VS_PWM[i]);
-                    }
-                    ID_index = 0;
-                    CONTROL_UTILITY_MODE = MODE_NO_ACT;
-                }
+                    if (temp_P_GAIN > 0.0f) temp_pos_ref_offset = temp_pos_ref_offset - (float)FINDDZ_VELOCITY*temp_P_GAIN;
+                    else temp_pos_ref_offset = temp_pos_ref_offset - (float)FINDDZ_VELOCITY*20.0f;
+
+                    pos.ref = temp_pos_ref + temp_pos_ref_offset;
+
+                    if (cnt_finddz > 10000) FINDDZ_STAGE = FINDDZ_START2;
 
 
-                break;
-            }
+                } else if (FINDDZ_STAGE == FINDDZ_START2) {
+                    CONTROL_MODE = MODE_JOINT_CONTROL;
+                    FINDDZ_POSITION = pos.sen;
+                    FINDDZ_VELOCITY = FINDDZ_POSITION - FINDDZ_POSITION_OLD;
+                    FINDDZ_POSITION_OLD = FINDDZ_POSITION;
 
-            case MODE_DDV_DEADZONE_AND_CENTER: {
-                CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
-                VALVE_DZ_timer = VALVE_DZ_timer + 1;
-                if(first_check == 0) {
-                    if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
-                    } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
-                        pos_plus_end = pos.sen;
-                    } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-                    } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-                        pos_minus_end = pos.sen;
-                    } else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
-                    } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
-                        data_num = data_num + 1;
-                        VALVE_POS_TMP = VALVE_POS_TMP + value;
-                    } else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
-                        DDV_POS_AVG = VALVE_POS_TMP / data_num;
-                        START_POS = pos.sen;
-                        VALVE_POS_TMP = 0;
-                        data_num = 0;
-
-                    } else if(VALVE_DZ_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
-                        valve_pos.ref = DDV_POS_AVG;
-                        VALVE_POS_CONTROL(valve_pos.ref);
-
-                    } else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) {
-                        valve_pos.ref = DDV_POS_AVG;
-                        VALVE_POS_CONTROL(valve_pos.ref);
-
-                    } else if(VALVE_DZ_timer == (int) (6.0f * (float) TMR_FREQ_5k)) {
-                        valve_pos.ref = DDV_POS_AVG;
-                        VALVE_POS_CONTROL(valve_pos.ref);
-                        FINAL_POS = pos.sen;
-
-                        if((FINAL_POS - START_POS)>200) {
-                            DZ_case = 1;
-                        } else if((FINAL_POS - START_POS)<-200) {
-                            DZ_case = -1;
-                        } else {
-                            DZ_case = 0;
+                    if (flag_finddz > 0) {
+                        temp_pos_ref = temp_pos_ref + 1.0f;
+                        if (FINDDZ_VELOCITY > 0) {
+                            cnt_vel_finddz = cnt_vel_finddz + 1;
+                        }
+                        if (cnt_vel_finddz > 5) {
+                            VALVE_DEADZONE_PLUS = (int)(I_REF_fil/mA_PER_pulse);
+//                            temp_VALVE_DEADZONE_PLUS = temp_VALVE_DEADZONE_PLUS + I_REF_fil/mA_PER_pulse;
+                            cnt_vel_finddz = 0;
+                            flag_finddz = - flag_finddz - 1;
                         }
 
-                        CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
-
-                        first_check = 1;
-                        DZ_DIRECTION = 1;
-                        VALVE_DZ_timer = 0;
-                        Ref_Valve_Pos_Old = DDV_POS_AVG;
-                        DZ_NUM = 1;
-                        DZ_index = 1;
-
-                    }
-                } else {
-                    if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) {
-                        if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) {
-                            Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
-                        } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            START_POS = pos.sen;
-                        } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            valve_pos.ref = Ref_Valve_Pos_Old  - DZ_case * DZ_DIRECTION * 64 / DZ_index;
-                            if(valve_pos.ref <= VALVE_MIN_POS) {
-                                valve_pos.ref = VALVE_MIN_POS;
-                            } else if(valve_pos.ref >= VALVE_MAX_POS) {
-                                valve_pos.ref = VALVE_MAX_POS;
-                            }
-                            VALVE_POS_CONTROL(valve_pos.ref);
-
-                        } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            Ref_Valve_Pos_Old = valve_pos.ref;
-                            FINAL_POS = pos.sen;
-
-                            if((FINAL_POS - START_POS)>100) {
-                                DZ_DIRECTION = 1 * DZ_case;
-                            } else if((FINAL_POS - START_POS)<-100) {
-                                DZ_DIRECTION = -1 * DZ_case;
-                            } else {
-                                DZ_DIRECTION = 1 * DZ_case;
-                            }
-
-                            VALVE_DZ_timer = 0;
-                            DZ_index= DZ_index *2;
-                            if(DZ_index >= 128) {
-                                FIRST_DZ = valve_pos.ref;
-                                DZ_NUM = 2;
-                                Ref_Valve_Pos_Old = FIRST_DZ;
-                                DZ_index = 1;
-                                DZ_DIRECTION = 1;
-                            }
+                    } else if (flag_finddz < 0) {
+                        temp_pos_ref = temp_pos_ref - 1.0f;
+                        if (FINDDZ_VELOCITY < 0) {
+                            cnt_vel_finddz = cnt_vel_finddz + 1;
                         }
-                    } else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) {
-                        if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
-                        } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            START_POS = pos.sen;
-                        } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            valve_pos.ref = Ref_Valve_Pos_Old  - DZ_case * DZ_DIRECTION * 64 / DZ_index;
-                            if(valve_pos.ref <= VALVE_MIN_POS) {
-                                valve_pos.ref = VALVE_MIN_POS;
-                            } else if(valve_pos.ref >= VALVE_MAX_POS) {
-                                valve_pos.ref = VALVE_MAX_POS;
-                            }
-                            VALVE_POS_CONTROL(valve_pos.ref);
-
-                        } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            Vout.ref = 0.0f;
-                        } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            Ref_Valve_Pos_Old = valve_pos.ref;
-                            FINAL_POS = pos.sen;
-
-                            if((FINAL_POS - START_POS)>100) {
-                                DZ_DIRECTION = 1 * DZ_case;
-                            } else if((FINAL_POS - START_POS)<-100) {
-                                DZ_DIRECTION = -1 * DZ_case;
-                            } else {
-                                DZ_DIRECTION = -1 * DZ_case;
-                            }
-
-                            VALVE_DZ_timer = 0;
-                            DZ_index= DZ_index * 2;
-                            if(DZ_index >= 128) {
-                                SECOND_DZ = valve_pos.ref;
-                                VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
-                                first_check = 0;
-                                VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
-                                VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
-
-                                spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
-                                spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
-                                spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
-
-                                CONTROL_UTILITY_MODE = MODE_NO_ACT;
-                                DZ_index = 1;
-                            }
-                        }
-                    } else if(DZ_case == 0 && DZ_NUM ==1) {
-                        if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
-                        } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            START_POS = pos.sen;
-                        } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            valve_pos.ref = Ref_Valve_Pos_Old  - DZ_DIRECTION * 64 / DZ_index;
-                            if(valve_pos.ref <= VALVE_MIN_POS) {
-                                valve_pos.ref = VALVE_MIN_POS;
-                            } else if(valve_pos.ref >= VALVE_MAX_POS) {
-                                valve_pos.ref = VALVE_MAX_POS;
-                            }
-                            VALVE_POS_CONTROL(valve_pos.ref);
-
-                        } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            Ref_Valve_Pos_Old = valve_pos.ref;
-                            FINAL_POS = pos.sen;
-
-                            if((FINAL_POS - START_POS)>100) {
-                                DZ_DIRECTION = 1;
-                            } else if((FINAL_POS - START_POS)<-100) {
-                                DZ_DIRECTION = -1;
-                            } else {
-                                DZ_DIRECTION = 1;
-                            }
-                            VALVE_DZ_timer = 0;
-                            DZ_index= DZ_index *2;
-                            if(DZ_index >= 128) {
-                                FIRST_DZ = valve_pos.ref;
-                                DZ_NUM = 2;
-                                Ref_Valve_Pos_Old = FIRST_DZ;
-                                DZ_index = 1;
-                                DZ_DIRECTION = 1;
-                            }
-                        }
-                    } else {
-                        if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
-                        } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                            START_POS = pos.sen;
-                        } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            valve_pos.ref = Ref_Valve_Pos_Old  + DZ_DIRECTION * 64 / DZ_index;
-                            if(valve_pos.ref <= VALVE_MIN_POS) {
-                                valve_pos.ref = VALVE_MIN_POS;
-                            } else if(valve_pos.ref > VALVE_MAX_POS) {
-                                valve_pos.ref = VALVE_MAX_POS - 1;
-                            }
-                            VALVE_POS_CONTROL(valve_pos.ref);
-
-                        } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            Vout.ref = 0.0f;
-                        } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
-                            Ref_Valve_Pos_Old = valve_pos.ref;
-                            FINAL_POS = pos.sen;
-
-                            if((FINAL_POS - START_POS)>100) {
-                                DZ_DIRECTION = 1;
-                            } else if((FINAL_POS - START_POS)<-100) {
-                                DZ_DIRECTION = -1;
-                            } else {
-                                DZ_DIRECTION = 1;
-                            }
-
-                            VALVE_DZ_timer = 0;
-                            DZ_index= DZ_index *2;
-                            if(DZ_index >= 128) {
-                                SECOND_DZ = valve_pos.ref;
-                                VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
-                                first_check = 0;
-                                VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
-                                VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
-
-                                spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
-                                spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
-                                spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
-
-                                CONTROL_UTILITY_MODE = MODE_NO_ACT;
-                                DZ_index = 1;
-                            }
+                        if (cnt_vel_finddz > 5) {
+                            VALVE_DEADZONE_MINUS = (int)(I_REF_fil/mA_PER_pulse);
+//                            temp_VALVE_DEADZONE_MINUS = temp_VALVE_DEADZONE_MINUS + I_REF_fil/mA_PER_pulse;
+                            cnt_vel_finddz = 0;
+                            flag_finddz = - flag_finddz + 1;
                         }
                     }
-                }
-                break;
-            }
-
-            case MODE_DDV_POS_VS_FLOWRATE: {
-                CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
-                VALVE_FR_timer = VALVE_FR_timer + 1;
-                if(first_check == 0) {
-                    if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
-                    } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
-                        pos_plus_end = pos.sen;
-                    } else if(VALVE_FR_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-                    } else if(VALVE_FR_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
-                        Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-                        pos_minus_end = pos.sen;
-                        first_check = 1;
-                        VALVE_FR_timer = 0;
-                        valve_pos.ref = (float) VALVE_CENTER;
-                        ID_index = 0;
-                        max_check = 0;
-                        min_check = 0;
+                    if (abs(flag_finddz) >= 6) {
+                        FINDDZ_STAGE = FINDDZ_STOP;
+//                        VALVE_DEADZONE_PLUS = (int)(temp_VALVE_DEADZONE_PLUS / (double)flag_finddz * 2.0f);
+//                        VALVE_DEADZONE_MINUS = (int)(temp_VALVE_DEADZONE_MINUS / (double)flag_finddz * 2.0f);
+                        spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
+                        spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
                     }
-                } else {
-                    if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
-                        CONTROL_MODE = MODE_JOINT_CONTROL;
-                    } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
-                        data_num = 0;
-                        valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
+                    pos.ref = temp_pos_ref + temp_pos_ref_offset;
 
-                        VALVE_POS_CONTROL(valve_pos.ref);
-                        START_POS = pos.sen;
-                    } else if(VALVE_FR_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
-                        valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
-                        VALVE_POS_CONTROL(valve_pos.ref);
-                        data_num = data_num + 1;
-                        if(abs(0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen) > 20000.0f) {
-                            FINAL_POS = pos.sen;
-                            one_period_end = 1;
-                        }
-                    } else if(VALVE_FR_timer == (int) (5.0f * (float) TMR_FREQ_5k)) {
-                        FINAL_POS = pos.sen;
-                        one_period_end = 1;
-                        V_out = 0.0f;
-                    }
-
-                    if(one_period_end == 1) {
-                        if(valve_pos.ref > VALVE_MAX_POS) {
-                            max_check = 1;
-                        } else if(valve_pos.ref < VALVE_MIN_POS) {
-                            min_check = 1;
-                        }
-                        JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k;   //  pulse/sec
-
-                        VALVE_FR_timer = 0;
-                        one_period_end = 0;
-                        ID_index= ID_index +1;
-                        V_out = 0.0f;
-                    }
-
-                    if(max_check == 1 && min_check == 1) {
-
-                        VALVE_POS_NUM = ID_index;
-                        for(int i=0; i<100; i++) {
-                            spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + i, (int16_t) (JOINT_VEL[i] & 0xFFFF));
-                            spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + i, (int16_t) ((JOINT_VEL[i] >> 16) & 0xFFFF));
-                        }
-                        ID_index = 0;
-                        first_check = 0;
-                        VALVE_FR_timer = 0;
-                        CONTROL_UTILITY_MODE = MODE_NO_ACT;
-                    }
+                } else if (FINDDZ_STAGE == FINDDZ_STOP) {
+                    FINDDZ_STAGE = FINDDZ_INIT;
+                    CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
+                    P_GAIN_JOINT_POSITION = temp_P_GAIN;
+                    I_GAIN_JOINT_POSITION = temp_I_GAIN;
+                    VELOCITY_COMP_GAIN = temp_VELOCITY_COMP_GAIN;
+                    flag_finddz = 0;
+                    FLAG_VALVE_DEADZONE = 1;
+                    REFERENCE_MODE=MODE_REF_DIRECT;
                 }
                 break;
             }
@@ -1254,23 +869,6 @@
                 }
                 break;
             }
-            case MODE_SEND_BUFFER: {
-//                if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
-//                    CAN_TX_PRES((int16_t) (pos_array[cnt_send_buffer]), (int16_t) (ref_array[cnt_send_buffer])); // 1400
-//                    if(cnt_send_buffer>=buffer_data_size) {
-//                        CONTROL_UTILITY_MODE = MODE_FREQ_TEST;
-//                    }
-//                    cnt_send_buffer++;
-//                }
-
-                break;
-            }
-            case MODE_SEND_OVER: {
-                CAN_TX_TORQUE((int16_t) (buffer_data_size)); //1300
-                CONTROL_UTILITY_MODE = MODE_NO_ACT;
-                CONTROL_MODE = MODE_NO_ACT;
-                break;
-            }
 
             case MODE_STEP_TEST: {
                 float valve_pos_ref = 0.0f;
@@ -1324,7 +922,7 @@
                 } else if (CURRENT_CONTROL_MODE == 0) { //PWM
                     V_out = valve_pos.ref;
                 } else {
-                    I_REF = (double)(valve_pos.ref * (float)mA_PER_pulse);
+                    I_REF = valve_pos.ref * 0.001f;
                 }
                 break;
             }
@@ -1511,7 +1109,6 @@
 
                 float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k;
                 x_4_des_old = x_4_des;
-                float V_input = 0.0f;
                 V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4;
 
                 float rho_a = 0.00001f;
@@ -1605,15 +1202,6 @@
             else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f);
             else V_out = (float) (CUR_PWM_lin);
         } else {            //////////////////////////sw valve
-            // Output Voltage Linearization
-//            double CUR_PWM_nonlin = V_out; // Unit : mV
-//            double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin);  // -8000~8000
-
-            // Dead Zone Cancellation (Electrical dead-zone)
-//            if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f);
-//            else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f);
-//            else V_out = (float) (CUR_PWM_lin);
-
             // Output Voltage Linearization & Dead Zone Cancellation (Electrical dead-zone) by SW
             if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f;
             else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f;
@@ -1667,51 +1255,21 @@
                         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));
                     }
                 }
-
             }
 
-
-            // ID:1300
+            // PWM, ID:1300
             if (flag_data_request[1] == HIGH) {
-                CAN_TX_TORQUE((int16_t) 7); //1300
+                CAN_TX_PWM((int16_t) (V_out)); //1300
             }
 
-            // Reference Current, Current Current (ID:1700)
+            // Reference Current, Current Current (ID:1400)
             if (flag_data_request[2] == HIGH) {
-
-                CAN_TX_SOMETHING((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (HOMEPOS_OFFSET), (int16_t) (0));
-                //double t_value = 0.0f;
-                //            if(value>=(float) VALVE_CENTER) {
-                //                t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-                //            } else {
-                //                t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-                //            }
-                //            double t_value_ref = 0.0f;
-                //            if(valve_pos.ref>=(float) VALVE_CENTER) {
-                //                t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-                //            } else {
-                //                t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-                //            }
-                //
-                //
-                //            CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
+                CAN_TX_CURRENT((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse)); // 1400
             }
-
-            //If it doesn't rest, below can can not work.
-            for (can_rest = 0; can_rest < 10000; can_rest++) {
-                ;
-            }
-
-            // ID:1500
+            
+            // Reference Current, Current Current (ID:1700)
             if (flag_data_request[3] == HIGH) {
-                //PWM
-                CAN_TX_PWM((int16_t) (torq.ref)); //1500
-            }
-
-            // ID:1600
-            if (flag_data_request[4] == HIGH) {
-                //valve position
-                CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
+                CAN_TX_SOMETHING((int16_t) (0), (int16_t) (0), (int16_t) (0), (int16_t) (0)); // 1700
             }
 
             TMR2_COUNT_CAN_TX = 0;
--- a/setting.h	Wed Mar 03 10:56:25 2021 +0000
+++ b/setting.h	Thu Mar 11 07:27:23 2021 +0000
@@ -9,6 +9,7 @@
 #define PWM_ARR     0x1194      // loop 20k, pwm 10k
 //#define PWM_ARR     0x2328      // loop 10k, pwm 5k
 #define TMR3_COUNT  0x4650      // loop 5k
+//#define TMR3_COUNT  0x2328      // loop 10k
 #define TMR2_COUNT  0x2710      // loop 500hz with prescale 18
 
 #define FREQ_500    500.0f
@@ -208,10 +209,11 @@
 extern float DAC_REF;
 extern float DAC_RESOL;
 
-extern int REF_POSITION;
-extern int REF_VELOCITY;
-extern int16_t REF_TORQUE;
-extern int16_t REF_PRES_DIFF;
+extern float REF_POSITION;
+extern float REF_VELOCITY;
+extern float REF_TORQUE;
+extern float REF_TORQUE_OLD;
+extern float REF_PRES_DIFF;
 extern int16_t REF_PWM;
 extern int16_t REF_VALVE_POSITION;
 extern int16_t REF_CURRENT;
@@ -376,6 +378,19 @@
 extern int FINDHOME_POSITION;
 extern int FINDHOME_POSITION_OLD;
 
+extern int cnt_finddz;
+extern int cnt_vel_finddz;
+extern int flag_finddz;
+extern int FINDDZ_VELOCITY;
+extern int FINDDZ_VELOCITY_OLD;
+extern int FINDDZ_POSITION;
+extern int FINDDZ_POSITION_OLD;
+
+extern double temp_VALVE_DEADZONE_PLUS;
+extern double temp_VALVE_DEADZONE_MINUS;
+extern float temp_pos_ref;
+extern float temp_pos_ref_offset;
+
 // valve gain
 extern int check_vel_pos_init;
 extern int check_vel_pos_fin;
@@ -418,6 +433,12 @@
 extern int FINDHOME_GOTOLIMIT;
 extern int FINDHOME_ZEROPOSE;
 
+extern int FINDDZ_STAGE;
+extern int FINDDZ_INIT;
+extern int FINDDZ_START1;
+extern int FINDDZ_START2;
+extern int FINDDZ_STOP;
+
 extern float alpha_trans;