rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
Lightvalve
Date:
Tue Apr 05 05:12:12 2022 +0000
Parent:
239:8ac5c6162bc1
Child:
241:27eca07c9591
Commit message:
220405

Changed in this revision

CAN/function_CAN.cpp Show annotated file Show diff for this revision Revisions of this file
INIT_HW/INIT_HW.cpp Show annotated file Show diff for this revision Revisions of this file
INIT_HW/INIT_HW.h Show annotated file Show diff for this revision Revisions of this file
function_utilities/function_utilities.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
setting.h Show annotated file Show diff for this revision Revisions of this file
--- a/CAN/function_CAN.cpp	Fri Jul 30 06:04:10 2021 +0000
+++ b/CAN/function_CAN.cpp	Tue Apr 05 05:12:12 2022 +0000
@@ -506,8 +506,9 @@
         }
 
         case CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE: {
-            TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.01f);
-            spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*100.0f));
+//            TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.01f);
+            TORQUE_SENSOR_PULSE_PER_TORQUE = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8)))*0.001f;
+            spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*1000.0f));
 
             break;
         }
@@ -1125,7 +1126,7 @@
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 3;
     temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
-    int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 100.0f);
+    int16_t temp_torque_sensor_pulse_per_torque = (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE * 1000.0f);
     temp_msg.data[1] = (uint8_t) temp_torque_sensor_pulse_per_torque;
     temp_msg.data[2] = (uint8_t) (temp_torque_sensor_pulse_per_torque >> 8);
 
--- a/INIT_HW/INIT_HW.cpp	Fri Jul 30 06:04:10 2021 +0000
+++ b/INIT_HW/INIT_HW.cpp	Tue Apr 05 05:12:12 2022 +0000
@@ -38,7 +38,8 @@
     NVIC_EnableIRQ(TIM4_IRQn);                         //Enable TIM4 IRQ
 
     TIM4->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
-    TIM4->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
+//    TIM4->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
+    TIM4->CR1 = 0x10;
     TIM4->CR1 |= TIM_CR1_UDIS;
     TIM4->CR1 |= TIM_CR1_ARPE;                                  // autoreload on, 
     TIM4->RCR |= 0x001;                                         // update event once per up/down count of TIM4 
@@ -47,38 +48,40 @@
     //PWM Setup
 
     TIM4->PSC = 0x0;                                            // no prescaler, timer counts up in sync with the peripheral clock
-    TIM4->ARR = PWM_ARR;                                          // set auto reload
+    TIM4->ARR = PWM_ARR-1;                                          // set auto reload
     TIM4->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
     TIM4->CR1 |= TIM_CR1_CEN;                                   // enable TIM4
     
 }
 
+
 void Init_TMR3(){
     RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;                         // enable TIM3 clock
     
      //ISR Setup     
     
-    NVIC_EnableIRQ(TIM3_IRQn);                         //Enable TIM3 IRQ
+    NVIC_EnableIRQ(TIM3_IRQn);                                  //Enable TIM3 IRQ
 
     TIM3->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
-    TIM3->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
+//    TIM3->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
+    TIM3->CR1 = 0x10;
     TIM3->CR1 |= TIM_CR1_UDIS;
     TIM3->CR1 |= TIM_CR1_ARPE;                                  // autoreload on, 
     TIM3->RCR |= 0x001;                                         // update event once per up/down count of TIM3 
     TIM3->EGR |= TIM_EGR_UG;
 
-    TIM3->PSC = 0x00;                                            // no prescaler, timer counts up in sync with the peripheral clock
-    TIM3->ARR = TMR3_COUNT;                                          // set auto reload, 5 khz
+//    TIM3->PSC = 0x00;                                            // no prescaler, timer counts up in sync with the peripheral clock
+    TIM3->PSC = 0x01;
+    TIM3->ARR = TMR3_COUNT-1;                                          // set auto reload, 5 khz
     TIM3->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
     TIM3->CR1 |= TIM_CR1_CEN;                                   // enable TIM4
 }
 
 void Init_TMR2(){
-    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;                         // enable TIM5 clock
+    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;                         // enable TIM2 clock
     
      //ISR Setup     
-    
-    NVIC_EnableIRQ(TIM2_IRQn);                         //Enable TIM5 IRQ
+    NVIC_EnableIRQ(TIM2_IRQn);                                  //Enable TIM2 IRQ
 
     TIM2->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
     TIM2->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
@@ -87,8 +90,28 @@
     TIM2->RCR |= 0x001;                                         // update event once per up/down count of TIM5
     TIM2->EGR |= TIM_EGR_UG;
 
-    TIM2->PSC = 0x12;                                            // no prescaler, timer counts up in sync with the peripheral clock
-    TIM2->ARR = TMR2_COUNT;                                          // set auto reload, 5 khz
+    TIM2->PSC = 0x00;                                            // no prescaler, timer counts up in sync with the peripheral clock
+    TIM2->ARR = TMR2_COUNT-1;                                          // set auto reload, 5 khz
     TIM2->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
-    TIM2->CR1 |= TIM_CR1_CEN;                                   // enable TIM5
-}
\ No newline at end of file
+    TIM2->CR1 |= TIM_CR1_CEN;                                   // enable TIM2
+}
+
+//void Init_TMR2(){
+//    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;                         // enable TIM5 clock
+//    
+//     //ISR Setup     
+//    
+//    NVIC_EnableIRQ(TIM2_IRQn);                         //Enable TIM5 IRQ
+//
+//    TIM2->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
+//    TIM2->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
+//    TIM2->CR1 |= TIM_CR1_UDIS;
+//    TIM2->CR1 |= TIM_CR1_ARPE;                                  // autoreload on, 
+//    TIM2->RCR |= 0x001;                                         // update event once per up/down count of TIM5
+//    TIM2->EGR |= TIM_EGR_UG;
+//
+//    TIM2->PSC = 0x12;                                            // no prescaler, timer counts up in sync with the peripheral clock
+//    TIM2->ARR = TMR2_COUNT;                                          // set auto reload, 5 khz
+//    TIM2->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
+//    TIM2->CR1 |= TIM_CR1_CEN;                                   // enable TIM5
+//}
\ No newline at end of file
--- a/INIT_HW/INIT_HW.h	Fri Jul 30 06:04:10 2021 +0000
+++ b/INIT_HW/INIT_HW.h	Tue Apr 05 05:12:12 2022 +0000
@@ -8,5 +8,6 @@
 void Init_PWM();
 void Init_TMR3();
 void Init_TMR2();
+//void Init_TMR5();
 
 #endif
--- a/function_utilities/function_utilities.cpp	Fri Jul 30 06:04:10 2021 +0000
+++ b/function_utilities/function_utilities.cpp	Tue Apr 05 05:12:12 2022 +0000
@@ -438,7 +438,7 @@
     ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
     STROKE = spi_eeprom_read(RID_STROKE);
     ENC_PULSE_PER_POSITION = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION));
-    TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.01f;
+    TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.001f;
     PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
     PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
     FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
--- a/main.cpp	Fri Jul 30 06:04:10 2021 +0000
+++ b/main.cpp	Tue Apr 05 05:12:12 2022 +0000
@@ -38,6 +38,8 @@
 // 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)
@@ -97,6 +99,7 @@
 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)
 {
@@ -170,13 +173,13 @@
     __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
     /* Initializes the CPU, AHB and APB busses clocks
     */
-    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
-    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
-    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
+    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
+    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
+//    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
     RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
-    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
-    RCC_OscInitStruct.PLL.PLLM = 8;//8
-    RCC_OscInitStruct.PLL.PLLN = 180; //180
+    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
+    RCC_OscInitStruct.PLL.PLLM = 4;//4
+    RCC_OscInitStruct.PLL.PLLN = 96; //96
     RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
     RCC_OscInitStruct.PLL.PLLQ = 2;
     RCC_OscInitStruct.PLL.PLLR = 2;
@@ -196,10 +199,6 @@
     RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
     RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
     RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
-
-    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) {
-        //Error_Handler();
-    }
 }
 
 
@@ -241,7 +240,7 @@
     make_delay();
 
     ////// bno rom
-    spi_eeprom_write(RID_BNO, (int16_t) 1);
+    spi_eeprom_write(RID_BNO, (int16_t) 0);
     make_delay();
     ////////
 
@@ -271,9 +270,15 @@
     TIM3->CR1 ^= TIM_CR1_UDIS;
     make_delay();
 
+    // TMR5 init
+    Init_TMR2();
+    TIM2->CR1 ^= TIM_CR1_UDIS;
+    make_delay();
+
     //Timer priority
     NVIC_SetPriority(TIM3_IRQn, 2);
     NVIC_SetPriority(TIM4_IRQn, 3);
+    NVIC_SetPriority(TIM2_IRQn, 4);
 
 
     //DAC init
@@ -282,8 +287,8 @@
         dac_2 = 0.0f;
     } else if (SENSING_MODE == 1) {
 //        if (DIR_VALVE_ENC > 0) {
-            dac_1 = PRES_A_VREF / 3.3f;
-            dac_2 = PRES_B_VREF / 3.3f;
+        dac_1 = PRES_A_VREF / 3.3f;
+        dac_2 = PRES_B_VREF / 3.3f;
 //        } else {
 //            dac_1 = PRES_B_VREF / 3.3f;
 //            dac_2 = PRES_A_VREF / 3.3f;
@@ -298,6 +303,12 @@
             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!
     *************************************/
@@ -468,6 +479,49 @@
                             TIMER INTERRUPT
 *******************************************************************************/
 
+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;
+
+        LVDT_sum = 0.0f;
+
+        LVDT_H = 1;
+        LVDT_L = 0;
+
+        for (int ij = 0; ij<400; ij++) {
+//            LED = 1;
+            ADC1->CR2  |= 0x40000000;
+            LVDT_new = ((float)ADC1->DR) - 2047.5f;
+            LVDT_sum = LVDT_sum + LVDT_new;
+//            LED = 0;
+        }
+
+        LVDT_H = 0;
+        LVDT_L = 0;
+
+//        LED = 0;
+        LVDT_new = LVDT_sum * 0.0025f;
+
+        float alpha_LVDT = 1.0f;//1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f));
+        LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
+        force.sen = LVDT_LPF;
+
+    }
+    TIM3->SR = 0x0;  // reset the status register
+}
+
+
+
 //------------------------------------------------
 //     TMR4 : Sensor Read & Data Handling
 //-----------------------------------------------
@@ -479,6 +533,9 @@
 {
     if (TIM4->SR & TIM_SR_UIF ) {
 
+//        if (LED > 0) LED = 0;
+//        else LED = 1;
+
         // Current ===================================================
         //ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
 
@@ -489,19 +546,20 @@
             ENC_UPDATE();
         }
 
+        /*
         // Force or Pressure Transducer =============================================
         ADC1->CR2  |= 0x40000000;
         if (SENSING_MODE == 0) {  // Force sensing
             force.UpdateSen((((float)ADC1->DR) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR4, 100.0f); // unit : N
         } else if (SENSING_MODE == 1) { // Pressure sensing
             float pres_A_new, pres_B_new;
-//            if (DIR_VALVE_ENC > 0) {
-                pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
-                pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
-//            } else {
-//                pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
-//                pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
-//            }
+        //            if (DIR_VALVE_ENC > 0) {
+            pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
+            pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
+        //            } else {
+        //                pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
+        //                pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
+        //            }
             pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f);
             pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f);
 
@@ -513,6 +571,7 @@
                 force.UpdateSen(force_new,FREQ_TMR4,1000.0f);  // unit : N
             }
         }
+        */
 
         CNT_TMR4++;
     }
@@ -521,32 +580,35 @@
 
 
 int j =0;
-float FREQ_TMR3 = (float)FREQ_5k;
-float DT_TMR3 = (float)DT_5k;
+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;
 
-extern "C" void TIM3_IRQHandler(void)
+extern "C" void TIM2_IRQHandler(void)
 {
-    if (TIM3->SR & TIM_SR_UIF ) {
+    if (TIM2->SR & TIM_SR_UIF ) {
+
+        if (LED > 0) LED = 0;
+        else LED = 1;
 
         if(MODE_POS_FT_TRANS == 1) {
             if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2;
-            alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
+            alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR5 /3.0f))/2.0f;
             cnt_trans++;
             torq.err_int = 0.0f;
             force.err_int = 0.0f;
-            if((float)cnt_trans * DT_TMR3 > 3.0f)
+            if((float)cnt_trans * DT_TMR5 > 3.0f)
                 MODE_POS_FT_TRANS = 2;
         } else if(MODE_POS_FT_TRANS == 3) {
             if (alpha_trans == 0.0f) MODE_POS_FT_TRANS = 0;
-            alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
+            alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR5 /3.0f))/2.0f;
             cnt_trans++;
             torq.err_int = 0.0f;
             force.err_int = 0.0f;
-            if((float) cnt_trans * DT_TMR3 > 3.0f )
+            if((float) cnt_trans * DT_TMR5 > 3.0f )
                 MODE_POS_FT_TRANS = 0;
         } else if(MODE_POS_FT_TRANS == 2) {
             alpha_trans = 1.0f;
@@ -1032,9 +1094,9 @@
                 } 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_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);
@@ -1068,15 +1130,15 @@
             }
 
             case MODE_FREQ_TEST: {
-                float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3);
+                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;
                 } 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_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);
@@ -1086,7 +1148,7 @@
 
                 CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
                 cnt_freq_test++;
-                if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR3 > 2) {
+                if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR5 > 2) {
                     buffer_data_size = cnt_freq_test;
                     cnt_freq_test = 0;
                     cnt_send_buffer = 0;
@@ -1166,11 +1228,11 @@
                 } else {
                     float force_ref_act = force.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : N
                     //////////////////////////////////////////////////force_reference_filter////////////////////////////////////////////////////////////////////
-                    float alpha_torque_ref = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*1.0f));
-                    force_ref_filter = (1.0f-alpha_torque_ref) * force_ref_filter + alpha_torque_ref * force_ref_act;
+//                    float alpha_torque_ref = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*1.0f));
+//                    force_ref_filter = (1.0f-alpha_torque_ref) * force_ref_filter + alpha_torque_ref * force_ref_act;
                     ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-                    force_ref_act_can = force_ref_filter;
-                    force.err = force_ref_filter - force.sen;
+                    force_ref_act_can = force_ref_act;
+                    force.err = force_ref_act - force.sen;
                     force.err_int += force.err/((float)TMR_FREQ_5k);
                     temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s
                 }
@@ -1264,6 +1326,7 @@
 //                        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;
@@ -1477,14 +1540,14 @@
 
                 } 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) (pos.sen*200.0f), (int16_t) (valve_pos_can*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+//                    CAN_TX_POSITION_FT((int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f), (int16_t) (vel.sen*20.0f), (int16_t) (((float)ADC1->DR)));
                 }
             }
 
             // 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)(alpha3));
+//                CAN_TX_PWM((int16_t)(TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f));
             }
 
             // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
@@ -1504,7 +1567,7 @@
 
                 valve_pos_ref_can = (float)valve_pos.ref;
 
-                CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) force_ref_act_can);
+                CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_pulse_can);
             }
 
             TMR2_COUNT_CAN_TX = 0;
@@ -1512,6 +1575,6 @@
         TMR2_COUNT_CAN_TX++;
 
     }
-    TIM3->SR = 0x0;  // reset the status register
+    TIM2->SR = 0x0;  // reset the status register
 
 }
\ No newline at end of file
--- a/setting.h	Fri Jul 30 06:04:10 2021 +0000
+++ b/setting.h	Tue Apr 05 05:12:12 2022 +0000
@@ -6,11 +6,13 @@
 #define PIN_W       PB_6
 //#define PWM_ARR     0x465       // loop 80k, pwm 40k 
 //#define PWM_ARR     0x8CA       // loop 40k, pwm 20k
-#define PWM_ARR     0x1194      // loop 20k, pwm 10k
+#define PWM_ARR     0x1194      // loop 20k, pwm 10k 원래이거
 //#define PWM_ARR     0x2328      // loop 10k, pwm 5k
-#define TMR3_COUNT  0x4650      // loop 5k
-//#define TMR3_COUNT  0x2328      // loop 10k
-#define TMR2_COUNT  0x2710      // loop 500hz with prescale 18
+//#define PWM_ARR     0xAFC8        // loop 2 k, pwm 1k
+
+#define TMR3_COUNT  0xAFC8      // loop 2 k, pwm 1k
+//#define TMR2_COUNT  0x2710      // loop 500hz with prescale 18
+#define TMR2_COUNT  0x4650      // loop 5 k
 
 #define FREQ_500    500.0f
 #define FREQ_1k     1000.0f