eeprom_test

Dependencies:   mbed FastPWM

Revision:
19:23b7c1ad8683
Parent:
18:b8adf1582ea3
Child:
20:806196fda269
--- a/main.cpp	Wed Sep 04 11:02:57 2019 +0000
+++ b/main.cpp	Mon Sep 09 06:29:56 2019 +0000
@@ -70,6 +70,22 @@
 int timer_while = 0;
 int while_index = 0;
 
+
+extern int CID_RX_CMD;
+extern int CID_RX_REF_POSITION;
+extern int CID_RX_REF_TORQUE;
+extern int CID_RX_REF_PRES_DIFF;
+extern int CID_RX_REF_VOUT;
+extern int CID_RX_REF_VALVE_POSITION;
+extern int CID_RX_REF_CURRENT;
+
+extern int CID_TX_INFO;
+extern int CID_TX_POSITION;
+extern int CID_TX_TORQUE;
+extern int CID_TX_PRES;
+extern int CID_TX_VOUT;
+extern int CID_TX_VALVE_POSITION;
+
 // =============================================================================
 // =============================================================================
 // =============================================================================
@@ -129,8 +145,8 @@
     /*********************************
     ***     Initialization
     *********************************/
-    LED = 1;
-    pc.baud(9600);
+    //LED = 1;
+    //pc.baud(9600);
     
     // i2c init
     i2c.frequency(400 * 1000);          // 0.4 mHz
@@ -147,8 +163,7 @@
     make_delay();
     
     //rom
-    
-    ROM_INIT_DATA();
+    ROM_CALL_DATA();
     make_delay();
 
     // ADC init
@@ -169,19 +184,18 @@
     can.attach(&CAN_RX_HANDLER);
     CAN_ID_INIT();
     make_delay();
-
+    can.filter(msg.id, 0xFFFFF000, CANStandard);
+    
     // spi _ enc
     spi_enc_set_init();
     make_delay();
 
     //DAC init
     dac_1 = PRES_A_VREF / 3.3;
-    //dac_1 = 0.0;
     dac_2 = PRES_B_VREF / 3.3;
-    //dac_2 = 0.0;
     make_delay();
 
-    for (int i=0; i<100; i++) {
+    for (int i=0; i<50; i++) {
         if(i%2==0)
             ID_index_array[i] = - i * 0.5;
         else
@@ -192,15 +206,15 @@
     ***     Program is operating!
     *************************************/
     while(1) {
-        if(timer_while==1){
+        if(timer_while==1000){
             //i2c
             read_field(i2c_slave_addr1);
             if(DIR_VALVE_ENC < 0) value = 1023 - value;
             
-            if(LED==1) {
-                LED=0;
-            } else
-                LED = 1;
+//            if(LED==1) {
+//                LED=0;
+//            } else
+//                LED = 1;
             timer_while = 0;
             
         }
@@ -216,9 +230,9 @@
     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;
+                Ref_Valve_Pos_FF = ((int) 10.0/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
             } 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];
+                Ref_Valve_Pos_FF = ((int) 10.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 + 10*ID_index_array[i-1];
             }
             break;
         }
@@ -228,6 +242,7 @@
     } 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;
     }
+    Ref_Valve_Pos_FF_CAN = Ref_Valve_Pos_FF;
     return Ref_Valve_Pos_FF;
 }
 
@@ -261,6 +276,9 @@
             break;
         }
     }
+
+    //VALVE_PWM_RAW_FF = 0.0;
+
     V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
 }
 
@@ -308,201 +326,74 @@
                             TIMER INTERRUPT
 *******************************************************************************/
 
-unsigned long CNT_TMR4 = 0;
+//unsigned long CNT_TMR4 = 0;
 double FREQ_TMR4 = (double)FREQ_10k;
 double DT_TMR4 = (double)DT_10k;
 extern "C" void TIM4_IRQHandler(void)
 {
-    if ( TIM4->SR & TIM_SR_UIF ) {
-        /*******************************************************
-        ***     Sensor Read & Data Handling
-        ********************************************************/
+    if (TIM4->SR & TIM_SR_UIF ) {
+        
+    /*******************************************************
+    ***     Sensor Read & Data Handling
+    ********************************************************/
 
- 
-        //Using LoadCell
+
+    //Using LoadCell
 //            ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
 //            //while((ADC1->SR & 0b10));
 //            double alpha_update_torque = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
 //            double torque_new = ((double)ADC1->DR - PRES_A_NULL)  / TORQUE_SENSOR_PULSE_PER_TORQUE + 1.0;
 //            torq.sen = torq.sen*(1.0-alpha_update_torque)+torque_new*(alpha_update_torque);
 
-        //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;
-        pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
+
 
-        //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;
-        pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
-        torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
+    //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;
+    pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
+
+    //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;
+    pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+    torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
 
 
 
-        //Current
-        //ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
+    //Current
+    //ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
 //          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);
-        cur.sen = raw_cur;
+    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);
+    cur.sen = raw_cur;
 
-        
-
-        /*******************************************************
-        ***     Timer Counting & etc.
-        ********************************************************/
-        CNT_TMR4++;
-    }
+    /*******************************************************
+    ***     Timer Counting & etc.
+    ********************************************************/
+    //CNT_TMR4++;
+    }    
     TIM4->SR = 0x0;  // reset the status register
 }
+
+
 int j =0;
-unsigned long CNT_TMR3 = 0;
+//unsigned long CNT_TMR3 = 0;
 double FREQ_TMR3 = (double)FREQ_5k;
 double DT_TMR3 = (double)DT_5k;
 extern "C" void TIM3_IRQHandler(void)
-{
-    if ( TIM3->SR & TIM_SR_UIF ) {
-        
+{ 
+    if (TIM3->SR & TIM_SR_UIF ) {
         ENC_UPDATE();
 
-        /*
-        // Reference Loop
-        switch (REFERENCE_MODE) {
-            case MODE_REF_NO_ACT: {
-                break;
-            }
-            case MODE_REF_DIRECT: {
-                if (FLAG_REFERENCE_VALVE_PWM) {
-                    Vout.ref = (double) Vout.ref;
-                }
-                if (FLAG_REFERENCE_VALVE_POSITION) {
-                    valve_pos.ref = (double) valve_pos.ref;
-                }
-                if (FLAG_REFERENCE_JOINT_POSITION) {
-                    pos.ref = (double) pos.ref;
-                    vel.ref = (double) vel.ref;
-                }
-                if (FLAG_REFERENCE_JOINT_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) {
-                    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) {
-                    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) {
-                    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) {
-                    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;
-                }
-                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;
-            }
-
+        // CONTROL LOOP ------------------------------------------------------------
 
-            case MODE_REF_LINE_INC: {
-                if (FLAG_REFERENCE_VALVE_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) {
-                    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) {
-                    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) {
-                    torq.ref = ((double) torq.ref - (double) INIT_torq.ref)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_torq.ref;
-                }
-                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;
-            }
-
-            case MODE_REF_SIN_WAVE: {
-                if (FLAG_REFERENCE_VALVE_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) {
-                    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) 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) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) torq.ref;
-                }
-                TMR3_COUNT_REFERENCE++;
-                if (TMR3_COUNT_REFERENCE >= TMR3_COUNT_REFERENCE * REF_NUM) {
-                    REFERENCE_MODE = MODE_REF_DIRECT;
-                    TMR3_COUNT_REFERENCE = 0;
-                }
-                break;
-            }
-
-            case MODE_REF_SQUARE_WAVE: {
-                if (FLAG_REFERENCE_VALVE_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) {
-                    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) 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) 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;
-                }
-                TMR3_COUNT_REFERENCE++;
-                if (TMR3_COUNT_REFERENCE >= TMR3_COUNT_REFERENCE * REF_NUM) {
-                    REFERENCE_MODE = MODE_REF_DIRECT;
-                    TMR3_COUNT_REFERENCE = 0;
-                }
-                break;
-            }
-            
-            default:
-                break;
-        }
-        
-        */
-
-        // CONTROL LOOP ------------------------------------------------------------
-        
         switch (CONTROL_MODE) {
             case MODE_NO_ACT: {
                 V_out = 0;
@@ -551,7 +442,7 @@
                 //            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;
 
                 V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
@@ -561,9 +452,9 @@
 
             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_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;
+                //int DDV_JOINT_CAN = 0;
                 // feedback input for position control
                 pos.err = pos.ref - (double) pos.sen;
                 pos.err_diff = pos.err - pos.err_old;
@@ -594,9 +485,8 @@
                 //            VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
                 VALVE_POS_RAW_FORCE_FB = 0.0;
 
-                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.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
+                valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
                 VALVE_POS_CONTROL(valve_pos.ref);
                 break;
             }
@@ -686,7 +576,7 @@
 
                 valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
                 VALVE_POS_CONTROL(valve_pos.ref);
-                
+
                 break;
             }
 
@@ -695,66 +585,66 @@
                 break;
             }
 
-           
-            case MODE_TEST_CURRENT_CONTROL:
-            {
+
+            case MODE_TEST_CURRENT_CONTROL: {
                 if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
                     TMR3_COUNT_IREF = 0;
-                } TMR3_COUNT_IREF++;
-            
-                // Set Current Reference 
+                }
+                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  
+                    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:
-            {
+
+            case MODE_TEST_PWM_CONTROL: {
                 if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
                     TMR3_COUNT_IREF = 0;
-                } TMR3_COUNT_IREF++;
-                
+                }
+                TMR3_COUNT_IREF++;
+
                 // Set PWM reference
                 double TMR3_CNT_MAX = (double)TMR_FREQ_5k/2.0;
-                double I_REF_MID = 0.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_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
@@ -762,7 +652,7 @@
                 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;
@@ -770,51 +660,51 @@
                 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_JOINT_POSITION_PRES_CONTROL_CURRENT: {
-                double T_REF = 0.0; // Torque Reference
+                //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;
+                //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
-            
+
                 // 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/(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); 
-                
+                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 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 
+                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 
+                //            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;
@@ -823,20 +713,20 @@
                     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; 
+                    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
@@ -844,7 +734,7 @@
                 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;
@@ -852,18 +742,18 @@
                 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;
             }
 
@@ -871,18 +761,18 @@
                 // DAC Voltage reference set
                 if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
                     CUR_TORQUE_sum += torq.sen;
-                    
+
                     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);
-                        
+
                         if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3;
                         if (TORQUE_VREF < 0) TORQUE_VREF = 0;
-                        
+
                         ROM_RESET_DATA();
-                        
+
                         //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
                         dac_1 = TORQUE_VREF / 3.3;
                     }
@@ -891,18 +781,18 @@
                     TMR3_COUNT_TORQUE_NULL = 0;
                     CUR_TORQUE_sum = 0;
                     CUR_TORQUE_mean = 0;
-                    
+
                     ROM_RESET_DATA();
-                    
+
                     //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
-                    
+
                     //pc.printf("%f\n", TORQUE_VREF);
                     dac_1 = TORQUE_VREF / 3.3;
 
                 }
                 TMR3_COUNT_TORQUE_NULL++;
                 break;
-            }     
+            }
 
             case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
                 if (TMR3_COUNT_DEADZONE == 0) {
@@ -967,9 +857,9 @@
                             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);
 
@@ -982,8 +872,8 @@
             }
 
             case MODE_FIND_HOME: {
-                
-                
+
+
                 ////////////////////////////////////////////FIND_HOME_PWM/////////////////////////////////////////////////////
                 if (FLAG_FIND_HOME == true) {
                     cnt_findhome = 0;
@@ -1032,7 +922,7 @@
                     cnt_findhome = 0;
                     cnt_vel_findhome = 0;
                 }
-                
+
                 /*
                 ////////////////////////////////////////////FIND_HOME_CURRENT/////////////////////////////////////////////////////
                 if (FLAG_FIND_HOME == true) {
@@ -1042,7 +932,7 @@
                     Ref_Joint_Pos = pos.sen;
                     FLAG_FIND_HOME = false;
                 }
-                
+
                 int cnt_check_enc = (TMR_FREQ_5k/500);
                 if(cnt_findhome%cnt_check_enc == 0){
                     FINDHOME_POSITION = pos.sen;
@@ -1050,19 +940,19 @@
                     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 < 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;
                     pos.err = Ref_Joint_Pos - pos.sen;
-                    I_REF = 0.001*((double)pos.err);    
+                    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;
@@ -1076,7 +966,7 @@
                     Ref_Joint_Vel = 0.0;
                     REFERENCE_MODE = MODE_REF_COS_INC;
                     CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION;
-                    
+
                     FINDHOME_POSITION = 0;
                     FINDHOME_POSITION_OLD = 0;
                     FINDHOME_VELOCITY = 0;
@@ -1084,7 +974,7 @@
                     cnt_vel_findhome = 0;
                 }
                 */
-                
+
                 break;
             }
 
@@ -1135,9 +1025,9 @@
                         }
                     }
                     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.0));
                         cur_vel_sum = 0;
                         fl_temp_cnt = 0;
@@ -1180,7 +1070,7 @@
                         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;
-                        
+
                         dac_1 = PRES_A_VREF / 3.3;
                         dac_2 = PRES_B_VREF / 3.3;
                     }
@@ -1193,7 +1083,7 @@
                     CUR_PRES_B_mean = 0;
 
                     ROM_RESET_DATA();
-                
+
                     dac_1 = PRES_A_VREF / 3.3;
                     dac_2 = PRES_B_VREF / 3.3;
                     //pc.printf("nulling end");
@@ -1225,7 +1115,7 @@
                     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.0));
@@ -1273,9 +1163,9 @@
                     VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
                     for(i=0; i<25; i++) {
                         VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
-                        
+
                         //ROM_RESET_DATA();
-                        
+
                         //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) {
                             VALVE_MAX_POS = VALVE_POS_AVG[i];
@@ -1285,9 +1175,9 @@
                             VALVE_POS_AVG_OLD = VALVE_MIN_POS;
                         }
                     }
-                    
+
                     ROM_RESET_DATA();
-                    
+
                     //spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) (VALVE_MAX_POS));
                     //spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) (VALVE_MIN_POS));
                     CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS));
@@ -1374,7 +1264,7 @@
                         //CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM));
 
                     }
-                }else {
+                } 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);
@@ -1468,9 +1358,11 @@
                                 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;
+                                VALVE_DEADZONE_PLUS = SECOND_DZ;
+
                                 ROM_RESET_DATA();
-                                
+
                                 //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
                                 //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
                                 //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
@@ -1566,14 +1458,10 @@
                                 DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
                                 first_check = 0;
                                 VALVE_DEADZONE_MINUS = FIRST_DZ;
-                                
-                                ROM_RESET_DATA();
-                                
                                 //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
                                 VALVE_DEADZONE_PLUS = SECOND_DZ;
-                                
                                 ROM_RESET_DATA();
-                                
+
                                 //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
                                 //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
                                 CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
@@ -1616,15 +1504,15 @@
                         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;
-                        valve_pos.ref = 50*(ID_index_array[ID_index]) + DDV_CENTER;
+                        valve_pos.ref = 10*(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.ref = 10*(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) pos.sen) > 20000) {
+                        if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen) > 20000.0) {
                             FINAL_POS = pos.sen;
                             one_period_end = 1;
                         }
@@ -1640,23 +1528,23 @@
                             min_check = 1;
                         }
                         JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k;   //  pulse/sec
-                        
-                        ROM_RESET_DATA();
-                        
+
+                        //ROM_RESET_DATA();
+
                         //spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + ID_index, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
                         //spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + ID_index, (int16_t) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF));
                         VALVE_FR_timer = 0;
                         one_period_end = 0;
                         ID_index= ID_index +1;
                         //                    CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (ID_index));
+                        V_out = 0.0;
                     }
 
                     if(max_check == 1 && min_check == 1) {
-                        
-                        ROM_RESET_DATA();
-                        
+
                         //spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index));
                         VALVE_POS_NUM = ID_index;
+                        ROM_RESET_DATA();
                         ID_index = 0;
                         first_check = 0;
                         VALVE_FR_timer = 0;
@@ -1704,14 +1592,13 @@
 
         //CAN ----------------------------------------------------------------------
         //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
-        if (TMR3_COUNT_CAN_TX % 10 == 0) {
+        if (TMR3_COUNT_CAN_TX % 1000 == 0) {
 
             if (flag_data_request[0] == HIGH) {
                 //position+velocity
                 CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
                 //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0);
                 //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]);
-                
                 //pc.printf("can good");
                 //                                    CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
             }
@@ -1721,6 +1608,7 @@
                 //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
                 //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
                 CAN_TX_TORQUE((int16_t) (cur.sen));
+                //CAN_TX_TORQUE((int16_t) (Ref_Valve_Pos_FF_CAN));
                 //            CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
             }
 
@@ -1737,7 +1625,7 @@
             if (flag_data_request[3] == HIGH) {
                 //PWM
                 CAN_TX_PWM((int16_t) CUR_PWM);
-                //            CAN_TX_PWM((int16_t) (CONTROL_MODE));
+                //CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN));
                 //            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.));
@@ -1762,9 +1650,9 @@
 
         }
         TMR3_COUNT_CAN_TX++;
-        
-        
-        
+
+
+
 
         /*******************************************************
         ***     Valve Control
@@ -1787,8 +1675,8 @@
         // Saturation of output voltage to 5.0V
 //        if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
 //        else if (PWM_out < -0.41667) PWM_out=-0.41667;
-        
-         // Saturation of output voltage to 12.0V
+
+        // Saturation of output voltage to 12.0V
         if(PWM_out > 1.0) PWM_out=1.0;
         else if (PWM_out < -1.0) PWM_out=-1.0;
 
@@ -1819,7 +1707,7 @@
 //            can.write(msg);
 //        }
 
-        if((CNT_TMR3%5000)==0) {
+        //if((CNT_TMR3%5000)==0) {
 //            if(LED==1) {
 //                LED=0;
 //            } else
@@ -1828,73 +1716,18 @@
 //            pc.printf("A %f\n", (double) pres_A.sen);
 //            CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[j]);
 //            j++;
-        }
+        //}
 
         /*******************************************************
         ***     Timer Counting & etc.
         ********************************************************/
-        CNT_TMR3++;
+        //CNT_TMR3++;
+    
     }
-    
     TIM3->SR = 0x0;  // reset the status register
     
 }
 
-/*
-void ValveControl(unsigned int ControlMode)
-{
-    switch (ControlMode) {
-        case MODE_NO_ACT: // 0
-            V_out = 0.0;
-            break;
-        case MODE_VALVE_OPEN_LOOP: // 1
-            V_out = Vout.ref;
-            break;
-        case MODE_VALVE_POSITION_CONTROL: // 2
-            CurrentControl();
-            break;
-        case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: // 3
-            V_out = 0.0;
-            break;
-        case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: // 4
-            double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
-            double I_REF_POS_FF = 0.0; // I_REF by Position 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;
-
-            cur.ref = I_REF_POS_FF + I_REF_POS_FB;
-            break;
-        case MODE_TEST_CURRENT_CONTROL: // 9
-            V_out = 0.0;
-            break;
-        case MODE_TEST_PWM_CONTROL: // 10
-            V_out = 0.0;
-            break;
-        case MODE_FIND_HOME: // 22
-            V_out = 0.0;
-            break;
-        default:
-            V_out = 0.0;
-            break;
-
-    }
-}
-*/
-
 void CurrentControl()
 {
     cur.err = cur.ref - cur.sen;