eeprom_test

Dependencies:   mbed FastPWM

Revision:
14:8e7590227d22
Parent:
13:747daba9cf59
Child:
15:bd0d12728506
--- a/main.cpp	Tue Aug 27 06:50:54 2019 +0000
+++ b/main.cpp	Thu Aug 29 07:38:00 2019 +0000
@@ -12,7 +12,9 @@
 DigitalOut check_2(PC_3);
 AnalogOut dac_1(PA_4);
 AnalogOut dac_2(PA_5);
-//AnalogIn adc3(PC_1);
+AnalogIn adc1(PC_4); //pressure_1
+AnalogIn adc2(PB_0); //pressure_2
+AnalogIn adc3(PC_1); //current
 
 // PWM ///////////////////////////////////////////
 double dtc_v=0.0;
@@ -49,6 +51,12 @@
 State pres_A;
 State pres_B;
 State cur;
+State valve_pos;
+
+State INIT_Vout;
+State INIT_Valve_Pos;
+State INIT_Pos;
+State INIT_torq;
 
 double V_out=0.0;
 double V_rem=0.0; // for anti-windup
@@ -92,6 +100,10 @@
 
     MODE_TEST_CURRENT_CONTROL,                          //9
     MODE_TEST_PWM_CONTROL,                              //10
+    
+    MODE_CURRENT_CONTROL,                               //11
+    MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT,         //12
+    MODE_JOINT_POSITION_PRES_CONTROL_CURRENT,           //13
 
     //utility
     MODE_TORQUE_SENSOR_NULLING = 20,                    //20
@@ -115,6 +127,10 @@
 
     LED = 1;
     pc.baud(9600);
+    
+    //eeprom
+    ROM_INIT_DATA();
+    make_delay();
 
     // i2c init
     i2c.frequency(400 * 1000);          // 0.4 mHz
@@ -143,7 +159,7 @@
     Init_TMR3();
     TIM3->CR1 ^= TIM_CR1_UDIS;
     make_delay();
-
+    
     // CAN
     can.attach(&CAN_RX_HANDLER);
     CAN_ID_INIT();
@@ -153,12 +169,11 @@
     spi_enc_set_init();
     make_delay();
 
-    //eeprom
-    ROM_INIT_DATA();
-    make_delay();
+    
 
     //DAC init
-    dac_1 = PRES_A_VREF/3.3;
+    //dac_1 = PRES_A_VREF/3.3;
+    dac_1 = 0.0;
     dac_2 = PRES_B_VREF/3.3;
     make_delay();
 
@@ -177,44 +192,39 @@
 
         //spi _ enc
         //int a = spi_enc_read();
-
+        //pc.printf("Message received: %d\n", msg.data[0]);
+        //pc.printf("Message received: %d\n", 13);
         //i2c
-        read_field(i2c_slave_addr1);
+        //read_field(i2c_slave_addr1);
     }
 }
 
-int DDV_JOINT_POS_FF(double REF_JOINT_VEL){
-    
+int DDV_JOINT_POS_FF(double REF_JOINT_VEL)
+{
+
     int i = 0;
     int Ref_Valve_Pos_FF = DDV_CENTER;
-    for(i=0; i<VALVE_POS_NUM; i++)
-    {
-        if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <=  max(JOINT_VEL[i],JOINT_VEL[i+1]))
-        {
-            if(i==0)
-            {
+    for(i=0; i<VALVE_POS_NUM; i++) {
+        if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <=  max(JOINT_VEL[i],JOINT_VEL[i+1])) {
+            if(i==0) {
                 Ref_Valve_Pos_FF = ((int) 50.0/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
-            }
-            else
-            {
+            } else {
                 Ref_Valve_Pos_FF = ((int) 50.0*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + 50*ID_index_array[i-1];
             }
             break;
         }
     }
-    if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2]))
-    {
+    if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
         Ref_Valve_Pos_FF = VALVE_MAX_POS;
-    }
-    else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2]))
-    {
+    } else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
         Ref_Valve_Pos_FF = VALVE_MIN_POS;
     }
     return Ref_Valve_Pos_FF;
 }
 
 
-void VALVE_POS_CONTROL(double REF_VALVE_POS){
+void VALVE_POS_CONTROL(double REF_VALVE_POS)
+{
     int i = 0;
 
     valve_pos_err = REF_VALVE_POS - value;
@@ -225,36 +235,44 @@
     if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000;
 
     VALVE_PWM_RAW_FB = P_GAIN_VALVE_POSITION * valve_pos_err + I_GAIN_VALVE_POSITION * valve_pos_err_sum + D_GAIN_VALVE_POSITION * valve_pos_err_diff;
-    
-    if(REF_VALVE_POS > VALVE_MAX_POS)
-    {
+
+    if(REF_VALVE_POS > VALVE_MAX_POS) {
         REF_VALVE_POS = VALVE_MAX_POS;
-    }
-    else if(REF_VALVE_POS < VALVE_MIN_POS)
-    {
+    } else if(REF_VALVE_POS < VALVE_MIN_POS) {
         REF_VALVE_POS = VALVE_MIN_POS;
     }
-    
-    for(i=0; i<16; i++)
-    {
-        if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && REF_VALVE_POS <=  max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]))
-        {
-            if(i==0)
-            {
+
+    for(i=0; i<16; i++) {
+        if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && REF_VALVE_POS <=  max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) {
+            if(i==0) {
                 VALVE_PWM_RAW_FF = (double) 1000.0/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * (REF_VALVE_POS - VALVE_POS_VS_PWM[i]);
-            }
-            else
-            {
+            } else {
                 VALVE_PWM_RAW_FF = (double) 1000.0*(ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * (REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000*ID_index_array[i-1];
             }
             break;
         }
     }
-    VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;   
+    V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
 }
 
+#define LT_MAX_IDX  57
+double LT_PWM_duty[LT_MAX_IDX] = {-100.0, -80.0, -60.0, -50.0, -40.0, -35.0, -30.0, -25.0, -20.0,
+                                  -19.0, -18.0, -17.0, -16.0, -15.0, -14.0, -13.0, -12.0, -11.0, -10.0,
+                                  -9.0, -8.0, -7.0, -6.0, -5.0, -4.0, -3.0, -2.0, -1.0, 0.0,
+                                  1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0,
+                                  11.0, 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0,
+                                  25.0, 30.0, 35.0, 40.0, 50.0, 60.0, 80.0, 100.0
+                                 };  // duty
+double LT_Voltage_Output[LT_MAX_IDX] = {-321.4, -291.3, -261.5, -246.8, -231.7, -223.9, -216.1, -207.9, -198.8,
+                                        -196.9, -195.0, -192.5, -188.8, -184.5, -180.2, -175.9, -171.5, -166.3, -161.0,
+                                        -156.0, -149.5, -139.0, -126.0, -107.0, -87.5, -64.0, -38.5, -9.4, 0.0,
+                                        12.0, 43.5, 69.0, 94.0, 114.0, 132.0, 146.0, 155.5, 162.3, 168.2,
+                                        173.1, 178.2, 182.8, 187.4, 191.8, 196.0, 199.7, 201.9, 203.8, 205.6,
+                                        214.6, 222.5, 230.4, 238.2, 253.3, 268.0, 297.6, 327.7
+                                       }; // mV
 
-double PWM_duty_byLT(double Ref_V) {
+double PWM_duty_byLT(double Ref_V)
+{
     double PWM_duty = 0.0;
     if(Ref_V<LT_Voltage_Output[0]) {
         PWM_duty = (Ref_V-LT_Voltage_Output[0])/1.5+LT_PWM_duty[0];
@@ -273,7 +291,7 @@
             }
         }
     }
-    
+
     return PWM_duty;
 }
 
@@ -293,24 +311,32 @@
 
         if((CNT_TMR4%2)==0) {
 
-            //ADC
+            //Pressure sensor A
+            ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
+            //while((ADC1->SR & 0b10));
+            double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
+            double pres_A_new = ((double)ADC1->DR - PRES_A_NULL)  / PRES_SENSOR_A_PULSE_PER_BAR + 1.0;
+            pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
+            //pres_A.sen = (double)ADC1->DR;
+/*            
+            //Pressure sensor 1B
+            //ADC2->CR2  |= 0x40000000;                        // adc _ 12bit
+            while((ADC2->SR & 0b10));
+            double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
+            double pres_B_new = ((double)ADC2->DR - PRES_B_NULL)  / PRES_SENSOR_B_PULSE_PER_BAR + 1.0;
+            pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+            
+            //Current
             ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
-//          a1=ADC1->DR;
 //          a1=ADC2->DR;
 //          int raw_cur = ADC3->DR;
             while((ADC3->SR & 0b10));
-
             double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
             double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
             cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
+*/
         }
 
-        //DAC
-//      dac_1 = ADC1->DR;
-//      dac_2 = ADC2->DR;
-
-
-
         /*******************************************************
         ***     Timer Counting & etc.
         ********************************************************/
@@ -325,55 +351,54 @@
 extern "C" void TIM3_IRQHandler(void)
 {
     if ( TIM3->SR & TIM_SR_UIF ) {
-
+        
         ENC_UPDATE();
-        CUR_PRES_A_BAR = (CUR_PRES_A - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.;
-        CUR_PRES_B_BAR = (CUR_PRES_B - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.;
-        //CUR_TORQUE_NM = (CUR_TORQUE - TORQUE_NULL) / (double) TORQUE_SENSOR_PULSE_PER_TORQUE; //
-        CUR_TORQUE_NM = CUR_TORQUE; //
-        CUR_TORQUE_NM_PRESS = 1. * (CUR_PRES_A_BAR - CUR_PRES_B_BAR);
 
+        /*
         // Reference Loop
         switch (REFERENCE_MODE) {
             case MODE_REF_NO_ACT: {
                 break;
             }
-
             case MODE_REF_DIRECT: {
                 if (FLAG_REFERENCE_VALVE_PWM) {
-                    Ref_PWM = (double) REF_PWM;
+                    Vout.ref = (double) Vout.ref;
                 }
                 if (FLAG_REFERENCE_VALVE_POSITION) {
-                    Ref_Valve_Pos = (double) REF_VALVE_POSITION;
+                    valve_pos.ref = (double) valve_pos.ref;
                 }
                 if (FLAG_REFERENCE_JOINT_POSITION) {
-                    Ref_Joint_Pos = (double) REF_POSITION;
-                    Ref_Joint_Vel = (double) REF_VELOCITY;
+                    pos.ref = (double) pos.ref;
+                    vel.ref = (double) vel.ref;
                 }
                 if (FLAG_REFERENCE_JOINT_TORQUE) {
-                    Ref_Joint_Torq = (double) REF_TORQUE;
+                    torq.ref = (double) torq.ref;
+                }
+                if (FLAG_REFERENCE_CURRENT) {
+                    cur.ref = ((double)cur.ref);
                 }
                 break;
             }
-
+            
             case MODE_REF_COS_INC: {
                 if (FLAG_REFERENCE_VALVE_PWM) {
-                    Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_PWM;
+                    Vout.ref = ((double) Vout.ref - (double) INIT_Vout.ref)*(0.5 - 0.5 * cos(3.14159 * (double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_Vout.ref;
                 }
                 if (FLAG_REFERENCE_VALVE_POSITION) {
-                    Ref_Valve_Pos = ((double) REF_VALVE_POSITION - (double) INIT_REF_VALVE_POS)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_VALVE_POS;
+                    valve_pos.ref = ((double) valve_pos.ref - (double) INIT_Valve_Pos.ref)*(0.5 - 0.5 * cos(3.14159 * (double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_Valve_Pos.ref;
                 }
                 if (FLAG_REFERENCE_JOINT_POSITION) {
-                    Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_POS;
-                    Ref_Joint_Vel = 0.0;
+                    pos.ref = ((double) pos.ref - (double) INIT_Pos.ref)*(0.5 - 0.5 * cos(3.14159 * (double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_Pos.ref;
+                    vel.ref = 0.0;
                 }
                 if (FLAG_REFERENCE_JOINT_TORQUE) {
-                    Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_TORQUE;
+                    torq.ref = ((double) torq.ref - (double) INIT_torq.ref)*(0.5 - 0.5 * cos(3.14159 * (double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_torq.ref;
                 }
-                TMR2_COUNT_REFERENCE++;
-                if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
-                    REFERENCE_MODE = MODE_REF_DIRECT;
-                    TMR2_COUNT_REFERENCE = 0;
+                TMR3_COUNT_REFERENCE++;
+                if (TMR3_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
+                    //REFERENCE_MODE = MODE_REF_DIRECT;
+                    TMR3_COUNT_REFERENCE = REF_MOVE_TIME_5k;
+                    //TMR3_COUNT_REFERENCE = 0;
                 }
                 break;
             }
@@ -381,217 +406,209 @@
 
             case MODE_REF_LINE_INC: {
                 if (FLAG_REFERENCE_VALVE_PWM) {
-                    Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_PWM;
+                    Vout.ref = ((double) Vout.ref - (double) INIT_Vout.ref)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_Vout.ref;
                 }
                 if (FLAG_REFERENCE_VALVE_POSITION) {
-                    Ref_Valve_Pos = ((double) REF_VALVE_POSITION - (double) INIT_REF_VALVE_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_VALVE_POS;
+                    valve_pos.ref = ((double) valve_pos.ref - (double) INIT_valve_pos.ref)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_valve_pos.ref;
                 }
                 if (FLAG_REFERENCE_JOINT_POSITION) {
-                    Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_POS;
-                    Ref_Vel_Test = ((double) REF_POSITION - (double) INIT_REF_POS) / (double) REF_MOVE_TIME_5k * (double) TMR_FREQ_5k;    //   pulse/sec
-                    //Ref_Vel_Test = 10;
+                    pos.ref = ((double) pos.ref - (double) INIT_REF_POS)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_POS;
+                    vel.ref = ((double) pos.ref - (double) INIT_REF_POS) / (double) REF_MOVE_TIME_5k * (double) TMR_FREQ_5k;    //   pulse/sec
                 }
                 if (FLAG_REFERENCE_JOINT_TORQUE) {
-                    Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_TORQUE;
+                    torq.ref = ((double) torq.ref - (double) INIT_torq.ref)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_torq.ref;
                 }
-                TMR2_COUNT_REFERENCE++;
-                if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
+                TMR3_COUNT_REFERENCE++;
+                if (TMR3_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
                     //REFERENCE_MODE = MODE_REF_DIRECT;
-                    Ref_Vel_Test = 1;
-                    TMR2_COUNT_REFERENCE = REF_MOVE_TIME_5k;
-                    //TMR2_COUNT_REFERENCE = 0;
+                    TMR3_COUNT_REFERENCE = REF_MOVE_TIME_5k;
+                    //TMR3_COUNT_REFERENCE = 0;
                 }
                 break;
             }
 
             case MODE_REF_SIN_WAVE: {
                 if (FLAG_REFERENCE_VALVE_PWM) {
-                    Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
+                    Vout.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) Vout.ref;
                 }
                 if (FLAG_REFERENCE_VALVE_POSITION) {
-                    Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
+                    valve_pos.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) valve_pos.ref;
                 }
                 if (FLAG_REFERENCE_JOINT_POSITION) {
-                    Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
+                    Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) pos.ref;
                 }
                 if (FLAG_REFERENCE_JOINT_TORQUE) {
-                    Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
+                    Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) torq.ref;
                 }
-                TMR2_COUNT_REFERENCE++;
-                if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
+                TMR3_COUNT_REFERENCE++;
+                if (TMR3_COUNT_REFERENCE >= TMR3_COUNT_REFERENCE * REF_NUM) {
                     REFERENCE_MODE = MODE_REF_DIRECT;
-                    TMR2_COUNT_REFERENCE = 0;
+                    TMR3_COUNT_REFERENCE = 0;
                 }
                 break;
             }
 
             case MODE_REF_SQUARE_WAVE: {
                 if (FLAG_REFERENCE_VALVE_PWM) {
-                    Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
-                    if (Ref_PWM >= REF_PWM) Ref_PWM = REF_MAG + REF_PWM;
-                    else Ref_PWM = -REF_MAG + REF_PWM;
+                    Vout.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) Vout.ref;
+                    if (Vout.ref >= Vout.ref) Vout.ref = REF_MAG + Vout.ref;
+                    else Vout.ref = -REF_MAG + Vout.ref;
                 }
                 if (FLAG_REFERENCE_VALVE_POSITION) {
-                    Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
-                    if (Ref_Valve_Pos >= REF_VALVE_POSITION) Ref_Valve_Pos = REF_MAG + REF_VALVE_POSITION;
-                    else Ref_Valve_Pos = -REF_MAG + REF_VALVE_POSITION;
+                    valve_pos.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) valve_pos.ref;
+                    if (valve_pos.ref >= valve_pos.ref) valve_pos.ref = REF_MAG + valve_pos.ref;
+                    else valve_pos.ref = -REF_MAG + valve_pos.ref;
                 }
                 if (FLAG_REFERENCE_JOINT_POSITION) {
-                    Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
-                    if (Ref_Joint_Pos >= REF_POSITION) Ref_Valve_Pos = REF_MAG + REF_POSITION;
-                    else Ref_Joint_Pos = -REF_MAG + REF_POSITION;
+                    Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) pos.ref;
+                    if (Ref_Joint_Pos >= pos.ref) valve_pos.ref = REF_MAG + pos.ref;
+                    else Ref_Joint_Pos = -REF_MAG + pos.ref;
                 }
                 if (FLAG_REFERENCE_JOINT_TORQUE) {
-                    Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
-                    if (Ref_Joint_Torq >= REF_TORQUE) Ref_Valve_Pos = REF_MAG + REF_TORQUE;
-                    else Ref_Joint_Torq = -REF_MAG + REF_TORQUE;
-                }
-                TMR2_COUNT_REFERENCE++;
-                if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
-                    REFERENCE_MODE = MODE_REF_DIRECT;
-                    TMR2_COUNT_REFERENCE = 0;
+                    Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) torq.ref;
+                    if (Ref_Joint_Torq >= torq.ref) valve_pos.ref = REF_MAG + torq.ref;
+                    else Ref_Joint_Torq = -REF_MAG + torq.ref;
                 }
-                break;
-            }
-
-            default:
-                break;
-        }
-
-        // CONTROL LOOP ------------------------------------------------------------
-        switch (CONTROL_MODE) {
-            case MODE_NO_ACT:
-            {
-                //            SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
-                VALVE_PWM_RAW = 0;
-                break;
-            }
-            
-            case MODE_VALVE_OPEN_LOOP:
-            {
-                VALVE_PWM_RAW = Ref_PWM;
+                TMR3_COUNT_REFERENCE++;
+                if (TMR3_COUNT_REFERENCE >= TMR3_COUNT_REFERENCE * REF_NUM) {
+                    REFERENCE_MODE = MODE_REF_DIRECT;
+                    TMR3_COUNT_REFERENCE = 0;
+                }
                 break;
             }
             
-            case MODE_VALVE_POSITION_CONTROL:
-            {
-                VALVE_POS_CONTROL(Ref_Valve_Pos);               
+            default:
+                break;
+        }
+        
+        */
+
+        // CONTROL LOOP ------------------------------------------------------------
+        switch (CONTROL_MODE) {
+            case MODE_NO_ACT: {
+                //            SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
+                V_out = 0;
                 break;
             }
-            
-            case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM:
-            {
+
+            case MODE_VALVE_OPEN_LOOP: {
+                V_out = Vout.ref;
+                break;
+            }
+
+            case MODE_VALVE_POSITION_CONTROL: {
+                VALVE_POS_CONTROL(valve_pos.ref);
+                break;
+            }
+
+            case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: {
                 double PWM_RAW_POS_FB = 0.0; // PWM by Position Feedback
                 double PWM_RAW_POS_FF = 0.0; // PWM by Position Feedforward
                 double PWM_RAW_FORCE_FB = 0.0; // PWM by Force Feedback
-                
+
                 // feedback input for position control
-                joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
-                joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
-                joint_pos_err_old = joint_pos_err;
-                joint_pos_err_sum += joint_pos_err;
-                if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
-                if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
-    //            PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff                        PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff;
-                PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * joint_pos_err;
+                pos.err = pos.ref - (double) pos.sen;
+                pos.err_diff = pos.err - pos.err_old;
+                pos.err_old = pos.err;
+                pos.err_sum += pos.err;
+                if (pos.err_sum > 1000) pos.err_sum = 1000;
+                if (pos.err_sum<-1000) pos.err_sum = -1000;
+                //            PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * pos.err + (double) I_GAIN_JOINT_POSITION * pos.err_sum + (double) D_GAIN_JOINT_POSITION * pos.err_diff;
+                PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * pos.err;
                 PWM_RAW_POS_FB = PWM_RAW_POS_FB * 0.01;
-                
+
                 // feedforward input for position control
-                double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] 
+                double Ref_Vel_Act = vel.ref/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
                 double K_ff = 0.9;
-                if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open
-                if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close
-                PWM_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50;
-                
+                if(Ref_Vel_Act > 0) K_ff = 0.90; // open
+                if(Ref_Vel_Act > 0) K_ff = 0.75; // close
+                PWM_RAW_POS_FF = K_ff*Ref_Vel_Act/0.50;
+
                 // torque feedback
-                //            joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
-                //            joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
-                //            joint_torq_err_old = joint_torq_err;
-                //            joint_torq_err_sum += joint_torq_err;
-                //            if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
-                //            if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
-                //            VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff;
+                //            torq.err = Ref_Joint_Torq - CUR_TORQUE_NM;
+                //            torq.err_diff = torq.err - torq.err_old;
+                //            torq.err_old = torq.err;
+                //            torq.err_sum += torq.err;
+                //            if (torq.err_sum > 1000) torq.err_sum = 1000;
+                //            if (torq.err_sum<-1000) torq.err_sum = -1000;
+                //            VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
                 //            VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
                 PWM_RAW_FORCE_FB = 0.0;
-                           
-    //            VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
-                VALVE_PWM_RAW = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
-    
+
+                //            VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
+                V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
+
                 break;
             }
-            
-            case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION:
-            {
-                TMR2_COUNT_JOINT++;
+
+            case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: {
                 double VALVE_POS_RAW_POS_FB = 0.0; // Valve Position by Position Feedback
                 double VALVE_POS_RAW_POS_FF = 0.0; // Valve Position by Position Feedforward
                 double VALVE_POS_RAW_FORCE_FB = 0.0; // Valve Position by Force Feedback
                 int DDV_JOINT_CAN = 0;
                 // feedback input for position control
-                joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
-                joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
-                joint_pos_err_old = joint_pos_err;
-                joint_pos_err_sum += joint_pos_err;
-                if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
-                if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
-                VALVE_POS_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * 0.01 * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff;
+                pos.err = pos.ref - (double) pos.sen;
+                pos.err_diff = pos.err - pos.err_old;
+                pos.err_old = pos.err;
+                pos.err_sum += pos.err;
+                if (pos.err_sum > 1000) pos.err_sum = 1000;
+                if (pos.err_sum<-1000) pos.err_sum = -1000;
+                VALVE_POS_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * 0.01 * pos.err + (double) I_GAIN_JOINT_POSITION * pos.err_sum + (double) D_GAIN_JOINT_POSITION * pos.err_diff;
                 VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01;
-                
-                
+
+
                 //Ref_Joint_Vel =  Ref_Vel_Test;
                 // feedforward input for position control
-    //            double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] 
-    //            double K_ff = 0.9;
-    //            if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open
-    //            if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close
-    //            VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50;
-                
+                //            double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+                //            double K_ff = 0.9;
+                //            if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open
+                //            if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close
+                //            VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50;
+
                 // torque feedback
-                //            joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
-                //            joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
-                //            joint_torq_err_old = joint_torq_err;
-                //            joint_torq_err_sum += joint_torq_err;
-                //            if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
-                //            if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
-                //            VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff;
+                //            torq.err = Ref_Joint_Torq - CUR_TORQUE_NM;
+                //            torq.err_diff = torq.err - torq.err_old;
+                //            torq.err_old = torq.err;
+                //            torq.err_sum += torq.err;
+                //            if (torq.err_sum > 1000) torq.err_sum = 1000;
+                //            if (torq.err_sum<-1000) torq.err_sum = -1000;
+                //            VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
                 //            VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
                 VALVE_POS_RAW_FORCE_FB = 0.0;
-                
-                Ref_Valve_Pos = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(Ref_Joint_Vel) + VALVE_POS_RAW_FORCE_FB;
-                //Ref_Valve_Pos = DDV_JOINT_POS_FF(Ref_Joint_Vel);
-                
-                VALVE_POS_CONTROL(Ref_Valve_Pos);
+
+                valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
+                //valve_pos.ref = DDV_JOINT_POS_FF(Ref_Joint_Vel);
+
+                VALVE_POS_CONTROL(valve_pos.ref);
                 break;
             }
-            
-            case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING:
-            {
-                
+
+            case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING: {
+
                 break;
             }
-            
-            case MODE_JOINT_POSITION_PRES_CONTROL_PWM:
-            {
-                joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
-                joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
-                joint_pos_err_old = joint_pos_err;
-                joint_pos_err_sum += joint_pos_err;
-                if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
-                if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
-                VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff) * 0.01;
-                
-                joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
-                joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
-                joint_torq_err_old = joint_torq_err;
-                joint_torq_err_sum += joint_torq_err;
-                if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
-                if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
-                VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff;
-                
+
+            case MODE_JOINT_POSITION_PRES_CONTROL_PWM: {
+                pos.err = pos.ref - (double) pos.sen;
+                pos.err_diff = pos.err - pos.err_old;
+                pos.err_old = pos.err;
+                pos.err_sum += pos.err;
+                if (pos.err_sum > 1000) pos.err_sum = 1000;
+                if (pos.err_sum<-1000) pos.err_sum = -1000;
+                VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * pos.err + (double) I_GAIN_JOINT_POSITION * pos.err_sum + (double) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01;
+
+                torq.err = torq.ref - torq.sen;
+                torq.err_diff = torq.err - torq.err_old;
+                torq.err_old = torq.err;
+                torq.err_sum += torq.err;
+                if (torq.err_sum > 1000) torq.err_sum = 1000;
+                if (torq.err_sum<-1000) torq.err_sum = -1000;
+                VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
+
                 VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
-                
-                VALVE_PWM_RAW = VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
-                
+
+                V_out = VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
+
                 CUR_FLOWRATE = (double) CUR_VELOCITY * 0.00009587;
                 CUR_FLOWRATE = CUR_FLOWRATE * 0.5757; // 0.4791=2*pi/65536*5000(pulse/tic to rad/s) 0.5757=0.02525*0.02*0.0095*2*60*1000 (radius * area * 2 * 60(sec --> min) * 1000(m^3 --> L))
                 if (DIR_VALVE > 0) {
@@ -624,126 +641,288 @@
                     else VALVE_FF_VOLTAGE = 0;
                 }
                 //            VALVE_FF_VOLTAGE = CUR_FLOWRATE * 0.5;
-                
+
                 if (CUR_FLOWRATE >= 0) VALVE_FF_VOLTAGE = (double) VELOCITY_COMP_GAIN * 0.001 * VALVE_FF_VOLTAGE * sqrt((double) PRES_SUPPLY - CUR_PRES_A_BAR) * 0.0707; // 0.0707 = 1/sqrt(200.))
                 else if (CUR_FLOWRATE < 0) VALVE_FF_VOLTAGE = (double) VELOCITY_COMP_GAIN * 0.001 * VALVE_FF_VOLTAGE * sqrt((double) PRES_SUPPLY - CUR_PRES_B_BAR) * 0.0707;
+
+                V_out = V_out + VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE;
+                break;
+            }
+
+            case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: {
+                pos.err = pos.ref - (double) pos.sen;
+                pos.err_diff = pos.err - pos.err_old;
+                pos.err_old = pos.err;
+                pos.err_sum += pos.err;
+                if (pos.err_sum > 1000) pos.err_sum = 1000;
+                if (pos.err_sum<-1000) pos.err_sum = -1000;
+                VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * pos.err + (double) I_GAIN_JOINT_POSITION * pos.err_sum + (double) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01;
+
+                torq.err = torq.ref - (double) torq.sen;
+                torq.err_diff = torq.err - torq.err_old;
+                torq.err_old = torq.err;
+                torq.err_sum += torq.err;
+                if (torq.err_sum > 1000) torq.err_sum = 1000;
+                if (torq.err_sum<-1000) torq.err_sum = -1000;
+                VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
+
+                valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
+                VALVE_POS_CONTROL(valve_pos.ref);
                 
-                VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE;
+                break;
+            }
+
+            case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: {
+
+                break;
+            }
+
+           
+            case MODE_TEST_CURRENT_CONTROL:
+            {
+                if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
+                    TMR3_COUNT_IREF = 0;
+                } TMR3_COUNT_IREF++;
+            
+                // Set Current Reference 
+                double TMR3_CNT_MAX = (double)TMR_FREQ_5k/2.0;
+                double I_REF_MID = 0.0;
+                if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
+                    I_REF = I_REF_MID + 1.0;
+                } else {
+                    I_REF = I_REF_MID - 1.0;
+                }    
+//              double T = 1.0; // wave period
+//              I_REF = (5. * sin(2. * 3.1415 * (double) TMR3_COUNT_IREF / (double)TMR_FREQ_5k/ T));
+//              I_REF = (2.0 * sin(2. * 2. * 3.14 * (double) TMR3_COUNT_IREF / 5000.)+(2.0 * sin(2. * 1. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 5. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 10. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.)));
+            
+                if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
+                    CAN_TX_PRES((int16_t)(I_REF*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas  
+                }
+                break;
+            }
+                    
+            case MODE_TEST_PWM_CONTROL:
+            {
+                if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
+                    TMR3_COUNT_IREF = 0;
+                } TMR3_COUNT_IREF++;
+                
+                // Set PWM reference
+                double TMR3_CNT_MAX = (double)TMR_FREQ_5k/2.0;
+                double I_REF_MID = 0.0;
+                if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
+                    CUR_PWM = 1000;
+                } else {
+                    CUR_PWM = -1000;
+                }    
+         
+                if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
+                    CAN_TX_PRES((int16_t)(u_CUR[0]*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas
+                }
                 break;
             }
             
-            case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION:
-            {
-                joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION;
-                joint_pos_err_diff = joint_pos_err - joint_pos_err_old;
-                joint_pos_err_old = joint_pos_err;
-                joint_pos_err_sum += joint_pos_err;
-                if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000;
-                if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000;
-                VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff) * 0.01;
+            
+            case MODE_CURRENT_CONTROL: {
+
+                cur.ref = cur.ref; // Unit : mA
+                CurrentControl();
+                break;
+            }
+            
+            case MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT: {
+                double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
+                double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
+                double I_REF_FORCE_FB = 0.0; // I_REF by Force Feedback
+                double I_REF_FORCE_FF = 0.0; // I_REF by Force Feedforward
+    
+                // feedback input for position control
+                pos.err = pos.ref - pos.sen;
+                double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
+                double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
+                pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
+                pos.err_old = pos.err;
+                I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
+    
+                // feedforward input for position control
+                double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
+                double K_ff = 1.3;
+                double K_v = 0.0;
+                if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
+                if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
+                I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
                 
-                joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM;
-                joint_torq_err_diff = joint_torq_err - joint_torq_err_old;
-                joint_torq_err_old = joint_torq_err;
-                joint_torq_err_sum += joint_torq_err;
-                if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000;
-                if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000;
-                VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff;
+                // feedback input for position control
+                I_REF_FORCE_FB = 0.0;
                 
-                Ref_Valve_Pos = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
+                // feedforward input for position control
+                I_REF_FORCE_FF = 0.0;
+    
+                cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
                 
-                valve_pos_err = Ref_Valve_Pos - value;
-                valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
-                valve_pos_err_old = valve_pos_err;
-                valve_pos_err_sum += valve_pos_err;
-                if (valve_pos_err_sum > 1000) valve_pos_err_sum = 1000;
-                if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000;
-                
-                VALVE_PWM_RAW = (double) P_GAIN_VALVE_POSITION * valve_pos_err + (double) I_GAIN_VALVE_POSITION * valve_pos_err_sum + (double) D_GAIN_VALVE_POSITION * valve_pos_err_diff;
+                CurrentControl();
                 
                 break;
             }
             
-            case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING:
-            {
+            case MODE_JOINT_POSITION_PRES_CONTROL_CURRENT: {
+                double T_REF = 0.0; // Torque Reference
+                double I_REF_FORCE_FB = 0.; // I_REF by Force Feedback
+                double I_REF_VC = 0.; // I_REF for velocity compensation
+                
+                // feedback input for position control
+                double Joint_Pos_Err = 34.0-(double) pos.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] 
+                double Joint_Vel_Err = 0.0-(double) vel.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] 
+                double K_spring = 0.7;
+                double D_damper = 0.02;
+//              T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm
+                
+                T_REF = torq.ref; // unit : Nm
+            
+                // torque feedback
+                torq.err = T_REF - (double) torq.sen;
+                //            torq.err_diff = torq.err - torq.err_old;
+                //            torq.err_old = torq.err;
+                torq.err_sum = torq.err_sum + torq.err/(double)TMR_FREQ_5k;
+                I_REF_FORCE_FB = 0.001*((double)P_GAIN_JOINT_TORQUE * torq.err + (double)I_GAIN_JOINT_TORQUE * torq.err_sum); 
+                
+                // velocity compensation for torque control
+                double Joint_Vel_Act = vel.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] 
+                double K_vc = 1.5; // Velocity comp. gain
+                double K_v = 0.0; // Valve gain 
+                if(Joint_Vel_Act > 0) K_v = 1.0/100.0; // open, tuning
+                if(Joint_Vel_Act < 0) K_v = 1.0/100.0; // close, tuning
+                I_REF_VC = K_vc*K_v*Joint_Vel_Act;
                 
+                cur.ref = I_REF_VC + I_REF_FORCE_FB;
+    //            cur.ref = I_REF_FORCE_FB;
+    
+                double I_MAX = 10.00; // Maximum Current : 10mV 
+                double Ka = 1.0/I_GAIN_JOINT_TORQUE;
+                if(cur.ref > I_MAX) {
+                    double I_rem = cur.ref-I_MAX;
+                    I_rem = Ka*I_rem;
+                    cur.ref = I_MAX;
+                    torq.err_sum = torq.err_sum - I_rem/(double)TMR_FREQ_5k;
+                } else if(cur.ref < -I_MAX) {
+                    double I_rem = cur.ref-(-I_MAX);
+                    I_rem = Ka*I_rem; 
+                    cur.ref = -I_MAX;
+                    torq.err_sum = torq.err_sum - I_rem/(double)TMR_FREQ_5k;
+                }
+                
+                CurrentControl();
+                
+                
+                /*
+                double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
+                double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
+                double I_REF_FORCE_FB = 0.0; // I_REF by Force Feedback
+                double I_REF_FORCE_FF = 0.0; // I_REF by Force Feedforward
+    
+                // feedback input for position control
+                pos.err = pos.ref - pos.sen;
+                double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
+                double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
+                pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
+                pos.err_old = pos.err;
+                I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
+    
+                // feedforward input for position control
+                double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
+                double K_ff = 1.3;
+                double K_v = 0.0;
+                if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
+                if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
+                I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
+                
+                // feedback input for position control
+                I_REF_FORCE_FB = 0.0;
+                
+                // feedforward input for position control
+                I_REF_FORCE_FF = 0.0;
+    
+                cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
+                
+                CurrentControl();
+                */
+               
                 break;
             }
-            
-            case MODE_TORQUE_SENSOR_NULLING:
-            {
+
+            case MODE_TORQUE_SENSOR_NULLING: {
                 // DAC Voltage reference set
-                if (TMR2_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
+                if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
                     CUR_TORQUE_sum += CUR_TORQUE;
-                    
-                    if (TMR2_COUNT_TORQUE_NULL % 10 == 0) {
+
+                    if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
                         CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0;
                         CUR_TORQUE_sum = 0;
-                        
+
                         TORQUE_VREF += 0.0001 * (TORQUE_NULL - CUR_TORQUE_mean);
-                        
+
                         //                    TORQUE_VREF = 0;
                         if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3;
                         if (TORQUE_VREF < 0) TORQUE_VREF = 0;
-                        
+
                         spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
                         spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
                     }
                 } else {
                     CONTROL_MODE = MODE_NO_ACT;
-                    TMR2_COUNT_TORQUE_NULL = 0;
+                    TMR3_COUNT_TORQUE_NULL = 0;
                     CUR_TORQUE_sum = 0;
                     CUR_TORQUE_mean = 0;
                     spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
-                    
+
                 }
-                TMR2_COUNT_TORQUE_NULL++;
+                TMR3_COUNT_TORQUE_NULL++;
                 break;
-            }
-            
-            case MODE_VALVE_NULLING_AND_DEADZONE_SETTING:
-            {
-                if (TMR2_COUNT_DEADZONE == 0) {
+            }     
+
+            case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
+                if (TMR3_COUNT_DEADZONE == 0) {
                     if (pos_plus_end == pos_minus_end) need_enc_init = true;
                     else temp_time = 0;
                 }
                 if (need_enc_init) {
-                    if (TMR2_COUNT_DEADZONE < (int) (0.5 * (double) TMR_FREQ_5k)) {
+                    if (TMR3_COUNT_DEADZONE < (int) (0.5 * (double) TMR_FREQ_5k)) {
                         VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                        pos_plus_end = CUR_POSITION;
-                    } else if (TMR2_COUNT_DEADZONE < TMR_FREQ_5k) {
+                        pos_plus_end = pos.sen;
+                    } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
                         VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                        pos_minus_end = CUR_POSITION;
-                    } else if (TMR2_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
+                        pos_minus_end = pos.sen;
+                    } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
                     temp_time = TMR_FREQ_5k;
                 }
-                
-                if (temp_time <= TMR2_COUNT_DEADZONE && TMR2_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
-                    VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
+
+                if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
+                    VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
                     VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
-                    
-                } else if (temp_time <= TMR2_COUNT_DEADZONE && TMR2_COUNT_DEADZONE < (temp_time + (int) (1.9 * (double) TMR_FREQ_5k))) {
+
+                } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9 * (double) TMR_FREQ_5k))) {
                     VALVE_PWM_RAW = 0;
                     CUR_VELOCITY_sum += CUR_VELOCITY;
-                } else if (TMR2_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) {
+                } else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) {
                     if (CUR_VELOCITY_sum == 0) DZ_dir = 1;
                     else if (CUR_VELOCITY_sum > 0) DZ_dir = 1;
                     else if (CUR_VELOCITY_sum < 0) DZ_dir = -1;
                     else DZ_temp_cnt2 = DZ_end;
                     CUR_VELOCITY_sum = 0;
-                } else if (TMR2_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
-                    if (TMR2_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end;
-                    
+                } else if (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
+                    if (TMR3_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end;
+
                     // Position of Dead Zone
                     //  (CUR_VELOCITY < 0)  (CUR_VELOCITY == 0)  (CUR_VELOCITY > 0)
-                    //     |        /                 |    /                      |/ 
+                    //     |        /                 |    /                      |/
                     //     | ______/               ___|___/                ______/|
                     //     |/                     /   |                   /       |
                     //    /|                     /    |                  /        |
                     //     0V                         0V                          0V
-                    
+
                     if (DZ_temp_cnt2 < DZ_end) {
-                        if (TMR2_COUNT_DEADZONE % 20 != 0) {
+                        if (TMR3_COUNT_DEADZONE % 20 != 0) {
                             CUR_VELOCITY_sum += CUR_VELOCITY;
                         } else {
                             VALVE_PWM_RAW -= DZ_dir;
@@ -758,7 +937,7 @@
                             DZ_temp_cnt2++;
                         }
                     } else {
-                        TMR2_COUNT_DEADZONE = -1;
+                        TMR3_COUNT_DEADZONE = -1;
                         VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2;
                         if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) {
                             VALVE_DEADZONE_PLUS = VALVE_CENTER;
@@ -767,28 +946,80 @@
                         VALVE_PWM_RAW = 0;
                         spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
                         spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
-                        
+
                         CONTROL_MODE = MODE_NO_ACT;
                         DZ_temp_cnt2 = 0;
                     }
                 }
-                TMR2_COUNT_DEADZONE++;
+                TMR3_COUNT_DEADZONE++;
                 break;
             }
-            
-            case MODE_FIND_HOME:
-            {
+
+            case MODE_FIND_HOME: {
+                
+                
+                ////////////////////////////////////////////FIND_HOME_PWM/////////////////////////////////////////////////////
+                if (FLAG_FIND_HOME == true) {
+                    cnt_findhome = 0;
+                    cnt_vel_findhome = 0;
+                    //REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
+                    pos.ref = pos.sen;
+                    FLAG_FIND_HOME = false;
+                }
+
+                int check_enc = (TMR_FREQ_5k/10);
+                if(cnt_findhome%check_enc == 0) {
+                    FINDHOME_POSITION = pos.sen;
+                    FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
+                    FINDHOME_POSITION_OLD = FINDHOME_POSITION;
+                }
+                cnt_findhome++;
+                if(cnt_findhome == 10000) cnt_findhome = 0;
+
+                if (abs(FINDHOME_VELOCITY) <= 1) {
+                    cnt_vel_findhome = cnt_vel_findhome + 1;
+                } else {
+                    cnt_vel_findhome = 0;
+                }
+
+                if (cnt_vel_findhome < 2*TMR_FREQ_5k) {
+                    //REFERENCE_MODE = MODE_REF_NO_ACT;
+                    if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 1.0;
+                    else pos.ref = pos.ref - 1.0;
+                    pos.err = pos.ref - (double) pos.sen;
+                    V_out = ((double) P_GAIN_JOINT_POSITION * pos.err) * 0.01;
+                    //                if (HOMEPOS_OFFSET > 0) VALVE_PWM_RAW = 100;
+                    //                else VALVE_PWM_RAW = -100;
+                } else {
+                    ENC_SET(HOMEPOS_OFFSET);
+                    FLAG_REFERENCE_JOINT_POSITION = 1;
+                    INIT_REF_POS = HOMEPOS_OFFSET;
+                    REF_MOVE_TIME_5k = 2 * TMR_FREQ_5k;
+                    TMR3_COUNT_REFERENCE = 0;
+                    vel.ref = 0.0;
+                    //REFERENCE_MODE = MODE_REF_COS_INC;
+                    CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_PWM;
+
+                    FINDHOME_POSITION = 0;
+                    FINDHOME_POSITION_OLD = 0;
+                    FINDHOME_VELOCITY = 0;
+                    cnt_findhome = 0;
+                    cnt_vel_findhome = 0;
+                }
+                
+                /*
+                ////////////////////////////////////////////FIND_HOME_CURRENT/////////////////////////////////////////////////////
                 if (FLAG_FIND_HOME == true) {
                     cnt_findhome = 0;
                     cnt_vel_findhome = 0;
                     REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
-                    Ref_Joint_Pos = CUR_POSITION;
+                    Ref_Joint_Pos = pos.sen;
                     FLAG_FIND_HOME = false;
                 }
                 
-                int check_enc = (TMR_FREQ_5k/10);
-                if(cnt_findhome%check_enc == 0){
-                    FINDHOME_POSITION = CUR_POSITION;
+                int cnt_check_enc = (TMR_FREQ_5k/500);
+                if(cnt_findhome%cnt_check_enc == 0){
+                    FINDHOME_POSITION = pos.sen;
                     FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
                     FINDHOME_POSITION_OLD = FINDHOME_POSITION;
                 } cnt_findhome++;
@@ -800,23 +1031,25 @@
                     cnt_vel_findhome = 0;
                 }
                 
-                if (cnt_vel_findhome < 2*TMR_FREQ_5k) {
+                if (cnt_vel_findhome < 3*TMR_FREQ_5k) { // wait for 3sec
                     REFERENCE_MODE = MODE_REF_NO_ACT;
                     if (HOMEPOS_OFFSET > 0) Ref_Joint_Pos = Ref_Joint_Pos + 1.0;
                     else Ref_Joint_Pos = Ref_Joint_Pos - 1.0;
-                    joint_pos_err = Ref_Joint_Pos - CUR_POSITION;
-                    VALVE_PWM_RAW = ((double) P_GAIN_JOINT_POSITION * joint_pos_err) * 0.01;
-    //                if (HOMEPOS_OFFSET > 0) VALVE_PWM_RAW = 100;
-    //                else VALVE_PWM_RAW = -100;
+                    pos.err = Ref_Joint_Pos - pos.sen;
+                    I_REF = 0.001*((double)pos.err);    
+                    if(I_REF>5.0) I_REF = 5.0;
+                    if(I_REF<-5.0) I_REF = -5.0;
+                    cur.ref = I_REF;
+                    CurrentControl();
                 } else {
                     ENC_SET(HOMEPOS_OFFSET);
                     FLAG_REFERENCE_JOINT_POSITION = 1;
                     INIT_REF_POS = HOMEPOS_OFFSET;
                     REF_MOVE_TIME_5k = 2 * TMR_FREQ_5k;
-                    TMR2_COUNT_REFERENCE = 0;
+                    TMR3_COUNT_REFERENCE = 0;
                     Ref_Joint_Vel = 0.0;
                     REFERENCE_MODE = MODE_REF_COS_INC;
-                    CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_PWM;
+                    CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION;
                     
                     FINDHOME_POSITION = 0;
                     FINDHOME_POSITION_OLD = 0;
@@ -824,12 +1057,13 @@
                     cnt_findhome = 0;
                     cnt_vel_findhome = 0;
                 }
+                */
+                
                 break;
             }
-            
-            case MODE_VALVE_GAIN_SETTING:
-            {
-                if (TMR2_COUNT_FLOWRATE == 0) {
+
+            case MODE_VALVE_GAIN_SETTING: {
+                if (TMR3_COUNT_FLOWRATE == 0) {
                     if (pos_plus_end == pos_minus_end) need_enc_init = true;
                     else {
                         VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
@@ -837,13 +1071,13 @@
                     }
                 }
                 if (need_enc_init) {
-                    if (TMR2_COUNT_FLOWRATE < (int) (0.5 * (double) TMR_FREQ_5k)) {
+                    if (TMR3_COUNT_FLOWRATE < (int) (0.5 * (double) TMR_FREQ_5k)) {
                         VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                        pos_plus_end = CUR_POSITION;
-                    } else if (TMR2_COUNT_FLOWRATE < TMR_FREQ_5k) {
+                        pos_plus_end = pos.sen;
+                    } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
                         VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                        pos_minus_end = CUR_POSITION;
-                    } else if (TMR2_COUNT_FLOWRATE == TMR_FREQ_5k) {
+                        pos_minus_end = pos.sen;
+                    } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
                         need_enc_init = false;
                         check_vel_pos_init = (int) (0.9 * (double) (pos_plus_end - pos_minus_end));
                         check_vel_pos_fin = (int) (0.95 * (double) (pos_plus_end - pos_minus_end));
@@ -851,14 +1085,14 @@
                     }
                     temp_time = TMR_FREQ_5k;
                 }
-                TMR2_COUNT_FLOWRATE++;
-                if (TMR2_COUNT_FLOWRATE > temp_time) {
+                TMR3_COUNT_FLOWRATE++;
+                if (TMR3_COUNT_FLOWRATE > temp_time) {
                     if (flag_flowrate % 2 == 0) { // (+)
                         VALVE_VOLTAGE = 1. * (double) (flag_flowrate / 2 + 1);
                         VALVE_PWM_RAW = VALVE_VOLTAGE / SUPPLY_VOLTAGE*PWM_RESOL;
-                        if (CUR_POSITION > (pos_minus_end + check_vel_pos_init) && CUR_POSITION < (pos_minus_end + check_vel_pos_fin)) {
+                        if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) {
                             fl_temp_cnt++;
-                        } else if (CUR_POSITION >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
+                        } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
                             VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873 * 0.5757 * (double) TMR_FREQ_5k / 10000.0 * (double) check_vel_pos_interv / (double) 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] = (double) TMR_FREQ_10k * (double) check_vel_pos_interv / (double) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
                             fl_temp_cnt2++;
@@ -866,9 +1100,9 @@
                     } else if (flag_flowrate % 2 == 1) { // (-)
                         VALVE_VOLTAGE = -1. * (double) (flag_flowrate / 2 + 1);
                         VALVE_PWM_RAW = VALVE_VOLTAGE / SUPPLY_VOLTAGE*PWM_RESOL;
-                        if (CUR_POSITION < (pos_plus_end - check_vel_pos_init) && CUR_POSITION > (pos_plus_end - check_vel_pos_fin)) {
+                        if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) {
                             fl_temp_cnt++;
-                        } else if (CUR_POSITION <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
+                        } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
                             VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873 * 0.5757 * (double) TMR_FREQ_5k / 10000.0 * (double) check_vel_pos_interv / (double) fl_temp_cnt / (-VALVE_VOLTAGE);
                             //                        VALVE_GAIN_LPM_PER_V[flag_flowrate] = (double) TMR_FREQ_10k * (double) check_vel_pos_interv / (double) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
                             fl_temp_cnt2++;
@@ -884,74 +1118,73 @@
                     if (flag_flowrate == 10) {
                         VALVE_PWM_RAW = 0;
                         flag_flowrate = 0;
-                        TMR2_COUNT_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:
-            {
+
+            case MODE_PRESSURE_SENSOR_NULLING: {
                 // DAC Voltage reference set
-                if (TMR2_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
-                    CUR_PRES_A_sum += CUR_PRES_A;
-                    CUR_PRES_B_sum += CUR_PRES_B;
-    
-                    if (TMR2_COUNT_PRES_NULL % 10 == 0) {
+                if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
+                    CUR_PRES_A_sum += pres_A.sen;
+                    CUR_PRES_B_sum += pres_B.sen;
+
+                    if (TMR3_COUNT_PRES_NULL % 10 == 0) {
                         CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0;
                         CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0;
                         CUR_PRES_A_sum = 0;
                         CUR_PRES_B_sum = 0;
-    
+
                         double VREF_NullingGain = 0.0003;
                         PRES_A_VREF -= VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
                         PRES_B_VREF -= VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
-    
+
                         if (PRES_A_VREF > 3.3) PRES_A_VREF = 3.3;
                         if (PRES_A_VREF < 0) PRES_A_VREF = 0;
                         if (PRES_B_VREF > 3.3) PRES_B_VREF = 3.3;
                         if (PRES_B_VREF < 0) PRES_B_VREF = 0;
-    
+
                         spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
                         spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
                     }
                 } else {
                     CONTROL_MODE = MODE_NO_ACT;
-                    TMR2_COUNT_PRES_NULL = 0;
+                    pc.printf("no_act");
+                    TMR3_COUNT_PRES_NULL = 0;
                     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_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
                     spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
                 }
-                TMR2_COUNT_PRES_NULL++;
+                TMR3_COUNT_PRES_NULL++;
                 break;
             }
-            
-            case MODE_PRESSURE_SENSOR_CALIB:
-            {
-                if (TMR2_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
+
+            case MODE_PRESSURE_SENSOR_CALIB: {
+                if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
                     VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL / 5.;
-                    if (TMR2_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
+                    if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
                         CUR_PRES_A_sum += CUR_PRES_A;
                     }
-                } else if (TMR2_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
+                } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
                     VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL / 5.;
-                    if (TMR2_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
+                    if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
                         CUR_PRES_B_sum += CUR_PRES_B;
                     }
                 } else {
                     CONTROL_MODE = MODE_NO_ACT;
-                    TMR2_COUNT_PRES_CALIB = 0;
+                    TMR3_COUNT_PRES_CALIB = 0;
                     VALVE_PWM_RAW = 0;
                     PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((double) TMR_FREQ_5k - 1.) - PRES_A_NULL;
                     PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.);
@@ -961,73 +1194,57 @@
                     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.0));
                     spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0));
                 }
-                TMR2_COUNT_PRES_CALIB++;
+                TMR3_COUNT_PRES_CALIB++;
                 break;
             }
-            
-            case MODE_ROTARY_FRICTION_TUNING:
-            {
-                if (TMR2_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4 + 3. * sin(2 * 3.14159 * 0.5 * TMR2_COUNT_ROTARY_FRIC_TUNE * 0.0001 * 0.05);
-                VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR2_COUNT_ROTARY_FRIC_TUNE * 0.0001);
+
+            case MODE_ROTARY_FRICTION_TUNING: {
+                if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4 + 3. * sin(2 * 3.14159 * 0.5 * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001 * 0.05);
+                VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001);
                 if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL;
                 else VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                TMR2_COUNT_ROTARY_FRIC_TUNE++;
-                if (TMR2_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
-                    TMR2_COUNT_ROTARY_FRIC_TUNE = 0;
+                TMR3_COUNT_ROTARY_FRIC_TUNE++;
+                if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
+                    TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
                     VALVE_PWM_RAW = 0;
                     CONTROL_MODE = MODE_NO_ACT;
                 }
                 break;
             }
-            
-            case MODE_DDV_POS_VS_PWM_ID:
-            {
+
+            case MODE_DDV_POS_VS_PWM_ID: {
                 VALVE_ID_timer = VALVE_ID_timer + 1;
-                
-                if(VALVE_ID_timer < TMR_FREQ_5k*1)
-                {
+
+                if(VALVE_ID_timer < TMR_FREQ_5k*1) {
                     VALVE_PWM_RAW = 3000 * sin(2*3.14*VALVE_ID_timer/TMR_FREQ_5k * 100);
-                }
-                else if(VALVE_ID_timer < TMR_FREQ_5k*2)
-                {
+                } else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
                     VALVE_PWM_RAW = 1000*(ID_index_array[ID_index]);
-                }
-                else if(VALVE_ID_timer == TMR_FREQ_5k*2)
-                {
+                } 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)
-                {
+                } 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)
-                {
+                } else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
                     VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num;
                     VALVE_ID_timer = 0;
                     ID_index= ID_index +1;
                 }
-                
-                if(ID_index>=18)
-                {
+
+                if(ID_index>=18) {
                     int i;
                     VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
-                    for(i=0;i<18;i++)
-                    {
+                    for(i=0; i<18; i++) {
                         VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
                         spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) (VALVE_POS_AVG[i]));
-                        if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD)
-                        {
+                        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)
-                        {
+                        } else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) {
                             VALVE_MIN_POS = VALVE_POS_AVG[i];
                             VALVE_POS_AVG_OLD = VALVE_MIN_POS;
                         }
@@ -1038,103 +1255,77 @@
                     ID_index = 0;
                     CONTROL_MODE = MODE_NO_ACT;
                 }
-                
-                
+
+
                 break;
             }
-            
-            case MODE_DDV_DEADZONE_AND_CENTER:
-            {
-                
+
+            case MODE_DDV_DEADZONE_AND_CENTER: {
+
                 VALVE_DZ_timer = VALVE_DZ_timer + 1;
-                if(first_check == 0)
-                {
-                    if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                    }
-                    else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                        pos_plus_end = CUR_POSITION;
-                    }
-                    else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                    }
-                    else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                        pos_minus_end = CUR_POSITION;
-                    }
-                    else if(VALVE_DZ_timer < (int) (3.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
-                    }
-                    else if(VALVE_DZ_timer < (int) (4.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
+                if(first_check == 0) {
+                    if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
+                        V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+                    } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
+                        V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+                        pos_plus_end = pos.sen;
+                    } else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) {
+                        V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+                    } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
+                        V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+                        pos_minus_end = pos.sen;
+                    } else if(VALVE_DZ_timer < (int) (3.0 * (double) TMR_FREQ_5k)) {
+                        V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
+                    } else if(VALVE_DZ_timer < (int) (4.0 * (double) TMR_FREQ_5k)) {
+                        V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
                         data_num = data_num + 1;
                         VALVE_POS_TMP = VALVE_POS_TMP + value;
-                    }
-                    else if(VALVE_DZ_timer == (int) (4.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
+                    } else if(VALVE_DZ_timer == (int) (4.0 * (double) TMR_FREQ_5k)) {
+                        V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
                         DDV_POS_AVG = VALVE_POS_TMP / data_num;
-                        START_POS = CUR_POSITION;
+                        START_POS = pos.sen;
                         CAN_TX_PRES((int16_t) (DDV_POS_AVG), (int16_t) (data_num));
                         VALVE_POS_TMP = 0;
                         data_num = 0;
-                        
-                    }
-                    else if(VALVE_DZ_timer < (int) (5.0 * (double) TMR_FREQ_5k))
-                    {
-                        Ref_Valve_Pos = DDV_POS_AVG;
-                        VALVE_POS_CONTROL(Ref_Valve_Pos);
+
+                    } else if(VALVE_DZ_timer < (int) (5.0 * (double) TMR_FREQ_5k)) {
+                        valve_pos.ref = DDV_POS_AVG;
+                        VALVE_POS_CONTROL(valve_pos.ref);
                         //CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS));
-                        
-                    }
-                    else if(VALVE_DZ_timer < (int) (6.0 * (double) TMR_FREQ_5k))
-                    {
-                        Ref_Valve_Pos = DDV_POS_AVG;
-                        VALVE_POS_CONTROL(Ref_Valve_Pos);
+
+                    } else if(VALVE_DZ_timer < (int) (6.0 * (double) TMR_FREQ_5k)) {
+                        valve_pos.ref = DDV_POS_AVG;
+                        VALVE_POS_CONTROL(valve_pos.ref);
                         if(CUR_VELOCITY >= 0)
                             VEL_POINT = VEL_POINT + 1;
                         else
                             VEL_POINT = VEL_POINT - 1;
-                    }
-                    else if(VALVE_DZ_timer == (int) (6.0 * (double) TMR_FREQ_5k))
-                    {
-                        Ref_Valve_Pos = DDV_POS_AVG;
-                        VALVE_POS_CONTROL(Ref_Valve_Pos);
-                        FINAL_POS = CUR_POSITION;
-    //                    if(VEL_POINT >= 0)
-    //                    {
-    //                        DZ_case = 1;
-    //                    }
-    //                    else
-    //                    {
-    //                        DZ_case = -1;
-    //                    }
-    //                    if(abs(FINAL_POS - START_POS) < 100)
-    //                    {
-    //                        DZ_case = 0;
-    //                    }
-                        
-                        if((FINAL_POS - START_POS)>100)
-                        {
+                    } else if(VALVE_DZ_timer == (int) (6.0 * (double) TMR_FREQ_5k)) {
+                        valve_pos.ref = DDV_POS_AVG;
+                        VALVE_POS_CONTROL(valve_pos.ref);
+                        FINAL_POS = pos.sen;
+                        //                    if(VEL_POINT >= 0)
+                        //                    {
+                        //                        DZ_case = 1;
+                        //                    }
+                        //                    else
+                        //                    {
+                        //                        DZ_case = -1;
+                        //                    }
+                        //                    if(abs(FINAL_POS - START_POS) < 100)
+                        //                    {
+                        //                        DZ_case = 0;
+                        //                    }
+
+                        if((FINAL_POS - START_POS)>100) {
                             DZ_case = 1;
-                        }
-                        else if((FINAL_POS - START_POS)<-100)
-                        {
+                        } else if((FINAL_POS - START_POS)<-100) {
                             DZ_case = -1;
-                        }
-                        else
-                        {
+                        } else {
                             DZ_case = 0;
                         }
-                        
-                        VEL_POINT = 0;     
+
+                        VEL_POINT = 0;
                         first_check = 1;
                         DZ_DIRECTION = 1;
                         VALVE_DZ_timer = 0;
@@ -1142,137 +1333,100 @@
                         DZ_NUM = 1;
                         DZ_index = 1;
                         CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM));
-                        
+
                     }
-                }
-                else
-                {
-                    if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1))
-                    {
-                        if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
-                        {
-                            VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
-                        }
-                        else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
-                        {
-                            START_POS = CUR_POSITION;
-                        }
-                        else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
-                        {
-                            Ref_Valve_Pos = Ref_Valve_Pos_Old  - DZ_case * DZ_DIRECTION * 64 / DZ_index;
-                            if(Ref_Valve_Pos <= VALVE_MIN_POS)
-                            {
-                                Ref_Valve_Pos = VALVE_MIN_POS;
+                }else {
+                    if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) {
+                        if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
+                            V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
+                        } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
+                            START_POS = pos.sen;
+                        } else if(VALVE_DZ_timer < (int) (2.0 * (double) 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;
                             }
-                            else if(Ref_Valve_Pos >= VALVE_MAX_POS)
-                            {
-                                Ref_Valve_Pos = VALVE_MAX_POS;
-                            }
-                            VALVE_POS_CONTROL(Ref_Valve_Pos);
+                            VALVE_POS_CONTROL(valve_pos.ref);
                             if(CUR_VELOCITY >= 0)
                                 VEL_POINT = VEL_POINT + 1;
                             else
                                 VEL_POINT = VEL_POINT - 1;
-                        }
-                        else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
-                        {
-                            Ref_Valve_Pos_Old = Ref_Valve_Pos;
-                            FINAL_POS = CUR_POSITION;
-    //                        if(VEL_POINT >= 0)
-    //                            DZ_DIRECTION = 1 * DZ_case;
-    //                        else
-    //                            DZ_DIRECTION = -1 * DZ_case;
-    //                        if(abs(FINAL_POS - START_POS) < 100)
-    //                            DZ_DIRECTION = 1 * DZ_case;
-                            
-                            if((FINAL_POS - START_POS)>100)
-                            {
+                        } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
+                            Ref_Valve_Pos_Old = valve_pos.ref;
+                            FINAL_POS = pos.sen;
+                            //                        if(VEL_POINT >= 0)
+                            //                            DZ_DIRECTION = 1 * DZ_case;
+                            //                        else
+                            //                            DZ_DIRECTION = -1 * DZ_case;
+                            //                        if(abs(FINAL_POS - START_POS) < 100)
+                            //                            DZ_DIRECTION = 1 * DZ_case;
+
+                            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;
                             }
-                            else if((FINAL_POS - START_POS)<-100)
-                            {
-                                DZ_DIRECTION = -1 * DZ_case;
-                            }
-                            else
-                            {
-                                DZ_DIRECTION = 1 * DZ_case;
-                            }
-                        
-                            VEL_POINT = 0;  
-                            
+
+                            VEL_POINT = 0;
+
                             VALVE_DZ_timer = 0;
                             DZ_index= DZ_index *2;
-                            if(DZ_index >= 128)
-                            {
-                                FIRST_DZ = Ref_Valve_Pos;
+                            if(DZ_index >= 128) {
+                                FIRST_DZ = valve_pos.ref;
                                 DZ_NUM = 2;
                                 Ref_Valve_Pos_Old = FIRST_DZ;
                                 DZ_index = 1;
                                 DZ_DIRECTION = 1;
                             }
-                            
+
                             //CAN_TX_PRES((int16_t) (1), (int16_t) (DZ_index));
                         }
-                    }
-                    else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2))
-                    {
-                        if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
-                        {
-                            VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
-                        }
-                        else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
-                        {
-                            START_POS = CUR_POSITION;
-                        }
-                        else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
-                        {
-                            Ref_Valve_Pos = Ref_Valve_Pos_Old  + DZ_DIRECTION + 64 / DZ_index;
-                            if(Ref_Valve_Pos <= VALVE_MIN_POS)
-                            {
-                                Ref_Valve_Pos = VALVE_MIN_POS;
+                    } else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) {
+                        if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
+                            V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
+                        } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
+                            START_POS = pos.sen;
+                        } else if(VALVE_DZ_timer < (int) (2.0 * (double) 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;
                             }
-                            else if(Ref_Valve_Pos >= VALVE_MAX_POS)
-                            {
-                                Ref_Valve_Pos = VALVE_MAX_POS;
-                            }
-                            VALVE_POS_CONTROL(Ref_Valve_Pos);
+                            VALVE_POS_CONTROL(valve_pos.ref);
                             if(CUR_VELOCITY >= 0)
                                 VEL_POINT = VEL_POINT + 1;
                             else
                                 VEL_POINT = VEL_POINT - 1;
-                        }
-                        else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
-                        {
-                            Ref_Valve_Pos_Old = Ref_Valve_Pos;
-                            FINAL_POS = CUR_POSITION;
-    //                        if(VEL_POINT >= 0)
-    //                            DZ_DIRECTION = 1 * DZ_case;
-    //                        else
-    //                            DZ_DIRECTION = -1 * DZ_case;
-    //                        if(abs(FINAL_POS - START_POS) < 100)
-    //                            DZ_DIRECTION = -1 * DZ_case;
-                            
-                            if((FINAL_POS - START_POS)>100)
-                            {
+                        } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
+                            Ref_Valve_Pos_Old = valve_pos.ref;
+                            FINAL_POS = pos.sen;
+                            //                        if(VEL_POINT >= 0)
+                            //                            DZ_DIRECTION = 1 * DZ_case;
+                            //                        else
+                            //                            DZ_DIRECTION = -1 * DZ_case;
+                            //                        if(abs(FINAL_POS - START_POS) < 100)
+                            //                            DZ_DIRECTION = -1 * DZ_case;
+
+                            if((FINAL_POS - START_POS)>100) {
                                 DZ_DIRECTION = -1;
-                            }
-                            else if((FINAL_POS - START_POS)<-100)
-                            {
+                            } else if((FINAL_POS - START_POS)<-100) {
+                                DZ_DIRECTION = 1;
+                            } else {
                                 DZ_DIRECTION = 1;
                             }
-                            else
-                            {
-                                DZ_DIRECTION = 1;
-                            }
-                        
-                            VEL_POINT = 0;  
-                            
-                            
+
+                            VEL_POINT = 0;
+
+
                             VALVE_DZ_timer = 0;
                             DZ_index= DZ_index *2;
-                            if(DZ_index >= 128)
-                            {
-                                SECOND_DZ = Ref_Valve_Pos;
+                            if(DZ_index >= 128) {
+                                SECOND_DZ = valve_pos.ref;
                                 DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
                                 first_check = 0;
                                 spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
@@ -1282,122 +1436,91 @@
                                 CONTROL_MODE = MODE_NO_ACT;
                                 DZ_index = 1;
                             }
-                            
+
                             CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index));
                         }
-                    }
-                    else if(DZ_case == 0 && DZ_NUM ==1)
-                    {
-                        if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k))
-                        {
-                            VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
-                        }
-                        else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
-                        {
-                            START_POS = CUR_POSITION;
-                        }
-                        else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
-                        {
-                            Ref_Valve_Pos = Ref_Valve_Pos_Old  - DZ_DIRECTION * 64 / DZ_index;
-                            if(Ref_Valve_Pos <= VALVE_MIN_POS)
-                            {
-                                Ref_Valve_Pos = VALVE_MIN_POS;
+                    } else if(DZ_case == 0 && DZ_NUM ==1) {
+                        if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
+                            V_out = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
+                        } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
+                            START_POS = pos.sen;
+                        } else if(VALVE_DZ_timer < (int) (2.0 * (double) 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;
                             }
-                            else if(Ref_Valve_Pos >= VALVE_MAX_POS)
-                            {
-                                Ref_Valve_Pos = VALVE_MAX_POS;
-                            }
-                            VALVE_POS_CONTROL(Ref_Valve_Pos);
+                            VALVE_POS_CONTROL(valve_pos.ref);
                             if(CUR_VELOCITY >= 0)
                                 VEL_POINT = VEL_POINT + 1;
                             else
                                 VEL_POINT = VEL_POINT - 1;
-                        }
-                        else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
-                        {
-                            Ref_Valve_Pos_Old = Ref_Valve_Pos;
-                            FINAL_POS = CUR_POSITION;
-    //                        if(VEL_POINT >= 0)
-    //                            DZ_DIRECTION = 1;
-    //                        else
-    //                            DZ_DIRECTION = -1;
-    //                        if(abs(FINAL_POS - START_POS) < 100)
-    //                            DZ_DIRECTION = 1;
-                            
-                            if((FINAL_POS - START_POS)>100)
-                            {
+                        } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
+                            Ref_Valve_Pos_Old = valve_pos.ref;
+                            FINAL_POS = pos.sen;
+                            //                        if(VEL_POINT >= 0)
+                            //                            DZ_DIRECTION = 1;
+                            //                        else
+                            //                            DZ_DIRECTION = -1;
+                            //                        if(abs(FINAL_POS - START_POS) < 100)
+                            //                            DZ_DIRECTION = 1;
+
+                            if((FINAL_POS - START_POS)>100) {
+                                DZ_DIRECTION = 1;
+                            } else if((FINAL_POS - START_POS)<-100) {
+                                DZ_DIRECTION = -1;
+                            } else {
                                 DZ_DIRECTION = 1;
                             }
-                            else if((FINAL_POS - START_POS)<-100)
-                            {
-                                DZ_DIRECTION = -1;
-                            }
-                            else
-                            {
-                                DZ_DIRECTION = 1;
-                            }
-                        
-                            VEL_POINT = 0;  
+
+                            VEL_POINT = 0;
                             //CAN_TX_PRES((int16_t) (7), (int16_t) (DZ_DIRECTION));
                             VALVE_DZ_timer = 0;
                             DZ_index= DZ_index *2;
-                            if(DZ_index >= 128)
-                            {
-                                FIRST_DZ = Ref_Valve_Pos;
+                            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.0 * (double) TMR_FREQ_5k))
-                        {
-                            VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
-                        }
-                        else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k))
-                        {
-                            START_POS = CUR_POSITION;
-                        }
-                        else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k))
-                        {
-                            Ref_Valve_Pos = Ref_Valve_Pos_Old  + DZ_DIRECTION * 64 / DZ_index;
-                            if(Ref_Valve_Pos <= VALVE_MIN_POS)
-                            {
-                                Ref_Valve_Pos = VALVE_MIN_POS;
+                    } else {
+                        if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
+                            V_out = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
+                        } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
+                            START_POS = pos.sen;
+                        } else if(VALVE_DZ_timer < (int) (2.0 * (double) 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;
                             }
-                            else if(Ref_Valve_Pos > VALVE_MAX_POS)
-                            {
-                                Ref_Valve_Pos = VALVE_MAX_POS - 1;
-                            }
-                            VALVE_POS_CONTROL(Ref_Valve_Pos);
+                            VALVE_POS_CONTROL(valve_pos.ref);
                             if(CUR_VELOCITY >= 0)
                                 VEL_POINT = VEL_POINT + 1;
                             else
                                 VEL_POINT = VEL_POINT - 1;
-                        }
-                        else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k))
-                        {
-                            Ref_Valve_Pos_Old = Ref_Valve_Pos;
-                            FINAL_POS = CUR_POSITION;
+                        } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
+                            Ref_Valve_Pos_Old = valve_pos.ref;
+                            FINAL_POS = pos.sen;
                             if(VEL_POINT >= 0)
                                 DZ_DIRECTION = -1;
                             else
                                 DZ_DIRECTION = 1;
                             if(abs(FINAL_POS - START_POS) < 100)
                                 DZ_DIRECTION = 1;
-                        
-                            VEL_POINT = 0;  
-                            
-                            
+
+                            VEL_POINT = 0;
+
+
                             VALVE_DZ_timer = 0;
                             //CAN_TX_PRES((int16_t) (3), (int16_t) (DZ_index));
                             DZ_index= DZ_index *2;
-                            if(DZ_index >= 128)
-                            {
-                                SECOND_DZ = Ref_Valve_Pos;
+                            if(DZ_index >= 128) {
+                                SECOND_DZ = valve_pos.ref;
                                 DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
                                 first_check = 0;
                                 VALVE_DEADZONE_MINUS = FIRST_DZ;
@@ -1409,85 +1532,63 @@
                                 CONTROL_MODE = MODE_NO_ACT;
                                 DZ_index = 1;
                             }
-                            
+
                             //CAN_TX_PRES((int16_t) (4), (int16_t) (DZ_index));
                         }
                     }
-                }          
+                }
                 break;
             }
-            
-            case MODE_DDV_POS_VS_FLOWRATE:
-            {
+
+            case MODE_DDV_POS_VS_FLOWRATE: {
                 VALVE_FR_timer = VALVE_FR_timer + 1;
-                if(first_check == 0)
-                {
-                    if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+                if(first_check == 0) {
+                    if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
+                        V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
                         CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
-                    }
-                    else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                        pos_plus_end = CUR_POSITION;
-    //                    CAN_TX_PRES((int16_t) (VALVE_PWM_RAW), (int16_t) (7));
-                    }
-                    else if(VALVE_FR_timer < (int) (2.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                    }
-                    else if(VALVE_FR_timer == (int) (2.0 * (double) TMR_FREQ_5k))
-                    {
-    //                    CAN_TX_PRES((int16_t) (VALVE_PWM_RAW), (int16_t) (8));
-                        VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
-                        pos_minus_end = CUR_POSITION;
+                    } else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
+                        V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+                        pos_plus_end = pos.sen;
+                        //                    CAN_TX_PRES((int16_t) (V_out), (int16_t) (7));
+                    } else if(VALVE_FR_timer < (int) (2.0 * (double) TMR_FREQ_5k)) {
+                        V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+                    } else if(VALVE_FR_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
+                        //                    CAN_TX_PRES((int16_t) (V_out), (int16_t) (8));
+                        V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+                        pos_minus_end = pos.sen;
                         first_check = 1;
                         VALVE_FR_timer = 0;
-                        Ref_Valve_Pos = DDV_CENTER;
+                        valve_pos.ref = DDV_CENTER;
                         ID_index = 0;
                         max_check = 0;
                         min_check = 0;
                     }
-                }
-                else
-                {
-                    if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k))
-                    {
-                        VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION);
-                    }
-                    else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k))
-                    {
+                } else {
+                    if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
+                        V_out = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
+                    } else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
                         data_num = 0;
-                        Ref_Valve_Pos = 50*(ID_index_array[ID_index]) + DDV_CENTER;
-                        
-                        VALVE_POS_CONTROL(Ref_Valve_Pos);
-                        START_POS = CUR_POSITION;
-                    }
-                    else if(VALVE_FR_timer < (int) (5.0 * (double) TMR_FREQ_5k))
-                    {
-                        Ref_Valve_Pos = 50*(ID_index_array[ID_index]) + DDV_CENTER;
-                        VALVE_POS_CONTROL(Ref_Valve_Pos);
+                        valve_pos.ref = 50*(ID_index_array[ID_index]) + DDV_CENTER;
+
+                        VALVE_POS_CONTROL(valve_pos.ref);
+                        START_POS = pos.sen;
+                    } else if(VALVE_FR_timer < (int) (5.0 * (double) TMR_FREQ_5k)) {
+                        valve_pos.ref = 50*(ID_index_array[ID_index]) + DDV_CENTER;
+                        VALVE_POS_CONTROL(valve_pos.ref);
                         data_num = data_num + 1;
-                        if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION) > 20000)
-                        {
-                            FINAL_POS = CUR_POSITION;
-                            one_period_end = 1;   
+                        if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen) > 20000) {
+                            FINAL_POS = pos.sen;
+                            one_period_end = 1;
                         }
-                    }
-                    else if(VALVE_FR_timer == (int) (5.0 * (double) TMR_FREQ_5k))
-                    {
-                        FINAL_POS = CUR_POSITION;
+                    } else if(VALVE_FR_timer == (int) (5.0 * (double) TMR_FREQ_5k)) {
+                        FINAL_POS = pos.sen;
                         one_period_end = 1;
                     }
-    
-                    if(one_period_end == 1){
-                        if(Ref_Valve_Pos > VALVE_MAX_POS)
-                        {
+
+                    if(one_period_end == 1) {
+                        if(valve_pos.ref > VALVE_MAX_POS) {
                             max_check = 1;
-                        }
-                        else if(Ref_Valve_Pos < VALVE_MIN_POS)
-                        {
+                        } 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
@@ -1496,80 +1597,86 @@
                         VALVE_FR_timer = 0;
                         one_period_end = 0;
                         ID_index= ID_index +1;
-    //                    CAN_TX_PRES((int16_t) (Ref_Valve_Pos), (int16_t) (ID_index));
+                        //                    CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (ID_index));
                     }
-    
-                    if(max_check == 1 && min_check == 1)
-                    {
+
+                    if(max_check == 1 && min_check == 1) {
                         spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index));
                         VALVE_POS_NUM = ID_index;
                         ID_index = 0;
                         first_check = 0;
                         VALVE_FR_timer = 0;
-    //                    CAN_TX_PRES((int16_t) (123), (int16_t) (123));
+                        //                    CAN_TX_PRES((int16_t) (123), (int16_t) (123));
                         CONTROL_MODE = MODE_NO_ACT;
                     }
                 }
                 break;
             }
+
             default:
                 break;
         }
-        
+
         if (FLAG_VALVE_OUTPUT_CALIB) {
             // Valve Dead Zone (Mechanical dead-zone canceling)
-        //    if (CONTROL_MODE != MODE_VALVE_NULLING_AND_DEADZONE_SETTING) {
-        //        if (VALVE_PWM_RAW > 0.) {
-        //            VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_DEADZONE_PLUS;
-        //        } else if (VALVE_PWM_RAW < 0.) {
-        //            VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_DEADZONE_MINUS;
-        //        } else VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_CENTER;
-        //    } else VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW;
-    
-            if(VALVE_PWM_RAW>0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS;
-            else if(VALVE_PWM_RAW<0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS;
-      
-            VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_CENTER;
-        
+            //    if (CONTROL_MODE != MODE_VALVE_NULLING_AND_DEADZONE_SETTING) {
+            //        if (V_out > 0.) {
+            //            VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_PLUS;
+            //        } else if (V_out < 0.) {
+            //            VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_MINUS;
+            //        } else VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_CENTER;
+            //    } else VALVE_PWM_VALVE_DZ = (int) V_out;
+
+            if(V_out>0) V_out = V_out + VALVE_DEADZONE_PLUS;
+            else if(V_out<0) V_out = V_out + VALVE_DEADZONE_MINUS;
+
+            VALVE_PWM_VALVE_DZ = V_out + VALVE_CENTER;
+
             // Output Voltage Linearization and Dead Zone Cancellation (Electrical dead-zone)
             double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/PWM_RESOL*SUPPLY_VOLTAGE*1000.0; // convert PWM duty to mV
             double CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin);
-           
-            if (CUR_PWM_DZ > 0) CUR_PWM = (int)CUR_PWM_DZ + 143;
-            else if (CUR_PWM_DZ < 0) CUR_PWM = (int)CUR_PWM_DZ - 138;
-            else CUR_PWM = CUR_PWM_DZ;        
+
+            if (CUR_PWM_DZ > 0) V_out = (int)CUR_PWM_DZ + 143;
+            else if (CUR_PWM_DZ < 0) V_out = (int)CUR_PWM_DZ - 138;
+            else V_out = CUR_PWM_DZ;
         } else {
-            CUR_PWM = VALVE_PWM_RAW;
+            CUR_PWM = V_out;
         }
-        
-        VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
-        
-        //CAN ----------------------------------------------------------------------    
-        if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
-            
+
+        //VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
+        PWM_out = CUR_PWM;
+
+
+
+        //CAN ----------------------------------------------------------------------
+        //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
+        if (TMR3_COUNT_CAN_TX % 10 == 0) {
+
             if (flag_data_request[0] == HIGH) {
                 //position+velocity
-                CAN_TX_POSITION((long) CUR_POSITION, (long) CUR_VELOCITY);
+                CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
+                //pc.printf("can good");
                 //                        CAN_TX_POSITION((int) (PRES_A_VREF * 100.), (int) (PRES_B_VREF * 100.));
                 //                                    CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
             }
-            
+
             if (flag_data_request[1] == HIGH) {
                 //torque
                 //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
                 CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
                 //            CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
             }
-            
+
             if (flag_data_request[2] == HIGH) {
-                //pressure A and B 
-                CAN_TX_PRES((int16_t) (CUR_PRES_A), (int16_t) (CUR_PRES_B)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+                //pressure A and B
+                //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+                CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
                 //                        CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
                 //            CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
                 //            CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
-                
+
             }
-            
+
             if (flag_data_request[3] == HIGH) {
                 //PWM
                 CAN_TX_PWM((int16_t) CUR_PWM);
@@ -1577,36 +1684,39 @@
                 //            CAN_TX_PWM((int16_t) cnt_vel_findhome);
                 //            CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
                 //                        CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
-                
+
             }
-            
+
             if (flag_data_request[4] == HIGH) {
                 //valve position
-                CAN_TX_VALVE_POSITION((int16_t) (I2C_val));
+                CAN_TX_VALVE_POSITION((int16_t) (value));
                 //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
                 //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
-                
-                
+
+
                 //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
                 //                        CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE));
                 //            CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION);
                 //            CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
                 //            CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
             }
-            
+
             TMR3_COUNT_CAN_TX = 0;
-            
+
         }
         TMR3_COUNT_CAN_TX++;
+        
+        
+        
 
         /*******************************************************
         ***     Valve Control
         ********************************************************/
-        ValveControl(CONTROL_MODE);
+        //ValveControl(CONTROL_MODE);
 
-        double t = (double)CNT_TMR4*DT_TMR4;
-        double T = 5.0;
-        V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty)
+//      double t = (double)CNT_TMR4*DT_TMR4;
+//      double T = 1.0;
+//      V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty)
 //      if(V_out > 0.0) V_out = 1000.0;
 //      else if(V_out < 0.0) V_out = -1000.0;
 
@@ -1634,7 +1744,7 @@
         /*******************************************************
         ***     Data Send (CAN) & Print out (UART)
         ********************************************************/
-        //if((CNT_TMR3%40)==0){
+//        if((CNT_TMR3%1000)==0){
 //            msg.id = 50;
 //            msg.len = 4;
 //            int temp_CUR = (int)(cur.sen*1000.0);
@@ -1652,6 +1762,9 @@
             } else
                 LED = 1;
 //            LED != LED;
+            pc.printf("flag %f\n", (double) pres_A.sen);
+            spi_eeprom_ready();
+            pc.printf("romrom %d\n", (int) PRES_SENSOR_A_PULSE_PER_BAR);
         }
 
         /*******************************************************
@@ -1662,6 +1775,7 @@
     TIM3->SR = 0x0;  // reset the status register
 }
 
+/*
 void ValveControl(unsigned int ControlMode)
 {
     switch (ControlMode) {
@@ -1714,6 +1828,7 @@
 
     }
 }
+*/
 
 void CurrentControl()
 {