Diff: main.cpp

Revision:

231:30896263bd8b

Parent:

230:b235d67d25ba

Child:

232:e9c5ec04e378

--- a/main.cpp	Thu Apr 07 06:18:38 2022 +0000
+++ b/main.cpp	Mon Jun 13 08:48:55 2022 +0000
@@ -1,20 +1,12 @@
-//Hydraulic Control Board
+//Hydraulic Control Board Rainbow
 //distributed by Sungwoo Kim
-//       2020/12/28
-//revised by Buyoun Cho
-//       2021/04/20
-
-// 유의사항
-// 소수 적을때 뒤에 f 꼭 붙이기
-// CAN 선은 ground까지 있는 3상 선으로 써야함.
-// 전원은 12~24V 인가.
+//       2022/05/31
 
 #include "mbed.h"
 #include "FastPWM.h"
 #include "INIT_HW.h"
 #include "function_CAN.h"
 #include "SPI_EEP_ENC.h"
-#include "I2C_AS5510.h"
 #include "setting.h"
 #include "function_utilities.h"
 #include "stm32f4xx_flash.h"
@@ -23,39 +15,34 @@
 #include <iostream>
 #include <cmath>
 
-using namespace std;
-Timer t;
 
-// dac & check ///////////////////////////////////////////
-DigitalOut check(PC_2);
-DigitalOut check_2(PC_3);
+// DAC ///////////////////////////////////////////
 AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f
 AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f
-AnalogIn adc1(PC_4); //pressure_1
-AnalogIn adc2(PB_0); //pressure_2
-AnalogIn adc3(PC_1); //current
+
+// ADC ///////////////////////////////////////////
+//AnalogIn adc1(PC_4); //pressure_1
+//AnalogIn adc2(PC_5); //pressure_2
+//AnalogIn adc3(PC_1); //current
+//AnalogIn adc4(PB_0); //LVDT
 
 // PWM ///////////////////////////////////////////
 float dtc_v=0.0f;
 float dtc_w=0.0f;
-DigitalOut LVDT_H(PB_4); //PWM_H
-DigitalOut LVDT_L(PB_5); //PWM_L
 
-// I2C ///////////////////////////////////////////
-I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
-const int i2c_slave_addr1 =  0x56;  // AS5510 address
-unsigned int value; // 10bit output of reading sensor AS5510
+// LVDT ///////////////////////////////////////////
+//DigitalOut LVDT_H(PB_6);
+//DigitalOut LVDT_L(PB_7);
 
 // SPI ///////////////////////////////////////////
 SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
 DigitalOut eeprom_cs(PB_12);
 SPI enc(PC_12,PC_11,PC_10);
 DigitalOut enc_cs(PD_2);
+
+// LED ///////////////////////////////////////////
 DigitalOut LED(PA_15);
 
-// UART ///////////////////////////////////////////
-Serial pc(PA_9,PA_10); //  _ UART
-
 // CAN ///////////////////////////////////////////
 CAN can(PB_8, PB_9, 1000000);
 CANMessage msg;
@@ -77,11 +64,6 @@
 State valve_pos;
 State valve_pos_raw;
 
-State INIT_Vout;
-State INIT_Valve_Pos;
-State INIT_Pos;
-State INIT_torq;
-
 extern int CID_RX_CMD;
 extern int CID_RX_REF_POSITION;
 extern int CID_RX_REF_OPENLOOP;
@@ -95,12 +77,6 @@
 extern int CID_TX_VALVE_POSITION;
 extern int CID_TX_SOMETHING;
 
-float temp_P_GAIN = 0.0f;
-float temp_I_GAIN = 0.0f;
-int temp_VELOCITY_COMP_GAIN = 0;
-int logging = 0;
-float valve_pos_pulse_can = 0.0f;
-
 inline float tanh_inv(float y)
 {
     if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f;
@@ -167,12 +143,9 @@
     RCC_OscInitTypeDef RCC_OscInitStruct = {0};
     RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
 
-    /* Configure the main internal regulator output voltage
-    */
     __HAL_RCC_PWR_CLK_ENABLE();
     __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
-    /* Initializes the CPU, AHB and APB busses clocks
-    */
+
     RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
     RCC_OscInitStruct.HSEState = RCC_HSE_ON;
 //    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
@@ -202,36 +175,26 @@
 }
 
 
+
 int main()
 {
-    /*********************************
-    ***     Initialization
-    *********************************/
-
+    
     HAL_Init();
     SystemClock_Config();
 
     LED = 0;
-    pc.baud(9600);
 
-    // i2c init
-    i2c.frequency(400 * 1000);          // 0.4 mHz
-    wait_ms(2);                         // Power Up wait
-    look_for_hardware_i2c();            // Hardware present
-    init_as5510(i2c_slave_addr1);
-    make_delay();
-
-    // spi init
+    // SPI INIT
     eeprom_cs = 1;
     eeprom.format(8,3);
     eeprom.frequency(5000000); //5M
     eeprom_cs = 0;
     make_delay();
 
-    enc_cs = 1;     //sw add
+    enc_cs = 1;     
     enc.format(8,0);
     enc.frequency(5000000); //10M
-    enc_cs = 0;     //sw add
+    enc_cs = 0;
 
     make_delay();
 
@@ -239,7 +202,7 @@
     spi_enc_set_init();
     make_delay();
 
-    ////// bno rom
+    //  bno rom
     spi_eeprom_write(RID_BNO, (int16_t) 0);
     make_delay();
     ////////
@@ -263,25 +226,41 @@
     make_delay();
 
     //can.reset();
-//    can.filter(msg.id, 0xFFFFF000, CANStandard);
-//    can.filter(0x050, 0xFFFFFFFF, CANStandard);
-    can.filter(0b100000000, 0b100000010, CANStandard);  //CAN ID 100~400번대 통과하게
+    can.filter(msg.id, 0xFFFFF000, CANStandard);
+//    can.filter(0b100000000, 0b100000010, CANStandard);  //CAN ID 100~400번대 통과하게
 
     // TMR3 init
     Init_TMR3();
     TIM3->CR1 ^= TIM_CR1_UDIS;
     make_delay();
 
-    // TMR5 init
+    // TMR2 init
     Init_TMR2();
     TIM2->CR1 ^= TIM_CR1_UDIS;
     make_delay();
 
+    // TMR1 init
+    Init_TMR1();
+    TIM1->CR1 ^= TIM_CR1_UDIS;
+    make_delay();
+
     //Timer priority
     NVIC_SetPriority(TIM3_IRQn, 2);
     NVIC_SetPriority(TIM4_IRQn, 3);
     NVIC_SetPriority(TIM2_IRQn, 4);
 
+//    HAL_NVIC_SetPriority(TIM2_IRQn, 4, 0);
+//    HAL_NVIC_EnableIRQ(TIM2_IRQn);
+//    /* TIM3_IRQn interrupt configuration */
+//    HAL_NVIC_SetPriority(TIM3_IRQn, 2, 0);
+//    HAL_NVIC_EnableIRQ(TIM3_IRQn);
+//    /* TIM4_IRQn interrupt configuration */
+//    HAL_NVIC_SetPriority(TIM4_IRQn, 3, 0);
+//    HAL_NVIC_EnableIRQ(TIM4_IRQn);
+//    /* CAN1_RX0_IRQn interrupt configuration */
+//    HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 14, 0);
+//    HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
+
 
     //DAC init
     if (SENSING_MODE == 0) {
@@ -305,29 +284,16 @@
             ID_index_array[i] =  (i+1) * 0.5f;
     }
 
-    //pwm
-    TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
-    TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f);
-    LVDT_H = 0;
-    LVDT_L = 0;
-
     /************************************
     ***     Program is operating!
     *************************************/
     while(1) {
+        
+//            if (LED > 0) LED = 0;
+//            else LED = 1;
+        TIM1->CCR1 = (TMR1_COUNT)*(0.7f);
 
-        // UART example
-//        if(timer_while==100000) {
-//            timer_while = 0;
-//            pc.printf("%f\n", value);
-//        }
-//        timer_while ++;
 
-        //i2c for SW valve
-//        if(OPERATING_MODE == 5) {
-//            read_field(i2c_slave_addr1);
-//            if(DIR_VALVE_ENC < 0) value = 1023 - value;
-//        }
     }
 }
 
@@ -375,7 +341,7 @@
     } else if(REF_VALVE_POS < VALVE_MIN_POS) {
         REF_VALVE_POS = VALVE_MIN_POS;
     }
-    valve_pos_err = (float) (REF_VALVE_POS - value);
+    valve_pos_err = (float) (REF_VALVE_POS - valve_pos.sen);
     valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
     valve_pos_err_old = valve_pos_err;
     valve_pos_err_sum += valve_pos_err;
@@ -473,7 +439,6 @@
             }
         }
     }
-
     return PWM_duty;
 }
 
@@ -483,43 +448,65 @@
                             TIMER INTERRUPT
 *******************************************************************************/
 
+
+//------------------------------------------------
+//     TMR3 : LVDT 1kHz
+//-----------------------------------------------
 float LVDT_new = 0.0f;
 float LVDT_old = 0.0f;
 float LVDT_f_cut = 1000.0f;
 float LVDT_LPF = 0.0f;
 float LVDT_sum = 0.0f;
-float LVDT_can[100] = {0.0f};
 
 extern "C" void TIM3_IRQHandler(void)
 {
     if (TIM3->SR & TIM_SR_UIF ) {
 
-//        if (LED > 0) LED = 0;
-//        else LED = 1;
+////        if (LED > 0) LED = 0;
+////        else LED = 1;
+//
+//        LVDT_sum = 0.0f;
+//
+//
+//        LVDT_L = 0;
+//        LVDT_H = 1;
+//
+////        LED = 1;
+//
+//        for (int ij = 0; ij<150; ij++) {
+//            ADC1->CR2  |= 0x40000000;
+//            LVDT_new = ((float)ADC1->DR) - 2047.5f;
+//            LVDT_sum = LVDT_sum + LVDT_new;
+//        }
+//
+////        LED = 0;
+//
+//        LVDT_H = 0;
+//        LVDT_L = 1;
 
-        LVDT_sum = 0.0f;
-
-        LVDT_H = 1;
-        LVDT_L = 0;
 
-        for (int ij = 0; ij<200; ij++) {
-//            LED = 1;
-            ADC1->CR2  |= 0x40000000;
-            LVDT_new = ((float)ADC1->DR) - 2047.5f;
-            LVDT_sum = LVDT_sum + LVDT_new;
-//            LED = 0;
-        }
+
+
+//        LVDT_new = LVDT_sum * 0.01f*2.0f;
+//
+//        float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f));
+//        LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
+//        valve_pos.sen = LVDT_LPF;
+//        if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen;
 
-        LVDT_H = 0;
-        LVDT_L = 0;
+//        TIM4->CCR2 = (PWM_ARR)*(0.95f);
+//        TIM4->CCR1 = (PWM_ARR)*(0.975f);
+
+//        TIM8->CCR1 = (TMR8_COUNT)*(0.95f);
+//        TIM8->CCR2 = (TMR8_COUNT)*(0.975f);
+
 
-//        LED = 0;
-        LVDT_new = LVDT_sum * 0.005f;
+//        PWM_H2 = 0;
+//        PWM_L2 = 1;
 
-        float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f));
-        LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
-        valve_pos.sen = LVDT_LPF;
-        if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen;
+//        TIM3->CCR2 = (TMR3_COUNT)*(0.95f);
+//        TIM3->CCR1 = (TMR3_COUNT)*(0.975f);
+
 
     }
     TIM3->SR = 0x0;  // reset the status register
@@ -538,9 +525,6 @@
 {
     if (TIM4->SR & TIM_SR_UIF ) {
 
-//        if (LED > 0) LED = 0;
-//        else LED = 1;
-
         // Current ===================================================
         //ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
 
@@ -578,6 +562,51 @@
         }
         */
 
+//        //FORWARD
+//        if(CNT_TMR4 < 2)
+//        {
+//            PWM_L2 = 0;
+//            PWM_H1 = 0;
+//
+//            PWM_H2 = 1;
+//            PWM_L1 = 1;
+//        } else if(CNT_TMR4 < 50)
+//        {
+//            PWM_H2 = 0;
+//            PWM_H1 = 0;
+//
+//            PWM_L2 = 1;
+//            PWM_L1 = 1;
+//        } else
+//        {
+//            CNT_TMR4 = 0;
+//        }
+
+
+//        //BACKWARD
+//        if(CNT_TMR4 < 2)
+//        {
+//            PWM_H2 = 0;
+//            PWM_L1 = 0;
+//
+//            PWM_L2 = 1;
+//            PWM_H1 = 1;
+//        } else if(CNT_TMR4 < 50)
+//        {
+//            PWM_H2 = 0;
+//            PWM_H1 = 0;
+//
+//            PWM_L2 = 1;
+//            PWM_L1 = 1;
+//        } else
+//        {
+//            CNT_TMR4 = 0;
+//        }
+
+
+
+
+
         CNT_TMR4++;
     }
     TIM4->SR = 0x0;  // reset the status register
@@ -588,7 +617,6 @@
 float FREQ_TMR5 = (float)FREQ_5k;
 float DT_TMR5 = (float)DT_5k;
 int cnt_trans = 0;
-double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
 int can_rest =0;
 float force_ref_act_can = 0.0f;
 
@@ -828,7 +856,6 @@
                     data_num = 0;
                 } else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
                     data_num = data_num + 1;
-//                    VALVE_POS_TMP = VALVE_POS_TMP + value;
                     VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen;
                 } else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
                     Vout.ref = 0.0f;
@@ -884,7 +911,7 @@
                     } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
                         Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
                         data_num = data_num + 1;
-                        VALVE_POS_TMP = VALVE_POS_TMP + value;
+                        VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen;
                     } else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
                         Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
                         DDV_POS_AVG = VALVE_POS_TMP / data_num;
@@ -923,7 +950,7 @@
                     }
                 } else {
                     if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) {
-                        if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) {
+                        if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
                             Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                             START_POS = pos.sen;
@@ -1096,18 +1123,18 @@
                     valve_pos_ref = 10000.0f;
                 }
                 if(valve_pos_ref >= 0) {
-                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 } else {
-                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 }
 
                 VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                 ref_array[cnt_step_test] = valve_pos_ref;
-                if(value>=(float) VALVE_ELECTRIC_CENTER) {
-                    pos_array[cnt_step_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER);
+                if(valve_pos.sen >= (float) VALVE_ELECTRIC_CENTER) {    
+                    pos_array[cnt_step_test] = 10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
                 } else {
-                    pos_array[cnt_step_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_CENTER);
+                    pos_array[cnt_step_test] = -10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
                 }
 
                 CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
@@ -1119,17 +1146,10 @@
                     CONTROL_UTILITY_MODE = MODE_SEND_OVER;
                     CONTROL_MODE = MODE_NO_ACT;
                 }
-//                if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k))
-//                {
-//                    CONTROL_UTILITY_MODE = MODE_NO_ACT;
-//                    CONTROL_MODE = MODE_NO_ACT;
-//                    CAN_TX_PWM((int16_t) (1)); //1300
-//                }
 
                 break;
             }
             case MODE_SEND_OVER: {
-                CAN_TX_TORQUE((int16_t) (buffer_data_size)); //1300
                 CONTROL_UTILITY_MODE = MODE_NO_ACT;
                 CONTROL_MODE = MODE_NO_ACT;
                 break;
@@ -1138,18 +1158,21 @@
             case MODE_FREQ_TEST: {
                 float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR5);
                 if(valve_pos_ref >= 0) {
-                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 } else {
-                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+                    valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 }
 
                 VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                 ref_array[cnt_freq_test] = valve_pos_ref;
-                if(value>=(float) VALVE_ELECTRIC_CENTER) {
-                    pos_array[cnt_freq_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER);
+//                if(value>=(float) VALVE_ELECTRIC_CENTER) {
+                if(valve_pos.sen>=(float) VALVE_ELECTRIC_CENTER) {    
+//                    pos_array[cnt_freq_test] = 10000.0f*((float)value - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
+                    pos_array[cnt_freq_test] = 10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
                 } else {
-                    pos_array[cnt_freq_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_CENTER);
+//                    pos_array[cnt_freq_test] = -10000.0f*((float)value - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
+                    pos_array[cnt_freq_test] = -10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
                 }
 
                 CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
@@ -1162,7 +1185,6 @@
                     if (freq_test_valve_ref >= 400) {
                         CONTROL_UTILITY_MODE = MODE_NO_ACT;
                         CONTROL_MODE = MODE_NO_ACT;
-                        CAN_TX_VOUT((int16_t) (1)); //1300
                     }
                     CONTROL_MODE = MODE_NO_ACT;
                     CONTROL_UTILITY_MODE = MODE_SEND_OVER;
@@ -1281,7 +1303,6 @@
 
                     float I_MAX = 10.0f; // Maximum Current : 10mA
                     // Anti-windup for FT
-                    //                if (I_GAIN_JOINT_TORQUE != 0.0f) {
                     if (I_GAIN_JOINT_TORQUE > 0.001f) {
                         float Ka = 2.0f;
                         if (I_REF > I_MAX) {
@@ -1305,7 +1326,6 @@
                 } else {    //SW valve
                     float Valve_pos_MAX = 10000.0f; // Maximum Valve Pos : 10000
                     // Anti-windup for FT
-                    //                if (I_GAIN_JOINT_TORQUE != 0.0f) {
                     if (I_GAIN_JOINT_TORQUE > 0.001f) {
                         float Ka = 2.0f;
                         if (valve_pos_pulse > Valve_pos_MAX) {
@@ -1332,7 +1352,6 @@
 //                        valve_pos.ref = -valve_pos_pulse/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS;
 //                    }
                     VALVE_POS_CONTROL_DZ(valve_pos_pulse);
-                    valve_pos_pulse_can = valve_pos_pulse;
                     V_out = Vout.ref;
                 }
                 break;
@@ -1343,79 +1362,6 @@
                 break;
             }
 
-//            case MODE_JOINT_ADAPTIVE_BACKSTEPPING: {
-//
-//                float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x,      unit : m^3
-//                float Vb = (1256.6f + Amm  * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x),      unit : m^3
-//
-//                V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f
-//
-//                //float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V_adapt * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s    //xdot=10mm/s일때 -137076
-//                float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s    //xdot=10mm/s일때 -137076
-//
-//                float g3_prime = 0.0f;
-//                if (torq.sen > Amm*(Ps-Pt)*0.000001f) {
-//                    g3_prime = 1.0f;
-//                } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) {
-//                    g3_prime = -1.0f;
-//                } else {
-//                    if ((value-VALVE_CENTER) > 0) {
-//                        g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f);
-////                        g3_prime = sqrt(Ps-Pt);
-//                    } else {
-//                        g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f);
-////                        g3_prime = sqrt(Ps-Pt);
-//                    }
-//                }
-//                float tau = 0.01f;
-//                float K_valve = 0.0004f;
-//
-//                float x_v = 0.0f;   //x_v : -1~1
-//                if(value>=VALVE_CENTER) {
-//                    x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-//                } else {
-//                    x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-//                }
-//                float f4 = -x_v/tau;
-//                float g4 = K_valve/tau;
-//
-//                float torq_ref_dot = torq.ref_diff * 500.0f;
-//
-//                pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
-//                vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
-//                pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
-//
-//                torq.err = torq.ref - torq.sen; //[N]
-//                torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
-//
-//                float k3 = 2000.0f; //2000  //20000
-//                float k4 = 10.0f;
-//                float rho3 = 3.2f;
-//                float rho4 = 10000000.0f;  //25000000.0f;
-//                float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime);
-//                if (x_4_des > 1) x_4_des = 1;
-//                else if (x_4_des < -1) x_4_des = -1;
-//
-//                if (x_4_des > 0) {
-//                    valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER;
-//                } else {
-//                    valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER;
-//                }
-//
-//                float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k;
-//                x_4_des_old = x_4_des;
-//                V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4;
-//
-//                float rho_a = 0.00001f;
-//                float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err);
-//                a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k;
-//
-//                if(a_hat > -3000000.0f) a_hat = -3000000.0f;
-//                else if(a_hat < -30000000.0f) a_hat = -30000000.0f;
-//
-//                break;
-//            }
-
             default:
                 break;
         }
@@ -1437,7 +1383,6 @@
                 I_ERR = I_REF_fil_DZ - (double)cur.sen;
                 I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
 
-
                 // Moog Valve Current Control Gain
                 double R_model = 500.0f; // ohm
                 double L_model = 1.2f;
@@ -1455,7 +1400,6 @@
                 }
 
                 double FF_gain = 1.0f;
-
                 VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT;
                 I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old;
                 I_REF_fil_old = I_REF_fil_DZ;
@@ -1527,54 +1471,38 @@
             dtc_w=0.0f;
         }
 
-        //pwm
-        TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
-        TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
 
         ////////////////////////////////////////////////////////////////////////////
         //////////////////////  Data transmission through CAN //////////////////////
         ////////////////////////////////////////////////////////////////////////////
 
-        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
 
+//        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/500) == 0) {
 
             // Position, Velocity, and Torque (ID:1200)
-//            if (flag_data_request[0] == LOW) {
+            if (flag_data_request[0] == HIGH) {
 
-//                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
-//                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-//                    CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
 
-//                } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
-//                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-            CAN_TX_POSITION_FT((int16_t) (7.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (valve_pos.sen));
-//                }
-//            }
+                } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+                }
+            }
 
             // Valve Position (ID:1300)
             if (flag_data_request[1] == HIGH) {
-                CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
-//                CAN_TX_PWM((int16_t)(TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f));
+                if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
+                    CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
+                } else {
+                    CAN_TX_PWM((int16_t)(valve_pos.sen));
+                }
             }
 
-            // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
+            // Others : SW (ID:1400)
             if (flag_data_request[2] == HIGH) {
-                float valve_pos_can = 0.0f;
-                if(value >= VALVE_ELECTRIC_CENTER) {
-                    valve_pos_can = 10000.0f*((float)value-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
-                } else {
-                    valve_pos_can = -10000.0f*((float)value -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
-                }
-                float valve_pos_ref_can = 0.0f;
-                if(valve_pos.ref >= VALVE_ELECTRIC_CENTER) {
-                    valve_pos_ref_can = 10000.0f*((float)valve_pos.ref-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
-                } else {
-                    valve_pos_ref_can = -10000.0f*((float)valve_pos.ref -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
-                }
-
-                valve_pos_ref_can = (float)valve_pos.ref;
-
-                CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_pulse_can);
+                CAN_TX_CURRENT((int16_t) valve_pos.sen, (int16_t) valve_pos.ref);
             }
 
             TMR2_COUNT_CAN_TX = 0;
@@ -1583,5 +1511,4 @@
 
     }
     TIM2->SR = 0x0;  // reset the status register
-
-}
\ No newline at end of file
+}