rainbow

Dependencies:   mbed FastPWM

Revision:
50:3c630b5eba9f
Parent:
49:e7bcfc244d40
Child:
51:b46bed7fec80
--- a/main.cpp	Wed Feb 05 08:35:58 2020 +0000
+++ b/main.cpp	Thu Feb 13 05:16:15 2020 +0000
@@ -109,7 +109,7 @@
     MODE_NO_ACT = 0,                                    //0
     MODE_VALVE_POSITION_CONTROL,                        //1
     MODE_JOINT_CONTROL,                                 //2
-    
+
     MODE_VALVE_OPEN_LOOP,                               //3
     MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION,  //4
     MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING,        //5
@@ -179,7 +179,7 @@
     TIM3->CR1 ^= TIM_CR1_UDIS;
     make_delay();
 
-    // TMR5 init
+    // TMR2 init
     Init_TMR2();
     TIM2->CR1 ^= TIM_CR1_UDIS;
     make_delay();
@@ -220,17 +220,14 @@
     *************************************/
     while(1) {
         if(timer_while==1000) {
-            //pc.printf("cmd %d\n ");
             //i2c
             read_field(i2c_slave_addr1);
             if(DIR_VALVE_ENC < 0) value = 1023 - value;
-
 //            if(LED==1) {
 //                LED=0;
 //            } else
 //                LED = 1;
             timer_while = 0;
-
         }
         timer_while ++;
     }
@@ -244,15 +241,15 @@
     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) {
-                if(JOINT_VEL[i+1] == JOINT_VEL[i]){
+                if(JOINT_VEL[i+1] == JOINT_VEL[i]) {
                     Ref_Valve_Pos_FF = DDV_CENTER;
-                }else{
+                } else {
                     Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
                 }
             } else {
-                if(JOINT_VEL[i+1] == JOINT_VEL[i-1]){
-                    Ref_Valve_Pos_FF = DDV_CENTER; 
-                }else{
+                if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) {
+                    Ref_Valve_Pos_FF = DDV_CENTER;
+                } else {
                     Ref_Valve_Pos_FF = ((float) 10*(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 + (float) (10*ID_index_array[i-1]);
                 }
             }
@@ -267,7 +264,7 @@
 
     Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
     return Ref_Valve_Pos_FF;
-    
+
 }
 
 
@@ -290,36 +287,6 @@
 
     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_POS_VS_PWM[0])
-//    {
-//        if(REF_VALVE_POS <=  VALVE_POS_VS_PWM[1]) {
-//            VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[1] - VALVE_POS_VS_PWM[0]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[0]);
-//        }else if(REF_VALVE_POS <=  VALVE_POS_VS_PWM[3]) {
-//            VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[3] - VALVE_POS_VS_PWM[1]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[1]) + 3000.0* (float) ID_index_array[1];
-//        }else if(REF_VALVE_POS <=  VALVE_POS_VS_PWM[5]) {
-//            VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[5] - VALVE_POS_VS_PWM[3]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[3]) + 3000.0* (float) ID_index_array[3];
-//        }else if(REF_VALVE_POS <=  VALVE_POS_VS_PWM[7]) {
-//            VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[7] - VALVE_POS_VS_PWM[5]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[5]) + 3000.0* (float) ID_index_array[5];
-//        }else
-//            VALVE_PWM_RAW_FF = 12000.0;
-//    }
-//    else
-//    {
-//        if(REF_VALVE_POS >= VALVE_POS_VS_PWM[2]) {
-//            VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[0] - VALVE_POS_VS_PWM[2]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[2]) + 3000.0* (float) ID_index_array[2];
-//        }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[4]) {
-//            VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[2] - VALVE_POS_VS_PWM[4]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[4]) + 3000.0* (float) ID_index_array[4];
-//        }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[6]) {
-//            VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[4] - VALVE_POS_VS_PWM[6]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[6]) + 3000.0* (float) ID_index_array[6];
-//        }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[8]) {
-//            VALVE_PWM_RAW_FF = (float) 3000.0 / (float) (VALVE_POS_VS_PWM[6] - VALVE_POS_VS_PWM[8]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[8]) + 3000.0* (float) ID_index_array[8];
-//        }else
-//            VALVE_PWM_RAW_FF = -12000.0;
-//    }
-
     for(i=0; i<24; 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) {
@@ -330,11 +297,7 @@
             break;
         }
     }
-    if(vel.sen>=0.0f)
-        V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
-    else
-        V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; // - (float) FLOWFORCE_GAIN * 0.01f * vel.sen / 30.0f;
-    //V_out = VALVE_PWM_RAW_FB;
+    V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
 }
 
 #define LT_MAX_IDX  57
@@ -411,7 +374,7 @@
         float pres_A_new = ((float)ADC1->DR);
         pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
         torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047
-        
+
 
         //Pressure sensor 1B
         //float alpha_update_pres_B = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f));
@@ -426,7 +389,7 @@
 //          a1=ADC2->DR;
         //int raw_cur = ADC3->DR;
         //while((ADC3->SR & 0b10));
-        float alpha_update_cur = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f)); // f_cutoff : 500Hz
+        float alpha_update_cur = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*200.0f)); // f_cutoff : 500Hz
         float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA
         cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur);
         //cur.sen = raw_cur;
@@ -453,31 +416,28 @@
 {
     if (TIM3->SR & TIM_SR_UIF ) {
         ENC_UPDATE();
-        
-        if(MODE_POS_FT_TRANS == 1){
+
+        if(MODE_POS_FT_TRANS == 1) {
             alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
             cnt_trans++;
             torq.err_sum = 0;
             if((float)cnt_trans * DT_TMR3 > 3.0f)
                 MODE_POS_FT_TRANS = 2;
-        }
-        else if(MODE_POS_FT_TRANS == 3){
+        } else if(MODE_POS_FT_TRANS == 3) {
             alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
             cnt_trans++;
             torq.err_sum = 0;
             if((float) cnt_trans * DT_TMR3 > 3.0f )
                 MODE_POS_FT_TRANS = 0;
-        }
-        else if(MODE_POS_FT_TRANS == 2){
+        } else if(MODE_POS_FT_TRANS == 2) {
             alpha_trans = 1.0;
             cnt_trans = 0;
-        }
-        else{
+        } else {
             alpha_trans = 0.0;
             cnt_trans = 0;
         }
 
-            
+
 
         // CONTROL LOOP ------------------------------------------------------------
 
@@ -493,34 +453,34 @@
             }
 
             case MODE_JOINT_CONTROL: {
-                
+
                 float VALVE_POS_RAW_FORCE_FB = 0.0f;
-                
+
                 pos.err = pos.ref - pos.sen; //[pulse]
                 vel.err = vel.ref - vel.sen; //[pulse/s]
                 double torq_ref = 0.0f;
                 torq_ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
-                
+
                 // torque feedback
                 torq.err = torq_ref - torq.sen; //[pulse]
                 torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse]
                 if (torq.err_sum > 1000) torq.err_sum = 1000;
                 if (torq.err_sum<-1000) torq.err_sum = -1000;
-                
+
 
                 VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) /(float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f
-                + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION + DDV_JOINT_POS_FF(vel.ref));   
+                                                      + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err /(float) ENC_PULSE_PER_POSITION + DDV_JOINT_POS_FF(vel.ref));
 
                 if (VALVE_POS_RAW_FORCE_FB >= 0) {
                     valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
-                } else{
+                } else {
                     valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
                 }
                 VALVE_POS_CONTROL(valve_pos.ref);
 
                 break;
             }
-            
+
             case MODE_VALVE_OPEN_LOOP: {
                 V_out = (float) Vout.ref;
                 break;
@@ -538,8 +498,7 @@
                 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 = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err + (float) I_GAIN_JOINT_POSITION * 0.01f * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff;
-                VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;
+                VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION + (float) I_GAIN_JOINT_POSITION * 0.01f * pos.err_sum/(float) ENC_PULSE_PER_POSITION + (float) D_GAIN_JOINT_POSITION * pos.err_diff/(float) ENC_PULSE_PER_POSITION;
 
 
                 //Ref_Joint_Vel =  Ref_Vel_Test;
@@ -574,272 +533,272 @@
                 break;
             }
 
-            case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING: {
-
-                break;
-            }
-
-            case MODE_JOINT_POSITION_PRES_CONTROL_PWM: {
-                pos.err = pos.ref - (float) 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 = ((float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01f;
-
-                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 = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
-
-                VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
-
-                V_out = VALVE_PWM_RAW_POS + (float) COMPLIANCE_GAIN * 0.01f * VALVE_PWM_RAW_TORQ;
+//            case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING: {
+//
+//                break;
+//            }
 
-                CUR_FLOWRATE = (float) CUR_VELOCITY * 0.00009587f;
-                CUR_FLOWRATE = CUR_FLOWRATE * 0.5757f; // 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) {
-                    if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]*1.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0f) / (VALVE_GAIN_LPM_PER_V[0]*1.0f - 0.0f) + 0.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*1.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0f) / (VALVE_GAIN_LPM_PER_V[2]*2.0f - VALVE_GAIN_LPM_PER_V[0]*1.) + 1.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*2.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0f) / (VALVE_GAIN_LPM_PER_V[4]*3.0f - VALVE_GAIN_LPM_PER_V[2]*2.) + 2.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*3.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0f) / (VALVE_GAIN_LPM_PER_V[6]*4.0f - VALVE_GAIN_LPM_PER_V[4]*3.) + 3.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*4.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0f) / (VALVE_GAIN_LPM_PER_V[8]*5.0f - VALVE_GAIN_LPM_PER_V[6]*4.) + 4.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = 5.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) && CUR_FLOWRATE < 0.0f) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) / (0.0f - VALVE_GAIN_LPM_PER_V[1]*(-1.)) - 1.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - VALVE_GAIN_LPM_PER_V[3]*(-2.)) - 2.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - VALVE_GAIN_LPM_PER_V[5]*(-3.)) - 3.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - VALVE_GAIN_LPM_PER_V[7]*(-4.)) - 4.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - VALVE_GAIN_LPM_PER_V[9]*(-5.)) - 5.0f;
-                    else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[9]*(-5.0f))) VALVE_FF_VOLTAGE = -5;
-                    else VALVE_FF_VOLTAGE = 0;
-                } else {
-                    if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]*1.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0f) / (VALVE_GAIN_LPM_PER_V[0]*1.0f - 0.0f) + 0.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*1.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0f) / (VALVE_GAIN_LPM_PER_V[2]*2.0f - VALVE_GAIN_LPM_PER_V[0]*1.0f) + 1.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*2.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0f) / (VALVE_GAIN_LPM_PER_V[4]*3.0f - VALVE_GAIN_LPM_PER_V[2]*2.0f) + 2.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*3.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0f) / (VALVE_GAIN_LPM_PER_V[6]*4.0f - VALVE_GAIN_LPM_PER_V[4]*3.0f) + 3.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*4.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0f) / (VALVE_GAIN_LPM_PER_V[8]*5.0f - VALVE_GAIN_LPM_PER_V[6]*4.0f) + 4.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*5.0f)) VALVE_FF_VOLTAGE = 5.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*(-1.0f)) && CUR_FLOWRATE < 0.0f) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) / (0.0f - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - 1.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*(-2.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - 2.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*(-3.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - 3.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*(-4.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - 4.0f;
-                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*(-5.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) - 5.0f;
-                    else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*(-5.0f))) VALVE_FF_VOLTAGE = -5;
-                    else VALVE_FF_VOLTAGE = 0;
-                }
-                //            VALVE_FF_VOLTAGE = CUR_FLOWRATE * 0.5f;
+//            case MODE_JOINT_POSITION_PRES_CONTROL_PWM: {
+//                pos.err = pos.ref - (float) 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 = ((float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff)/(float) ENC_PULSE_PER_POSITION;
+//
+//                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 = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
+//
+//                VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
+//
+//                V_out = VALVE_PWM_RAW_POS + (float) COMPLIANCE_GAIN * 0.01f * VALVE_PWM_RAW_TORQ;
+//
+//                CUR_FLOWRATE = (float) CUR_VELOCITY * 0.00009587f;
+//                CUR_FLOWRATE = CUR_FLOWRATE * 0.5757f; // 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) {
+//                    if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]*1.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0f) / (VALVE_GAIN_LPM_PER_V[0]*1.0f - 0.0f) + 0.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*1.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0f) / (VALVE_GAIN_LPM_PER_V[2]*2.0f - VALVE_GAIN_LPM_PER_V[0]*1.) + 1.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*2.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0f) / (VALVE_GAIN_LPM_PER_V[4]*3.0f - VALVE_GAIN_LPM_PER_V[2]*2.) + 2.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*3.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0f) / (VALVE_GAIN_LPM_PER_V[6]*4.0f - VALVE_GAIN_LPM_PER_V[4]*3.) + 3.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*4.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0f) / (VALVE_GAIN_LPM_PER_V[8]*5.0f - VALVE_GAIN_LPM_PER_V[6]*4.) + 4.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = 5.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) && CUR_FLOWRATE < 0.0f) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) / (0.0f - VALVE_GAIN_LPM_PER_V[1]*(-1.)) - 1.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - VALVE_GAIN_LPM_PER_V[3]*(-2.)) - 2.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - VALVE_GAIN_LPM_PER_V[5]*(-3.)) - 3.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - VALVE_GAIN_LPM_PER_V[7]*(-4.)) - 4.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - VALVE_GAIN_LPM_PER_V[9]*(-5.)) - 5.0f;
+//                    else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[9]*(-5.0f))) VALVE_FF_VOLTAGE = -5;
+//                    else VALVE_FF_VOLTAGE = 0;
+//                } else {
+//                    if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]*1.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0f) / (VALVE_GAIN_LPM_PER_V[0]*1.0f - 0.0f) + 0.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]*1.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]*2.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]*1.0f) / (VALVE_GAIN_LPM_PER_V[2]*2.0f - VALVE_GAIN_LPM_PER_V[0]*1.0f) + 1.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]*2.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]*3.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]*2.0f) / (VALVE_GAIN_LPM_PER_V[4]*3.0f - VALVE_GAIN_LPM_PER_V[2]*2.0f) + 2.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]*3.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]*4.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]*3.0f) / (VALVE_GAIN_LPM_PER_V[6]*4.0f - VALVE_GAIN_LPM_PER_V[4]*3.0f) + 3.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]*4.0f) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*5.0f)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]*4.0f) / (VALVE_GAIN_LPM_PER_V[8]*5.0f - VALVE_GAIN_LPM_PER_V[6]*4.0f) + 4.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*5.0f)) VALVE_FF_VOLTAGE = 5.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]*(-1.0f)) && CUR_FLOWRATE < 0.0f) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) / (0.0f - VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - 1.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]*(-2.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1]*(-1.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) / ((VALVE_GAIN_LPM_PER_V[1]*(-1.0f)) - VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - 2.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]*(-3.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3]*(-2.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) / ((VALVE_GAIN_LPM_PER_V[3]*(-2.0f)) - VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - 3.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]*(-4.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5]*(-3.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) / ((VALVE_GAIN_LPM_PER_V[5]*(-3.0f)) - VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - 4.0f;
+//                    else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*(-5.0f)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7]*(-4.0f))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) / ((VALVE_GAIN_LPM_PER_V[7]*(-4.0f)) - VALVE_GAIN_LPM_PER_V[9]*(-5.0f)) - 5.0f;
+//                    else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*(-5.0f))) VALVE_FF_VOLTAGE = -5;
+//                    else VALVE_FF_VOLTAGE = 0;
+//                }
+//                //            VALVE_FF_VOLTAGE = CUR_FLOWRATE * 0.5f;
+//
+//                if (CUR_FLOWRATE >= 0) VALVE_FF_VOLTAGE = (float) VELOCITY_COMP_GAIN * 0.001f * VALVE_FF_VOLTAGE * sqrt((float) PRES_SUPPLY - CUR_PRES_A_BAR) * 0.0707f; // 0.0707 = 1/sqrt(200.))
+//                else if (CUR_FLOWRATE < 0) VALVE_FF_VOLTAGE = (float) VELOCITY_COMP_GAIN * 0.001f * VALVE_FF_VOLTAGE * sqrt((float) PRES_SUPPLY - CUR_PRES_B_BAR) * 0.0707f;
+//
+//                V_out = V_out + VALVE_FF_VOLTAGE;
+//                break;
+//            }
 
-                if (CUR_FLOWRATE >= 0) VALVE_FF_VOLTAGE = (float) VELOCITY_COMP_GAIN * 0.001f * VALVE_FF_VOLTAGE * sqrt((float) PRES_SUPPLY - CUR_PRES_A_BAR) * 0.0707f; // 0.0707 = 1/sqrt(200.))
-                else if (CUR_FLOWRATE < 0) VALVE_FF_VOLTAGE = (float) VELOCITY_COMP_GAIN * 0.001f * VALVE_FF_VOLTAGE * sqrt((float) PRES_SUPPLY - CUR_PRES_B_BAR) * 0.0707f;
-
-                V_out = V_out + VALVE_FF_VOLTAGE;
-                break;
-            }
-
-            case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: {
-                
-                pos.err = pos.ref - (float) 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 = ((float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01f;
+//            case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: {
+//
+//                pos.err = pos.ref - (float) 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 = ((float) P_GAIN_JOINT_POSITION * pos.err + (float) I_GAIN_JOINT_POSITION * pos.err_sum + (float) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01f;
+//
+//                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 = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
+//
+//                valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
+//                VALVE_POS_CONTROL(valve_pos.ref);
+//
+//                break;
+//            }
 
-                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 = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
-
-                valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
-                VALVE_POS_CONTROL(valve_pos.ref);
-
-                break;
-            }
-
-            case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: {
-
-                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
-                float TMR3_CNT_MAX = (float)TMR_FREQ_5k/2.0f;
-                float I_REF_MID = 0.0f;
-                if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
-                    I_REF = I_REF_MID + 1.0f;
-                } else {
-                    I_REF = I_REF_MID - 1.0f;
-                }
-//              float T = 1.0; // wave period
-//              I_REF = (5. * sin(2. * 3.1415 * (float) TMR3_COUNT_IREF / (float)TMR_FREQ_5k/ T));
-//              I_REF = (2.0 * sin(2. * 2. * 3.14 * (float) TMR3_COUNT_IREF / 5000.)+(2.0 * sin(2. * 1. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 5. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 10. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.)));
+//            case MODE_TEST_CURRENT_CONTROL: {
+//                if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
+//                    TMR3_COUNT_IREF = 0;
+//                }
+//                TMR3_COUNT_IREF++;
+//
+//                // Set Current Reference
+//                float TMR3_CNT_MAX = (float)TMR_FREQ_5k/2.0f;
+//                float I_REF_MID = 0.0f;
+//                if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
+//                    I_REF = I_REF_MID + 1.0f;
+//                } else {
+//                    I_REF = I_REF_MID - 1.0f;
+//                }
+////              float T = 1.0; // wave period
+////              I_REF = (5. * sin(2. * 3.1415 * (float) TMR3_COUNT_IREF / (float)TMR_FREQ_5k/ T));
+////              I_REF = (2.0 * sin(2. * 2. * 3.14 * (float) TMR3_COUNT_IREF / 5000.)+(2.0 * sin(2. * 1. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 5. * 3.14 * (float)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 10. * 3.14 * (float)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;
+//            }
 
-                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
-                float TMR3_CNT_MAX = (float)TMR_FREQ_5k/2.0f;
-                //float I_REF_MID = 0.0f;
-                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.0f), (int16_t) (CUR_CURRENT*1000.0f)); // 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
+//                float TMR3_CNT_MAX = (float)TMR_FREQ_5k/2.0f;
+//                //float I_REF_MID = 0.0f;
+//                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.0f), (int16_t) (CUR_CURRENT*1000.0f)); // to check the datas
+//                }
+//                break;
+//            }
 
 
-            case MODE_CURRENT_CONTROL: {
-
-                cur.ref = cur.ref; // Unit : mA
-                CurrentControl();
-                break;
-            }
-
-            case MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT: {
-                float I_REF_POS_FB = 0.0f; // I_REF by Position Feedback
-                float I_REF_POS_FF = 0.0f; // I_REF by Position Feedforward
-                float I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
-                float I_REF_FORCE_FF = 0.0f; // I_REF by Force Feedforward
-
-                // feedback input for position control
-                pos.err = pos.ref - pos.sen;
-                float alpha_update_vel = 1.0f/(1.0f+(float)FREQ_TMR4/(2.0f*3.1415f*50.0f)); // f_cutoff : 50Hz
-                float err_diff = (pos.err - pos.err_old)*(float)FREQ_5k;
-                pos.err_diff = (1.0f-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
-                pos.err_old = pos.err;
-                I_REF_POS_FB = 0.001f*((float)P_GAIN_JOINT_POSITION * pos.err + (float)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1f);
-
-                // feedforward input for position control
-                float Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
-                float K_ff = 1.3f;
-                float K_v = 0.0f;
-                if(Vel_Act_Ref > 0) K_v = 1.0f/100.0f; // open, tuning. (deg/s >> mA)
-                if(Vel_Act_Ref < 0) K_v = 1.0f/100.0f; // close, tuning. (deg/s >> mA)
-                I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
+//            case MODE_CURRENT_CONTROL: {
+//
+//                cur.ref = cur.ref; // Unit : mA
+//                CurrentControl();
+//                break;
+//            }
 
-                // feedback input for position control
-                I_REF_FORCE_FB = 0.0f;
-
-                // feedforward input for position control
-                I_REF_FORCE_FF = 0.0f;
-
-                cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
-
-                CurrentControl();
-
-                break;
-            }
-
-            case MODE_JOINT_POSITION_PRES_CONTROL_CURRENT: {
-                //float T_REF = 0.0; // Torque Reference
-                float I_REF_FORCE_FB = 0.; // I_REF by Force Feedback
-                float I_REF_VC = 0.; // I_REF for velocity compensation
-
-                // feedback input for position control
-                //float Joint_Pos_Err = 34.0f-(float) pos.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
-                //float Joint_Vel_Err = 0.0f-(float) vel.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
-                //float K_spring = 0.7f;
-                //float D_damper = 0.02f;
-//              T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm
-
-                // torque feedback
-                torq.err = torq.ref - torq.sen;
-                //            torq.err_diff = torq.err - torq.err_old;
-                //            torq.err_old = torq.err;
-                torq.err_sum = torq.err_sum + torq.err/(float)TMR_FREQ_5k;
-                I_REF_FORCE_FB = 0.001f*((float)P_GAIN_JOINT_TORQUE * torq.err + (float)I_GAIN_JOINT_TORQUE * torq.err_sum);
+//            case MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT: {
+//                float I_REF_POS_FB = 0.0f; // I_REF by Position Feedback
+//                float I_REF_POS_FF = 0.0f; // I_REF by Position Feedforward
+//                float I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
+//                float I_REF_FORCE_FF = 0.0f; // I_REF by Force Feedforward
+//
+//                // feedback input for position control
+//                pos.err = pos.ref - pos.sen;
+//                float alpha_update_vel = 1.0f/(1.0f+(float)FREQ_TMR4/(2.0f*3.1415f*50.0f)); // f_cutoff : 50Hz
+//                float err_diff = (pos.err - pos.err_old)*(float)FREQ_5k;
+//                pos.err_diff = (1.0f-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
+//                pos.err_old = pos.err;
+//                I_REF_POS_FB = 0.001f*((float)P_GAIN_JOINT_POSITION * pos.err + (float)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1f);
+//
+//                // feedforward input for position control
+//                float Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
+//                float K_ff = 1.3f;
+//                float K_v = 0.0f;
+//                if(Vel_Act_Ref > 0) K_v = 1.0f/100.0f; // open, tuning. (deg/s >> mA)
+//                if(Vel_Act_Ref < 0) K_v = 1.0f/100.0f; // 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.0f;
+//
+//                // feedforward input for position control
+//                I_REF_FORCE_FF = 0.0f;
+//
+//                cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
+//
+//                CurrentControl();
+//
+//                break;
+//            }
 
-                // velocity compensation for torque control
-                float Joint_Vel_Act = vel.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
-                float K_vc = 1.5f; // Velocity comp. gain
-                float K_v = 0.0f; // Valve gain
-                if(Joint_Vel_Act > 0) K_v = 1.0f/100.0f; // open, tuning
-                if(Joint_Vel_Act < 0) K_v = 1.0f/100.0f; // 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;
-
-                float I_MAX = 10.0f; // Maximum Current : 10mV
-                float Ka = 1.0f/I_GAIN_JOINT_TORQUE;
-                if(cur.ref > I_MAX) {
-                    float I_rem = cur.ref-I_MAX;
-                    I_rem = Ka*I_rem;
-                    cur.ref = I_MAX;
-                    torq.err_sum = torq.err_sum - I_rem/(float)TMR_FREQ_5k;
-                } else if(cur.ref < -I_MAX) {
-                    float I_rem = cur.ref-(-I_MAX);
-                    I_rem = Ka*I_rem;
-                    cur.ref = -I_MAX;
-                    torq.err_sum = torq.err_sum - I_rem/(float)TMR_FREQ_5k;
-                }
-
-                CurrentControl();
-
-
-                /*
-                float I_REF_POS_FB = 0.0f; // I_REF by Position Feedback
-                float I_REF_POS_FF = 0.0f; // I_REF by Position Feedforward
-                float I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
-                float I_REF_FORCE_FF = 0.0f; // I_REF by Force Feedforward
-
-                // feedback input for position control
-                pos.err = pos.ref - pos.sen;
-                float alpha_update_vel = 1.0f/(1.0f+(float)FREQ_TMR4/(2.0f*3.1415f*50.0f)); // f_cutoff : 50Hz
-                float err_diff = (pos.err - pos.err_old)*(float)FREQ_5k;
-                pos.err_diff = (1.0f-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
-                pos.err_old = pos.err;
-                I_REF_POS_FB = 0.001f*((float)P_GAIN_JOINT_POSITION * pos.err + (float)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1f);
-
-                // feedforward input for position control
-                float Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
-                float K_ff = 1.3f;
-                float K_v = 0.0f;
-                if(Vel_Act_Ref > 0) K_v = 1.0f/100.0f; // open, tuning. (deg/s >> mA)
-                if(Vel_Act_Ref < 0) K_v = 1.0f/100.0f; // 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.0f;
-
-                // feedforward input for position control
-                I_REF_FORCE_FF = 0.0f;
-
-                cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
-
-                CurrentControl();
-                */
-
-                break;
-            }
+//            case MODE_JOINT_POSITION_PRES_CONTROL_CURRENT: {
+//                //float T_REF = 0.0; // Torque Reference
+//                float I_REF_FORCE_FB = 0.; // I_REF by Force Feedback
+//                float I_REF_VC = 0.; // I_REF for velocity compensation
+//
+//                // feedback input for position control
+//                //float Joint_Pos_Err = 34.0f-(float) pos.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+//                //float Joint_Vel_Err = 0.0f-(float) vel.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+//                //float K_spring = 0.7f;
+//                //float D_damper = 0.02f;
+////              T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm
+//
+//                // torque feedback
+//                torq.err = torq.ref - torq.sen;
+//                //            torq.err_diff = torq.err - torq.err_old;
+//                //            torq.err_old = torq.err;
+//                torq.err_sum = torq.err_sum + torq.err/(float)TMR_FREQ_5k;
+//                I_REF_FORCE_FB = 0.001f*((float)P_GAIN_JOINT_TORQUE * torq.err + (float)I_GAIN_JOINT_TORQUE * torq.err_sum);
+//
+//                // velocity compensation for torque control
+//                float Joint_Vel_Act = vel.sen/(float)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+//                float K_vc = 1.5f; // Velocity comp. gain
+//                float K_v = 0.0f; // Valve gain
+//                if(Joint_Vel_Act > 0) K_v = 1.0f/100.0f; // open, tuning
+//                if(Joint_Vel_Act < 0) K_v = 1.0f/100.0f; // 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;
+//
+//                float I_MAX = 10.0f; // Maximum Current : 10mV
+//                float Ka = 1.0f/I_GAIN_JOINT_TORQUE;
+//                if(cur.ref > I_MAX) {
+//                    float I_rem = cur.ref-I_MAX;
+//                    I_rem = Ka*I_rem;
+//                    cur.ref = I_MAX;
+//                    torq.err_sum = torq.err_sum - I_rem/(float)TMR_FREQ_5k;
+//                } else if(cur.ref < -I_MAX) {
+//                    float I_rem = cur.ref-(-I_MAX);
+//                    I_rem = Ka*I_rem;
+//                    cur.ref = -I_MAX;
+//                    torq.err_sum = torq.err_sum - I_rem/(float)TMR_FREQ_5k;
+//                }
+//
+//                CurrentControl();
+//
+//
+//                /*
+//                float I_REF_POS_FB = 0.0f; // I_REF by Position Feedback
+//                float I_REF_POS_FF = 0.0f; // I_REF by Position Feedforward
+//                float I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
+//                float I_REF_FORCE_FF = 0.0f; // I_REF by Force Feedforward
+//
+//                // feedback input for position control
+//                pos.err = pos.ref - pos.sen;
+//                float alpha_update_vel = 1.0f/(1.0f+(float)FREQ_TMR4/(2.0f*3.1415f*50.0f)); // f_cutoff : 50Hz
+//                float err_diff = (pos.err - pos.err_old)*(float)FREQ_5k;
+//                pos.err_diff = (1.0f-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
+//                pos.err_old = pos.err;
+//                I_REF_POS_FB = 0.001f*((float)P_GAIN_JOINT_POSITION * pos.err + (float)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1f);
+//
+//                // feedforward input for position control
+//                float Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
+//                float K_ff = 1.3f;
+//                float K_v = 0.0f;
+//                if(Vel_Act_Ref > 0) K_v = 1.0f/100.0f; // open, tuning. (deg/s >> mA)
+//                if(Vel_Act_Ref < 0) K_v = 1.0f/100.0f; // 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.0f;
+//
+//                // feedforward input for position control
+//                I_REF_FORCE_FF = 0.0f;
+//
+//                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: {
                 // DAC Voltage reference set
@@ -854,7 +813,7 @@
 
                         if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f;
                         if (TORQUE_VREF < 0) TORQUE_VREF = 0;
-                        
+
                         //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
                         dac_1 = TORQUE_VREF / 3.3f;
                     }
@@ -874,10 +833,10 @@
                 }
                 TMR3_COUNT_TORQUE_NULL++;
                 break;
-                
-                
-                
-                     //           // DAC Voltage reference set
+
+
+
+                //           // DAC Voltage reference set
 //                if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
 //                    CUR_PRES_A_sum += pres_A.sen;
 //                    CUR_PRES_B_sum += pres_B.sen;
@@ -916,89 +875,89 @@
 //                }
 //                TMR3_COUNT_PRES_NULL++;
 //                break;
-                
-                
-                
-                
-                
+
+
+
+
+
             }
 
-            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 (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
-                        pos_plus_end = pos.sen;
-                    } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
-                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-                        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 <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
-                    V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
-                    VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
-
-                } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) {
-                    V_out = 0;
-                    CUR_VELOCITY_sum += CUR_VELOCITY;
-                } 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 (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 (TMR3_COUNT_DEADZONE % 20 != 0) {
-                            CUR_VELOCITY_sum += CUR_VELOCITY;
-                        } else {
-                            V_out -= DZ_dir;
-                            if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++;
-                            CUR_VELOCITY_sum = 0;
-                        }
-                        if (DZ_temp_cnt == 5) {
-                            if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out;
-                            else VALVE_DEADZONE_PLUS = (int16_t) V_out;
-                            DZ_dir = -DZ_dir;
-                            DZ_temp_cnt = 0;
-                            DZ_temp_cnt2++;
-                        }
-                    } else {
-                        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;
-                            VALVE_DEADZONE_MINUS = VALVE_CENTER;
-                        }
-                        V_out = 0;
-
-                        ROM_RESET_DATA();
-
-                        //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;
-                    }
-                }
-                TMR3_COUNT_DEADZONE++;
-                break;
-            }
+//            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 (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) {
+//                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+//                        pos_plus_end = pos.sen;
+//                    } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
+//                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+//                        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 <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
+//                    V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
+//                    VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
+//
+//                } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) {
+//                    V_out = 0;
+//                    CUR_VELOCITY_sum += CUR_VELOCITY;
+//                } 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 (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 (TMR3_COUNT_DEADZONE % 20 != 0) {
+//                            CUR_VELOCITY_sum += CUR_VELOCITY;
+//                        } else {
+//                            V_out -= DZ_dir;
+//                            if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++;
+//                            CUR_VELOCITY_sum = 0;
+//                        }
+//                        if (DZ_temp_cnt == 5) {
+//                            if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out;
+//                            else VALVE_DEADZONE_PLUS = (int16_t) V_out;
+//                            DZ_dir = -DZ_dir;
+//                            DZ_temp_cnt = 0;
+//                            DZ_temp_cnt2++;
+//                        }
+//                    } else {
+//                        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;
+//                            VALVE_DEADZONE_MINUS = VALVE_CENTER;
+//                        }
+//                        V_out = 0;
+//
+//                        ROM_RESET_DATA();
+//
+//                        //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;
+//                    }
+//                }
+//                TMR3_COUNT_DEADZONE++;
+//                break;
+//            }
 
             case MODE_FIND_HOME: {
                 if (FINDHOME_STAGE == FINDHOME_INIT) {
@@ -1031,8 +990,7 @@
                         else pos.ref_home_pos = pos.ref_home_pos - 2.0f;
                         pos.err = pos.ref_home_pos - pos.sen;
                         float VALVE_POS_RAW_POS_FB = 0.0f;
-                        VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err;
-                        VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;
+                        VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f;
                         valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
                         VALVE_POS_CONTROL(valve_pos.ref);
 
@@ -1062,8 +1020,7 @@
                     // input for position control
                     pos.err = pos.ref_home_pos - (float)pos.sen;
                     float VALVE_POS_RAW_POS_FB = 0.0f;
-                    VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err;
-                    VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01f;
+                    VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION;
                     valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
                     VALVE_POS_CONTROL(valve_pos.ref);
 
@@ -1102,77 +1059,77 @@
                 break;
             }
 
-            case MODE_VALVE_GAIN_SETTING: {
-                if (TMR3_COUNT_FLOWRATE == 0) {
-                    if (pos_plus_end == pos_minus_end) need_enc_init = true;
-                    else {
-                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-                        temp_time = (int) (0.5f * (float) TMR_FREQ_5k);
-                    }
-                }
-                if (need_enc_init) {
-                    if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) {
-                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
-                        pos_plus_end = pos.sen;
-                    } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
-                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-                        pos_minus_end = pos.sen;
-                    } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
-                        need_enc_init = false;
-                        check_vel_pos_init = (int) (0.9f * (float) (pos_plus_end - pos_minus_end));
-                        check_vel_pos_fin = (int) (0.95f * (float) (pos_plus_end - pos_minus_end));
-                        check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
-                    }
-                    temp_time = TMR_FREQ_5k;
-                }
-                TMR3_COUNT_FLOWRATE++;
-                if (TMR3_COUNT_FLOWRATE > temp_time) {
-                    if (flag_flowrate % 2 == 0) { // (+)
-                        VALVE_VOLTAGE = 1000.0f * (float) (flag_flowrate / 2 + 1);
-                        V_out = VALVE_VOLTAGE;
-                        if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) {
-                            fl_temp_cnt++;
-                        } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
-                            VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0 * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // 0.9587=6*pi/65536*10000 0.5757=0.02525*0.02*0.0095*2*60*1000
-                            //                        VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
-                            fl_temp_cnt2++;
-                        }
-                    } else if (flag_flowrate % 2 == 1) { // (-)
-                        VALVE_VOLTAGE = -1. * (float) (flag_flowrate / 2 + 1);
-                        V_out = VALVE_VOLTAGE;
-                        if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) {
-                            fl_temp_cnt++;
-                        } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
-                            VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0f * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE);
-                            //                        VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
-                            fl_temp_cnt2++;
-                        }
-                    }
-                    if (fl_temp_cnt2 == 100) {
-
-                        ROM_RESET_DATA();
-
-                        //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f));
-                        cur_vel_sum = 0;
-                        fl_temp_cnt = 0;
-                        fl_temp_cnt2 = 0;
-                        flag_flowrate++;
-                    }
-                    if (flag_flowrate == 10) {
-                        V_out = 0;
-                        flag_flowrate = 0;
-                        TMR3_COUNT_FLOWRATE = 0;
-                        valve_gain_repeat_cnt++;
-                        if (valve_gain_repeat_cnt >= 1) {
-                            CONTROL_MODE = MODE_NO_ACT;
-                            valve_gain_repeat_cnt = 0;
-                        }
-
-                    }
-                    break;
-                }
-
-            }
+//            case MODE_VALVE_GAIN_SETTING: {
+//                if (TMR3_COUNT_FLOWRATE == 0) {
+//                    if (pos_plus_end == pos_minus_end) need_enc_init = true;
+//                    else {
+//                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+//                        temp_time = (int) (0.5f * (float) TMR_FREQ_5k);
+//                    }
+//                }
+//                if (need_enc_init) {
+//                    if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) {
+//                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+//                        pos_plus_end = pos.sen;
+//                    } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
+//                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+//                        pos_minus_end = pos.sen;
+//                    } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
+//                        need_enc_init = false;
+//                        check_vel_pos_init = (int) (0.9f * (float) (pos_plus_end - pos_minus_end));
+//                        check_vel_pos_fin = (int) (0.95f * (float) (pos_plus_end - pos_minus_end));
+//                        check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
+//                    }
+//                    temp_time = TMR_FREQ_5k;
+//                }
+//                TMR3_COUNT_FLOWRATE++;
+//                if (TMR3_COUNT_FLOWRATE > temp_time) {
+//                    if (flag_flowrate % 2 == 0) { // (+)
+//                        VALVE_VOLTAGE = 1000.0f * (float) (flag_flowrate / 2 + 1);
+//                        V_out = VALVE_VOLTAGE;
+//                        if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) {
+//                            fl_temp_cnt++;
+//                        } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
+//                            VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0 * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // 0.9587=6*pi/65536*10000 0.5757=0.02525*0.02*0.0095*2*60*1000
+//                            //                        VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
+//                            fl_temp_cnt2++;
+//                        }
+//                    } else if (flag_flowrate % 2 == 1) { // (-)
+//                        VALVE_VOLTAGE = -1. * (float) (flag_flowrate / 2 + 1);
+//                        V_out = VALVE_VOLTAGE;
+//                        if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) {
+//                            fl_temp_cnt++;
+//                        } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
+//                            VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0f * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE);
+//                            //                        VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
+//                            fl_temp_cnt2++;
+//                        }
+//                    }
+//                    if (fl_temp_cnt2 == 100) {
+//
+//                        ROM_RESET_DATA();
+//
+//                        //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f));
+//                        cur_vel_sum = 0;
+//                        fl_temp_cnt = 0;
+//                        fl_temp_cnt2 = 0;
+//                        flag_flowrate++;
+//                    }
+//                    if (flag_flowrate == 10) {
+//                        V_out = 0;
+//                        flag_flowrate = 0;
+//                        TMR3_COUNT_FLOWRATE = 0;
+//                        valve_gain_repeat_cnt++;
+//                        if (valve_gain_repeat_cnt >= 1) {
+//                            CONTROL_MODE = MODE_NO_ACT;
+//                            valve_gain_repeat_cnt = 0;
+//                        }
+//
+//                    }
+//                    break;
+//                }
+//
+//            }
 
             case MODE_PRESSURE_SENSOR_NULLING: {
                 // DAC Voltage reference set
@@ -1216,52 +1173,52 @@
                 break;
             }
 
-            case MODE_PRESSURE_SENSOR_CALIB: {
-                if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
-                    V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
-                    if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
-                        CUR_PRES_A_sum += CUR_PRES_A;
-                    }
-                } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
-                    V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
-                    if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
-                        CUR_PRES_B_sum += CUR_PRES_B;
-                    }
-                } else {
-                    CONTROL_MODE = MODE_NO_ACT;
-                    TMR3_COUNT_PRES_CALIB = 0;
-                    V_out = 0;
-                    PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_A_NULL;
-                    PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
-                    PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_B_NULL;
-                    PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
-                    CUR_PRES_A_sum = 0;
-                    CUR_PRES_B_sum = 0;
-                    CUR_PRES_A_mean = 0;
-                    CUR_PRES_B_mean = 0;
+//            case MODE_PRESSURE_SENSOR_CALIB: {
+//                if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
+//                    V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+//                    if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
+//                        CUR_PRES_A_sum += CUR_PRES_A;
+//                    }
+//                } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
+//                    V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+//                    if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
+//                        CUR_PRES_B_sum += CUR_PRES_B;
+//                    }
+//                } else {
+//                    CONTROL_MODE = MODE_NO_ACT;
+//                    TMR3_COUNT_PRES_CALIB = 0;
+//                    V_out = 0;
+//                    PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_A_NULL;
+//                    PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
+//                    PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_B_NULL;
+//                    PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
+//                    CUR_PRES_A_sum = 0;
+//                    CUR_PRES_B_sum = 0;
+//                    CUR_PRES_A_mean = 0;
+//                    CUR_PRES_B_mean = 0;
+//
+//                    ROM_RESET_DATA();
+//
+//                    //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
+//                    //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
+//                }
+//                TMR3_COUNT_PRES_CALIB++;
+//                break;
+//            }
 
-                    ROM_RESET_DATA();
-
-                    //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
-                    //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
-                }
-                TMR3_COUNT_PRES_CALIB++;
-                break;
-            }
-
-            case MODE_ROTARY_FRICTION_TUNING: {
-                if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f);
-                V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f);
-                if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
-                else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; 
-                TMR3_COUNT_ROTARY_FRIC_TUNE++;
-                if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
-                    TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
-                    V_out = 0.0f;
-                    CONTROL_MODE = MODE_NO_ACT;
-                }
-                break;
-            }
+//            case MODE_ROTARY_FRICTION_TUNING: {
+//                if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f);
+//                V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f);
+//                if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
+//                else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
+//                TMR3_COUNT_ROTARY_FRIC_TUNE++;
+//                if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
+//                    TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
+//                    V_out = 0.0f;
+//                    CONTROL_MODE = MODE_NO_ACT;
+//                }
+//                break;
+//            }
 
             case MODE_DDV_POS_VS_PWM_ID: {
                 VALVE_ID_timer = VALVE_ID_timer + 1;
@@ -1356,7 +1313,7 @@
                         }
 
                         CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
-                        
+
                         VEL_POINT = 0;
                         first_check = 1;
                         DZ_DIRECTION = 1;
@@ -1621,31 +1578,31 @@
                 break;
         }
 
-        if (FLAG_VALVE_OUTPUT_CALIB) {
-            // Valve Dead Zone (Mechanical dead-zone canceling)
-            //    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)
-            float CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/5.0f*1000.0f; // convert PWM duty to mV
-            float CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin);
-
-            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 {
-            V_out = V_out;
-        }
+//        if (FLAG_VALVE_OUTPUT_CALIB) {
+//            // Valve Dead Zone (Mechanical dead-zone canceling)
+//            //    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)
+//            float CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/5.0f*1000.0f; // convert PWM duty to mV
+//            float CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin);
+//
+//            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 {
+//            V_out = V_out;
+//        }
 
         /*******************************************************
         ***     PWM
@@ -1695,7 +1652,7 @@
         if (flag_data_request[0] == HIGH) {
             //position+velocity
             //CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
-            CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (V_out/100.0f), (int16_t) (torq.sen*10.0f));
+            CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
         }
 
         if (flag_data_request[1] == HIGH) {
@@ -1703,9 +1660,9 @@
             //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
             //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
             double t_value = 0;
-            if(value>=DDV_CENTER){
+            if(value>=DDV_CENTER) {
                 t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
-            }else{
+            } else {
                 t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
             }
             CAN_TX_TORQUE((int16_t) (t_value));
@@ -1713,7 +1670,7 @@
 
         if (flag_data_request[2] == HIGH) {
             //pressure A and B
-            CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (PRES_A_VREF * 1000.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+            CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (MODE_POS_FT_TRANS * 100.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar)
             //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // 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.);
@@ -1741,7 +1698,7 @@
             //            CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
             //            CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
         }
-        
+
 
     }
     TIM2->SR = 0x0;  // reset the status register
@@ -1788,3 +1745,4 @@
 
 
 
+