rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
Lightvalve
Date:
Fri Jun 17 06:06:53 2022 +0000
Parent:
243:30896263bd8b
Child:
245:071785d74ad0
Commit message:
220617

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	Mon Jun 13 08:48:55 2022 +0000
+++ b/CAN/function_CAN.cpp	Fri Jun 17 06:06:53 2022 +0000
@@ -25,12 +25,7 @@
 
 extern DigitalOut LED;
 
-extern float u_past[];
-extern float x_past[];
-extern float x_future[];
-extern float f_past[];
-extern float f_future[];
-extern float input_NN[];
+int can_test = 0;
 
 /*******************************************************************************
  * State Class functions
@@ -674,10 +669,6 @@
 
 void CAN_RX_HANDLER()
 {
-    
-//    if (LED > 0) LED = 0;
-//    else LED = 1;
-    
     can.read(msg);
     
     unsigned int address = msg.id;
--- a/INIT_HW/INIT_HW.cpp	Mon Jun 13 08:48:55 2022 +0000
+++ b/INIT_HW/INIT_HW.cpp	Fri Jun 17 06:06:53 2022 +0000
@@ -4,142 +4,184 @@
 
 extern ADC_HandleTypeDef        hadc1;
 extern ADC_HandleTypeDef        hadc2;
+extern ADC_HandleTypeDef        hadc3;
 
-//void Init_ADC(void){
-//    // ADC Setup
-//     RCC->APB2ENR |= RCC_APB2ENR_ADC3EN;                        // clock for ADC3
-//     RCC->APB2ENR |= RCC_APB2ENR_ADC2EN;                        // clock for ADC2
-//     RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;                        // clock for ADC1
-//
-//     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // Enable clock for GPIOC
-//     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;                        // Enable clock for GPIOA
-//
-//     ADC->CCR = 0x00000016;                                     // Regular simultaneous mode only
-//     ADC1->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC1 ON
-//     ADC1->SQR3 = 0x0000000E;                    //channel      // use PC_4 as input- ADC1_IN14
-//     ADC2->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC2 ON
-//     ADC2->SQR3 = 0x00000008;                                   // use PB_0 as input - ADC2_IN8
-//     ADC3->CR2 |= ADC_CR2_ADON;                                 // ADC3 ON
-//     ADC3->SQR3 = 0x0000000B;                                   // use PC_1, - ADC3_IN11
-//     GPIOC->MODER |= 0b1100001100;             //each channel   // PC_4, PC_1 are analog inputs
-//     GPIOB->MODER |= 0x3;                                       // PB_0 as analog input
-//
-//     ADC1->SMPR1 |= 0x00001000;                                     // 15 cycles on CH_14, 0b0001000000000000
-//     ADC2->SMPR2 |= 0x01000000;                                     // 15 cycles on CH_8,  0b0000000100000000<<16
-//     ADC3->SMPR1 |= 0x00000008;                                     // 15 cycles on CH_11, 0b0000000000001000
-//}
-
-
-void Init_ADC(void)
+void Init_ADC1(void)
 {
+/*
     // ADC Setup
 //     RCC->APB2ENR |= RCC_APB2ENR_ADC3EN;                        // clock for ADC3
 //     RCC->APB2ENR |= RCC_APB2ENR_ADC2EN;                        // clock for ADC2
     RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;                        // clock for ADC1
-
-//     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // Enable clock for GPIOC
-    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;                        // Enable clock for GPIOB
-
-    ADC->CCR = 0x00000016;                                     // Regular simultaneous mode only
+     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // Enable clock for GPIOC
+//    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;                        // Enable clock for GPIOB
     ADC1->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC1 ON
-    ADC1->SQR3 = 0x00000008;                                   // use PB_0 as input - ADC1_IN8
-//     ADC1->SQR3 = 0x0000000E;                    //channel      // use PC_4 as input- ADC1_IN14
+//    ADC->CCR = 0x00000016;                                     // Regular simultaneous mode only
+    ADC1->CR1 |= 0x800;                                         // Discontinuous mode
+    ADC1->CR1 |= 0x100;                                         // Scan mode
+    ADC1->CR2 |= 0x400;                                         // EOC
+    ADC1->SQR1 |= 0x100000;                                     // 2 conversions
+//    ADC1->SQR3 = 0x00000008;                                   // use PB_0 as input - ADC1_IN8
+     ADC1->SQR3 |= 0x0000000E;                    //channel      // use PC_4 as input - ADC1_IN14
+     ADC1->SQR3 |= 0x000001E0;                                   // use PC_5 as input - ADC1_IN15  0b0000 0000 0000 0000 0000 0001 1110 0000
 //     ADC2->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC2 ON
 //     ADC2->SQR3 = 0x00000008;                                   // use PB_0 as input - ADC2_IN8
-//     ADC3->CR2 |= ADC_CR2_ADON;                                 // ADC3 ON
+//     ADC3->CR2 |= ADC_CR2_ADON;                                 // ADC3 ON00000000000
 //     ADC3->SQR3 = 0x0000000B;                                   // use PC_1, - ADC3_IN11
-//     GPIOC->MODER |= 0b1100001100;             //each channel   // PC_4, PC_1 are analog inputs
-    GPIOB->MODER |= 0x3;                                       // PB_0 as analog input
-
-    ADC1->SMPR2 |= 0x01000000;                                     // 15 cycles on CH_8,  0b0000000100000000<<16
-//     ADC1->SMPR1 |= 0x00001000;                                     // 15 cycles on CH_14, 0b0001000000000000
+     GPIOC->MODER |= 0b111100000000;             //each channel   // PC_4, PC_5 are analog inputs
+//    GPIOB->MODER |= 0x3;                                       // PB_0 as analog input
+//    ADC1->SMPR2 |= 0x01000000;                                     // 15 cycles on CH_8,  0b0000000100000000<<16
+//     ADC1->SMPR1 |= 0x00001000;                                     // 15 cycles on CH_14
+//     ADC1->SMPR1 |= 0x00008000;                                     // 15 cycles on CH_15
 //     ADC2->SMPR2 |= 0x01000000;                                     // 15 cycles on CH_8,  0b0000000100000000<<16
 //     ADC3->SMPR1 |= 0x00000008;                                     // 15 cycles on CH_11, 0b0000000000001000
+
+*/
+    
+  //////////////////////////////////////////////////ADC1////////////////////////////////////////////////  
+  ADC_ChannelConfTypeDef sConfig = {0};
+
+  hadc1.Instance = ADC1;
+  hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
+  hadc1.Init.Resolution = ADC_RESOLUTION_12B;
+  hadc1.Init.ScanConvMode = ENABLE;
+  hadc1.Init.ContinuousConvMode = DISABLE;
+  hadc1.Init.DiscontinuousConvMode = ENABLE;
+  hadc1.Init.NbrOfDiscConversion = 1;
+  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+  hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
+  hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
+  hadc1.Init.NbrOfConversion = 2;
+  hadc1.Init.DMAContinuousRequests = DISABLE;
+  hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  if (HAL_ADC_Init(&hadc1) != HAL_OK)
+  {
+//    Error_Handler();
+  }
+  sConfig.Channel = ADC_CHANNEL_14;
+  sConfig.Rank = 1;
+  sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
+  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+  {
+//    Error_Handler();
+  }
+  sConfig.Channel = ADC_CHANNEL_15;
+  sConfig.Rank = 2;
+  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+  {
+//    Error_Handler();
+  }
+  hadc1.Instance->CR2 |=  ADC_CR2_ADON;
 }
 
-
-
-
+void Init_ADC2(void)
+{
+  
+  //////////////////////////////////////////////////ADC2//////////////////////////////////////////////// 
+  ADC_ChannelConfTypeDef sConfig = {0};
+  
+  hadc2.Instance = ADC2;
+  hadc2.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
+  hadc2.Init.Resolution = ADC_RESOLUTION_12B;
+  hadc2.Init.ScanConvMode = ENABLE;
+  hadc2.Init.ContinuousConvMode = DISABLE;
+  hadc2.Init.DiscontinuousConvMode = ENABLE;
+  hadc2.Init.NbrOfDiscConversion = 1;
+  hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+  hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;
+  hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
+  hadc2.Init.NbrOfConversion = 2;
+  hadc2.Init.DMAContinuousRequests = DISABLE;
+  hadc2.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  if (HAL_ADC_Init(&hadc2) != HAL_OK)
+  {
+//    Error_Handler();
+  }
+  sConfig.Channel = ADC_CHANNEL_11;
+  sConfig.Rank = 1;
+  sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
+  if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
+  {
+//    Error_Handler();
+  }
+  sConfig.Channel = ADC_CHANNEL_9;
+  sConfig.Rank = 2;
+  if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
+  {
+//    Error_Handler();
+  }
+  hadc2.Instance->CR2 |=  ADC_CR2_ADON;
+}
 
-void Init_PWM()
+void Init_ADC3(void)
 {
+  //////////////////////////////////////////////////ADC3//////////////////////////////////////////////// 
+  ADC_ChannelConfTypeDef sConfig = {0};
+  
+  hadc3.Instance = ADC3;
+  hadc3.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
+  hadc3.Init.Resolution = ADC_RESOLUTION_12B;
+  hadc3.Init.ScanConvMode = DISABLE;
+  hadc3.Init.ContinuousConvMode = DISABLE;
+  hadc3.Init.DiscontinuousConvMode = DISABLE;
+  hadc3.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+  hadc3.Init.ExternalTrigConv = ADC_SOFTWARE_START;
+  hadc3.Init.DataAlign = ADC_DATAALIGN_RIGHT;
+  hadc3.Init.NbrOfConversion = 1;
+  hadc3.Init.DMAContinuousRequests = DISABLE;
+  hadc3.Init.EOCSelection = ADC_EOC_SEQ_CONV;
+  if (HAL_ADC_Init(&hadc3) != HAL_OK)
+  {
+//    Error_Handler();
+  }
+  sConfig.Channel = ADC_CHANNEL_1;
+  sConfig.Rank = 1;
+  sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
+  if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK)
+  {
+//    Error_Handler();
+  }
+  hadc3.Instance->CR2 |=  ADC_CR2_ADON;
+}
 
+void Init_TIM4()
+{
     RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;                         // enable TIM4 clock
-//    FastPWM pwm_v(PIN_V);
-//    FastPWM pwm_w(PIN_W);
-
-    //ISR Setup
-
-    NVIC_EnableIRQ(TIM4_IRQn);                         //Enable TIM4 IRQ
-
+//    FastPWM pwm_H3(PIN_H3);
+//    FastPWM pwm_L3(PIN_L3);
+    
+    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 = 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
     TIM4->EGR |= TIM_EGR_UG;
 
     //PWM Setup
-    TIM4->PSC = 0x0;                                            // no prescaler, timer counts up in sync with the peripheral clock
-//    TIM4->PSC = 10-1;                                            // no prescaler, timer counts up in sync with the peripheral clock
+//    TIM4->PSC = 0x0;                                           
+    TIM4->PSC = 10-1;                                            // 10 prescaler, timer counts up in sync with the peripheral clock
     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
-
-    TIM4->CCMR1 |= 0x7060;
-
+//    TIM4->CCMR1 |= 0x7060;
 }
 
-
-//void Init_TMR3()
-//{
-//    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;                         // enable TIM3 clock
-////    FastPWM pwm_v(PIN_V);
-////    FastPWM pwm_w(PIN_W);
-//    //ISR Setup
-//
-//    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 = 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->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_TMR3()
 {
     RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;                         // enable TIM3 clock
-//    FastPWM pwm_v(PIN_V);
-//    FastPWM pwm_w(PIN_W);
-    //ISR Setup
-
     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 = 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->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
-
-    TIM3->CCMR1 |= 0x7060;
 }
 
 void Init_TMR2()
@@ -152,7 +194,6 @@
     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 TIM2
     TIM2->EGR |= TIM_EGR_UG;
 
@@ -164,11 +205,12 @@
 
 void Init_TMR1()
 {
-
     RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;                         // enable TIM1 clock
-//    RCC->APB2ENR |= 0x02;                                        // enable TIM8 clock
-    FastPWM pwm_v(PIN_V);
-    FastPWM pwm_w(PIN_W);
+    
+    FastPWM pwm_H1(PIN_H1);
+    FastPWM pwm_L1(PIN_L1);
+    FastPWM pwm_H2(PIN_H2);
+    FastPWM pwm_L2(PIN_L2);
 
     TIM1->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
     TIM1->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
@@ -177,34 +219,16 @@
     TIM1->RCR |= 0x001;                                         // update event once per up/down count of TIM8
     TIM1->EGR |= TIM_EGR_UG;
 
+    TIM1->CCMR1 |= 0x60;                                        //CH1 - PWM mode 1
+    TIM1->CCMR1 |= 0x6000;                                      //CH2 - PWM mode 1
 
-    TIM1->CCMR1 |= 0x60;
-//    TIM1->CCMR1 |= TIM_CCMR1_OC1PE;
-//    TIM1->CCMR1 &= ~TIM_CCMR1_OC1FE;
-//    TIM1->CCMR1 |= TIM_CCMR1_OC2PE;
-//    TIM1->CCMR1 &= ~TIM_CCMR1_OC2FE;
-
-    TIM1->PSC = 0x00;                                            // no prescaler, timer counts up in sync with the peripheral clock
-    TIM1->ARR = TMR1_COUNT-1;                                          // set auto reload, 5 khz
-//    TIM1->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
+    TIM1->PSC = 0x00;                                           // no prescaler, timer counts up in sync with the peripheral clock
+    TIM1->ARR = TMR1_COUNT-1;                                   // set auto reload, 20 khz
     TIM1->CCER |= 0x05;                                         // CC1E = 1, CC1P = 0, CC1NE = 1, CC1NP = 0
+    TIM1->CCER |= 0x50;                                         // CC2E = 1, CC2P = 0, CC2NE = 1, CC2NP = 0
     
     TIM1->BDTR |= 0x8000;                                       // MOE = 1;
     TIM1->BDTR |= 0x7F;                                         // Dead-time 7Ftick
     
-    TIM1->CR1 |= TIM_CR1_CEN;                                   // enable TIM8
-
-
-    
-//    __HAL_RCC_GPIOA_CLK_ENABLE();
-//
-//    GPIO_InitTypeDef GPIO_InitStruct = {0};
-//
-//    GPIO_InitStruct.Pin = GPIO_PIN_7|GPIO_PIN_6;
-//    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-//    GPIO_InitStruct.Pull = GPIO_NOPULL;
-//    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-//    GPIO_InitStruct.Alternate = GPIO_AF3_TIM1;
-//    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
-
+    TIM1->CR1 |= TIM_CR1_CEN;                                   // enable TIM1
 }
\ No newline at end of file
--- a/INIT_HW/INIT_HW.h	Mon Jun 13 08:48:55 2022 +0000
+++ b/INIT_HW/INIT_HW.h	Fri Jun 17 06:06:53 2022 +0000
@@ -4,11 +4,12 @@
 #include "mbed.h"
 #include "FastPWM.h"
 
-void Init_ADC(void);
-void Init_PWM();
+void Init_ADC1(void);
+void Init_ADC2(void);
+void Init_ADC3(void);
+void Init_TMR1();
+void Init_TMR2();
 void Init_TMR3();
-void Init_TMR2();
-void Init_TMR1();
-//void Init_TMR5();
+void Init_TIM4();
 
 #endif
--- a/function_utilities/function_utilities.cpp	Mon Jun 13 08:48:55 2022 +0000
+++ b/function_utilities/function_utilities.cpp	Fri Jun 17 06:06:53 2022 +0000
@@ -21,7 +21,7 @@
 uint8_t FLAG_VALVE_DEADZONE = 0;
 uint8_t REFERENCE_MODE = 1;
 int16_t CAN_FREQ = 500;
-int16_t DIR_JOINT_ENC = 0;
+int16_t DIR_JOINT_ENC = 1;
 int16_t DIR_VALVE = 0;
 int16_t DIR_VALVE_ENC = 0;
 
@@ -46,21 +46,10 @@
 
 int16_t flag_delay_test = 0;
 
-//float P_GAIN_VALVE_POSITION_OPP = 0.0f;
-//float I_GAIN_VALVE_POSITION_OPP= 0.0f;
-//float D_GAIN_VALVE_POSITION_OPP= 0.0f;
-//float P_GAIN_JOINT_POSITION_OPP = 0.0f;
-//float I_GAIN_JOINT_POSITION_OPP = 0.0f;
-//float D_GAIN_JOINT_POSITION_OPP = 0.0f;
-//float P_GAIN_JOINT_TORQUE_OPP = 0.0f;
-//float I_GAIN_JOINT_TORQUE_OPP = 0.0;
-//float D_GAIN_JOINT_TORQUE_OPP = 0.0;
-
 int16_t VALVE_DEADZONE_PLUS;
 int16_t VALVE_DEADZONE_MINUS;
 
 int16_t VELOCITY_COMP_GAIN;
-//int16_t COMPLIANCE_GAIN;
 
 int16_t VALVE_CENTER;
 
@@ -94,9 +83,6 @@
 float V_adapt = 0.0000053f;           // (1256.6f + 236.4f * 39.75f) * 0.000000001f / 2
 float x_4_des_old = 0.0f;
 
-//int16_t VALVE_LIMIT_PLUS;
-//int16_t VALVE_LIMIT_MINUS;
-
 float ENC_PULSE_PER_POSITION = 1.0f;
 float TORQUE_SENSOR_PULSE_PER_TORQUE = 1.0f;
 float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
@@ -348,10 +334,7 @@
 float K_LPF = 0.0f;
 float D_LPF = 0.0f;
 
-float torq_sen_past = 0.0f;
-float torq_ref_past = 0.0f;
-float output_normalized = 0.0f;
-
+float V_EXI = 0.0f;
 
 /*******************************************************************************
  * General math functions
--- a/main.cpp	Mon Jun 13 08:48:55 2022 +0000
+++ b/main.cpp	Fri Jun 17 06:06:53 2022 +0000
@@ -21,18 +21,18 @@
 AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f
 
 // ADC ///////////////////////////////////////////
-//AnalogIn adc1(PC_4); //pressure_1
-//AnalogIn adc2(PC_5); //pressure_2
-//AnalogIn adc3(PC_1); //current
-//AnalogIn adc4(PB_0); //LVDT
+ADC_HandleTypeDef hadc1;
+ADC_HandleTypeDef hadc2;
+ADC_HandleTypeDef hadc3;
+
+AnalogIn adc1(PC_4); //pressure_1
+AnalogIn adc2(PC_5); //pressure_2
+AnalogIn adc3(PC_1); //current
+AnalogIn adc4(PB_1); //V_EXI
+AnalogIn adc5(PA_1); //LVDT
 
 // PWM ///////////////////////////////////////////
-float dtc_v=0.0f;
-float dtc_w=0.0f;
-
-// LVDT ///////////////////////////////////////////
-//DigitalOut LVDT_H(PB_6);
-//DigitalOut LVDT_L(PB_7);
+float PWM_duty = 0.0f;
 
 // SPI ///////////////////////////////////////////
 SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
@@ -43,6 +43,13 @@
 // LED ///////////////////////////////////////////
 DigitalOut LED(PA_15);
 
+// LVDT ///////////////////////////////////////////
+DigitalOut LVDT_H(PB_6);
+DigitalOut LVDT_L(PB_7);
+
+// MOTOR_ENA ///////////////////////////////////////////
+DigitalOut M_ENABLE(PA_2);
+
 // CAN ///////////////////////////////////////////
 CAN can(PB_8, PB_9, 1000000);
 CANMessage msg;
@@ -159,13 +166,9 @@
     if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
         //Error_Handler();
     }
-    /** Activate the Over-Drive mode
-    */
     if (HAL_PWREx_EnableOverDrive() != HAL_OK) {
         //Error_Handler();
     }
-    /** Initializes the CPU, AHB and APB busses clocks
-    */
     RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                                   |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
     RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
@@ -178,7 +181,7 @@
 
 int main()
 {
-    
+
     HAL_Init();
     SystemClock_Config();
 
@@ -191,7 +194,7 @@
     eeprom_cs = 0;
     make_delay();
 
-    enc_cs = 1;     
+    enc_cs = 1;
     enc.format(8,0);
     enc.frequency(5000000); //10M
     enc_cs = 0;
@@ -211,13 +214,20 @@
     ROM_CALL_DATA();
     make_delay();
 
+
+
     // ADC init
-    Init_ADC();
-    make_delay();
+    RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;                        // clock for ADC3
+    RCC->APB2ENR |= RCC_APB2ENR_ADC2EN;                        // clock for ADC2
+    RCC->APB2ENR |= RCC_APB2ENR_ADC3EN;                        // clock for ADC1
 
-    // Pwm init
-    Init_PWM();
-    TIM4->CR1 ^= TIM_CR1_UDIS;
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;                        // Enable clock for GPIOC
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;                        // Enable clock for GPIOB
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;                        // Enable clock for GPIOB
+
+    Init_ADC1();
+    Init_ADC2();
+    Init_ADC3();
     make_delay();
 
     // CAN
@@ -229,38 +239,31 @@
     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();
-
-    // TMR2 init
-    Init_TMR2();
-    TIM2->CR1 ^= TIM_CR1_UDIS;
-    make_delay();
-
-    // TMR1 init
+    // TMR1 init (PWM)
     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);
+    // TMR2 init (Control)
+    Init_TMR2();
+    TIM2->CR1 ^= TIM_CR1_UDIS;
+    make_delay();
+
+    // TMR3 init (Sensors)
+    Init_TMR3();
+    TIM3->CR1 ^= TIM_CR1_UDIS;
+    make_delay();
 
-//    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);
+    // TIM4 init (LVDT)
+    Init_TIM4();
+    TIM4->CR1 ^= TIM_CR1_UDIS;
+    make_delay();
 
+    //Timer priority
+    NVIC_SetPriority(TIM3_IRQn, 3);
+    NVIC_SetPriority(TIM2_IRQn, 4);
+    NVIC_SetPriority(TIM4_IRQn, 2);
+    HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 14, 0);
 
     //DAC init
     if (SENSING_MODE == 0) {
@@ -275,6 +278,10 @@
 //            dac_2 = PRES_A_VREF / 3.3f;
 //        }
     }
+
+    dac_1 = 1.0f/3.3f;
+    dac_2 = 1.0f/3.3f;
+
     make_delay();
 
     for (int i=0; i<50; i++) {
@@ -284,16 +291,15 @@
             ID_index_array[i] =  (i+1) * 0.5f;
     }
 
+    M_ENABLE = 1;
+
     /************************************
     ***     Program is operating!
     *************************************/
     while(1) {
-        
-//            if (LED > 0) LED = 0;
-//            else LED = 1;
-        TIM1->CCR1 = (TMR1_COUNT)*(0.7f);
 
-
+//        if (LED > 0) LED = 0;
+//        else LED = 1;
     }
 }
 
@@ -450,64 +456,78 @@
 
 
 //------------------------------------------------
-//     TMR3 : LVDT 1kHz
-//-----------------------------------------------
+//     TMR3 : PWM, Sensor 20kHz
+//------------------------------------------------
 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 FREQ_TMR3 = (float)FREQ_20k;
+long  CNT_TMR3 = 0;
+float DT_TMR3 = (float)DT_20k;
 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_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;
+        if (LED > 0) LED = 0;
+        else LED = 1;
+
+        float PSEN1 = 0.0f;
+        float PSEN2 = 0.0f;
+        float CURRENT_SEN = 0.0f;
+
+        /////////////////////////Current////////////////////////////////////////////////////////////////////////////
+        HAL_ADC_Start(&hadc2);
+        HAL_ADC_PollForConversion(&hadc2, 1);
+        CURRENT_SEN = (float) HAL_ADC_GetValue(&hadc2);
+        cur.UpdateSen(((float)CURRENT_SEN-2047.5f)/2047.5f*10.0f, FREQ_TMR3, 500.0f); // unit : mA
+
+        /////////////////////////V_EXI////////////////////////////////////////////////////////////////////////////
+        HAL_ADC_Start(&hadc2);
+        HAL_ADC_PollForConversion(&hadc2, 1);
+        V_EXI = (float) HAL_ADC_GetValue(&hadc2);
+
+        /////////////////////////Encoder////////////////////////////////////////////////////////////////////////////
+//        if (CNT_TMR1 % 2) == 0) {
+        ENC_UPDATE();
 //        }
-//
-////        LED = 0;
-//
-//        LVDT_H = 0;
-//        LVDT_L = 1;
-
 
-
+        /////////////////////////Force or Pressure//////////////////////////////////////////////////////////////////
+        if (SENSING_MODE == 0) {  // Force sensing
+        
+            HAL_ADC_Start(&hadc1);
+            HAL_ADC_PollForConversion(&hadc1, 1);
+            PSEN1 = (float) HAL_ADC_GetValue(&hadc1);
+            force.UpdateSen((((float)PSEN1) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR3, 100.0f); // unit : N
 
-//        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;
+        } else if (SENSING_MODE == 1) { // Pressure sensing
 
-//        TIM4->CCR2 = (PWM_ARR)*(0.95f);
-//        TIM4->CCR1 = (PWM_ARR)*(0.975f);
+            HAL_ADC_Start(&hadc1);
+            HAL_ADC_PollForConversion(&hadc1, 1);
+            PSEN1 = (float) HAL_ADC_GetValue(&hadc1);
 
-//        TIM8->CCR1 = (TMR8_COUNT)*(0.95f);
-//        TIM8->CCR2 = (TMR8_COUNT)*(0.975f);
+            HAL_ADC_Start(&hadc1);
+            HAL_ADC_PollForConversion(&hadc1, 1);
+            PSEN2 = (float) HAL_ADC_GetValue(&hadc1);
 
+            float pres_A_new, pres_B_new;
+            pres_A_new = (((float)PSEN1) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
+            pres_B_new = (((float)PSEN2) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
 
-//        PWM_H2 = 0;
-//        PWM_L2 = 1;
+            pres_A.UpdateSen(pres_A_new,FREQ_TMR3,200.0f);
+            pres_B.UpdateSen(pres_B_new,FREQ_TMR3,200.0f);
 
-//        TIM3->CCR2 = (TMR3_COUNT)*(0.95f);
-//        TIM3->CCR1 = (TMR3_COUNT)*(0.975f);
-
-
+            if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+                float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
+                torq.UpdateSen(torq_new,FREQ_TMR3,1000.0f);  // unit : Nm
+            } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+                float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
+                force.UpdateSen(force_new,FREQ_TMR3,1000.0f);  // unit : N
+            }
+        }
+        CNT_TMR3++;
     }
     TIM3->SR = 0x0;  // reset the status register
 }
@@ -515,8 +535,8 @@
 
 
 //------------------------------------------------
-//     TMR4 : Sensor Read & Data Handling
-//-----------------------------------------------
+//     TMR4 : LVDT 1kHz
+//------------------------------------------------
 float FREQ_TMR4 = (float)FREQ_20k;
 float DT_TMR4 = (float)DT_20k;
 long  CNT_TMR4 = 0;
@@ -525,123 +545,59 @@
 {
     if (TIM4->SR & TIM_SR_UIF ) {
 
-        // Current ===================================================
-        //ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
+        float LVDT_OUT = 0.0f;
+        LVDT_sum = 0.0f;
+
+        LVDT_L = 0;
+        LVDT_H = 1;
 
-        cur.UpdateSen(((float)ADC3->DR-2047.5f)/2047.5f*10.0f, FREQ_TMR4, 500.0f); // unit : mA
+        for (int ij = 0; ij<150; ij++) {
 
-        // Encoder ===================================================
-        if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) {
-            ENC_UPDATE();
+            ADC3->CR2  |= 0x40000000;
+            LVDT_new = ((float)ADC3->DR) - 2047.5f;
+            LVDT_sum = LVDT_sum + LVDT_new;
         }
 
-        /*
-        // 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;
-        //            }
-            pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f);
-            pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f);
+        LVDT_H = 0;
+        LVDT_L = 1;
 
-            if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
-                float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
-                torq.UpdateSen(torq_new,FREQ_TMR4,1000.0f);  // unit : Nm
-            } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
-                float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
-                force.UpdateSen(force_new,FREQ_TMR4,1000.0f);  // unit : N
-            }
-        }
-        */
+        LVDT_new = LVDT_sum * 0.01f*2.0f;
 
-//        //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;
-//        }
-
-
-
-
+        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;
 
         CNT_TMR4++;
     }
     TIM4->SR = 0x0;  // reset the status register
 }
 
-
-int j =0;
-float FREQ_TMR5 = (float)FREQ_5k;
-float DT_TMR5 = (float)DT_5k;
+//------------------------------------------------
+//     TMR2 : Control 5kHz
+//------------------------------------------------
+float FREQ_TMR2 = (float)FREQ_5k;
+float DT_TMR2 = (float)DT_5k;
 int cnt_trans = 0;
-int can_rest =0;
-float force_ref_act_can = 0.0f;
-
 extern "C" void TIM2_IRQHandler(void)
 {
     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_TMR5 /3.0f))/2.0f;
+            alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f;
             cnt_trans++;
             torq.err_int = 0.0f;
             force.err_int = 0.0f;
-            if((float)cnt_trans * DT_TMR5 > 3.0f)
+            if((float)cnt_trans * DT_TMR2 > 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_TMR5 /3.0f))/2.0f;
+            alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f;
             cnt_trans++;
             torq.err_int = 0.0f;
             force.err_int = 0.0f;
-            if((float) cnt_trans * DT_TMR5 > 3.0f )
+            if((float) cnt_trans * DT_TMR2 > 3.0f )
                 MODE_POS_FT_TRANS = 0;
         } else if(MODE_POS_FT_TRANS == 2) {
             alpha_trans = 1.0f;
@@ -1131,7 +1087,7 @@
                 VALVE_POS_CONTROL(valve_pos_raw.ref);
 
                 ref_array[cnt_step_test] = valve_pos_ref;
-                if(valve_pos.sen >= (float) 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*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
@@ -1156,7 +1112,7 @@
             }
 
             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);
+                float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR2);
                 if(valve_pos_ref >= 0) {
                     valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 } else {
@@ -1167,7 +1123,7 @@
 
                 ref_array[cnt_freq_test] = valve_pos_ref;
 //                if(value>=(float) VALVE_ELECTRIC_CENTER) {
-                if(valve_pos.sen>=(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 {
@@ -1177,7 +1133,7 @@
 
                 CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
                 cnt_freq_test++;
-                if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR5 > 2) {
+                if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR2 > 2) {
                     buffer_data_size = cnt_freq_test;
                     cnt_freq_test = 0;
                     cnt_send_buffer = 0;
@@ -1259,7 +1215,6 @@
 //                    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_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
@@ -1296,9 +1251,6 @@
                     }
                 }
 
-
-
-
                 if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
 
                     float I_MAX = 10.0f; // Maximum Current : 10mA
@@ -1464,31 +1416,29 @@
         else if (PWM_out < -1.0f) PWM_out=-1.0f;
 
         if (PWM_out>0.0f) {
-            dtc_v=0.0f;
-            dtc_w=PWM_out;
+            TIM1->CCR1 = (TMR1_COUNT)*(PWM_out);
+            TIM1->CCR2 = (TMR1_COUNT)*(0.0f);
         } else {
-            dtc_v=-PWM_out;
-            dtc_w=0.0f;
+            TIM1->CCR1 = (TMR1_COUNT)*(0.0f);
+            TIM1->CCR2 = 0.0f -(TMR1_COUNT)*(PWM_out);
         }
 
-
         ////////////////////////////////////////////////////////////////////////////
         //////////////////////  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/500) == 0) {
-
             // Position, Velocity, and Torque (ID:1200)
-            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));
-
-                } 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));
-                }
+            if (flag_data_request[0] == LOW) {
+                CAN_TX_POSITION_FT((int16_t) (cur.sen*1000.0f), (int16_t) (V_EXI), (int16_t) (valve_pos.sen));
+//                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));
+//                }
             }
 
             // Valve Position (ID:1300)
@@ -1508,7 +1458,6 @@
             TMR2_COUNT_CAN_TX = 0;
         }
         TMR2_COUNT_CAN_TX++;
-
     }
     TIM2->SR = 0x0;  // reset the status register
 }
--- a/setting.h	Mon Jun 13 08:48:55 2022 +0000
+++ b/setting.h	Fri Jun 17 06:06:53 2022 +0000
@@ -2,18 +2,17 @@
 #include "FastPWM.h"
 
 // pwm 
-//#define PIN_V       PB_4
-//#define PIN_W       PB_5
-//#define PIN_V      PC_6
-//#define PIN_W      PC_7
-#define PIN_V      PA_8
-#define PIN_W      PA_7
-//#define PIN_V       PB_6
-//#define PIN_W       PB_7
+#define PIN_H1      PA_8
+#define PIN_L1      PA_7
+#define PIN_H2      PA_9
+#define PIN_L2      PB_0
+//#define PIN_H3      PB_6
+//#define PIN_L3      PB_7
+
 //#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     0x2328      // loop 10k, pwm 5k
+//#define PWM_ARR     0x1194      // loop 20k, pwm 10k 
+#define PWM_ARR     0x2328      // loop 10k, pwm 5k
 //#define PWM_ARR     0xAFC8        // loop 2 k, pwm 1k
 
 //#define TMR3_COUNT  0xAFC8      // loop 2 k, pwm 1k
@@ -42,8 +41,7 @@
 extern AnalogOut dac_1;
 extern AnalogOut dac_2;
 
-extern float dtc_v;
-extern float dtc_w;
+extern float PWM_duty;
 
 // SPI
 extern SPI eeprom; //(SPI_MOSI, SPI_MISO, SPI_SCK);
@@ -58,6 +56,7 @@
 extern CAN can;
 extern CANMessage msg;
 
+
 // Board Information 
 
 
@@ -445,9 +444,7 @@
 extern float K_LPF;
 extern float D_LPF;
 
-extern float torq_sen_past;
-extern float torq_ref_past;
-extern float output_normalized;
+extern float V_EXI;
 
 
 
@@ -457,6 +454,3 @@
 
 
 
-
-
-