#define COM_BEGIN                   ((uint8_t)252)       //0xFC    //Loop Begin

#define COM_END                     ((uint8_t)253)       //0xFD    //Loop End

#define COM_EXIT                    ((uint8_t)255)       //0xFF    //Exit

#define COM_WAIT                    ((uint8_t)255)       //0xFF    //Wait

#define LED_PutData(s)              UART_PutData(LED_PORT, (uint8_t *)(s), LED_PACKET_SIZE)

uint8_t *LED_Stream[] = { (uint8_t*)LED_Stream0, (uint8_t*)LED_Stream1, (uint8_t*)LED_Stream2, (uint8_t*)LED_Stream3, (uint8_t*)LED_Stream4,

                          (uint8_t*)LED_Stream5, (uint8_t*)LED_Stream6, (uint8_t*)LED_Stream7, (uint8_t*)LED_Stream8, (uint8_t*)LED_Stream9

#define   LED_Stream(i, j)           (*(LED_Stream_+(i)*(LED_PACKET_SIZE+1)+(j)))

void LED_Send(uint8_t n, uint8_t red, uint8_t green, uint8_t blue, uint8_t red2, uint8_t green2, uint8_t blue2)

  uint8_t buffer[LED_PACKET_SIZE] = {n, red, green, blue, red2, green2, blue2};

volatile int8_t    LED_Loop_Index = -1;                  //0~(MAX_NESTED_LOOP-1)

          if(++LED_Loop_Index >= MAX_NESTED_LOOP) {          // ERROR if over MAX_NESTED_LOOP.

          LED_Loop_Count[LED_Loop_Index] = LED_Stream(LED_Index,2);

          DEBUG_printf("LED_Loop_Begin[0]%2d [1]%2d\r\n", LED_Loop_Begin[0], LED_Loop_Begin[1]);

          DEBUG_printf("LED_Loop_Count[0]%2d [1]%2d\r\n", LED_Loop_Count[0], LED_Loop_Count[1]);

          DEBUG_printf("LED_Loop_Index:%2d   LED_Index:%2d\r\n", LED_Loop_Index, LED_Index);

          if(LED_Loop_Index<0) {                             // ERROR if under 0.

            DEBUG_printf("\r\n[%d]COM_END-Repeat\r\n", LED_Index);

            if(LED_Loop_Count[LED_Loop_Index]!=0xFF)      LED_Loop_Count[LED_Loop_Index]--;

            LED_Index = LED_Loop_Begin[LED_Loop_Index];       // Jump

            DEBUG_printf("LED_Loop_Begin[0]%2d [1]%2d\r\n", LED_Loop_Begin[0], LED_Loop_Begin[1]);

            DEBUG_printf("LED_Loop_Count[0]%2d [1]%2d\r\n", LED_Loop_Count[0], LED_Loop_Count[1]);

            DEBUG_printf("LED_Loop_Index:%2d   LED_Index:%2d\r\n", LED_Loop_Index, LED_Index);

            DEBUG_printf("\r\n[%d]COM_END-Next\r\n", LED_Index);

          DEBUG_printf("[%d]Out Right Inc_DecC\r\n", LED_Index);

            else            LED_Buffer[com][i] += Inc_Dec[d-D10];

int check_LvDisplay1,check_LvDisplay2,check_LvDisplay3;   // HSW (24.02.09)

void LED_Display_Level(int8_t  mode, int16_t position_left, int16_t position_right, uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2)

    else                                        n = (position-POSITION_MIN)*LED_NUMBER/(POSITION_DIFF+1);

      if(mode==LED_MODE_FULL_LEFT)              LED_Send_All(n,r1,g1,b1,r2,g2,b2);

      else                                      LED_Send_All(LED_NUMBER-n,r2,g2,b2,r1,g1,b1);

      if(position_left>=POSITION_MAX)           n = LED_HALF_NUMBER;

      else                                      n = (position_left-POSITION_MIN)*LED_HALF_NUMBER/(POSITION_DIFF+1);

        if(mode==LED_MODE_HALF_SIDE)            LED_Send_Left(n,r1,g1,b1,r2,g2,b2);

        else                                    LED_Send_Left(LED_HALF_NUMBER-n,r2,g2,b2,r1,g1,b1);

      if(position_right>=POSITION_MAX)          n = LED_HALF_NUMBER;

      else                                      n = (position_right-POSITION_MIN)*LED_HALF_NUMBER/(POSITION_DIFF+1);

//        if(flag!=0)                             delay_msec(2);    //  HSW

        if(mode==LED_MODE_HALF_SIDE)            LED_Send_Right(n,r1,g1,b1,r2,g2,b2);

        else                                    LED_Send_Right(LED_HALF_NUMBER-n,r2,g2,b2,r1,g1,b1);

    LED_Send_Wait(i,3,3,3);     //  향후엔 머신 통합 default color로 바꾸기 (파라미터 연동)

//        led_mgr.read_idx = (led_mgr.read_idx + 1) % LED_QUEUE_SIZE;

//  led_mgr.write_idx = (led_mgr.write_idx + 1) % LED_QUEUE_SIZE;

//  if(led_mgr.count > 0 && !led_mgr.busy && !is_Playing_LED_Task()) {

//      LED_Display_Level(LED_MODE_HALF_CENTER, WP_Gym.Position[R], WP_Gym.Position[L], WP_LEDCtrl.R, WP_LEDCtrl.G, WP_LEDCtrl.B, 3, 3, 3);

//    } else if(WP_Weight.Ctrl.OnOffScale[L] <= 0.1f && cnt_Off == 0){

  if(WP_LEDCtrl.MotionAutoWeight > 0) {           //  AutoOn 발동시 +N으로 설정하고 있음 at MotionAutoWeight() in WESPION_App.c

  } else if (WP_LEDCtrl.MotionAutoWeight < 0) {   //  AutoOff 발동시 -N으로 설정하고 있음 at MotionAutoWeight() in WESPION_App.c

    if(WP_Weight.Ctrl.OnOffScale[L] >= 1.0f){     // 완전히 On인 경우에만 위치에 수행

      LED_Display_Level(LED_MODE_HALF_CENTER, WP_Gym.Position[R], WP_Gym.Position[L], WP_LEDCtrl.R, WP_LEDCtrl.G, WP_LEDCtrl.B, 3, 3, 3);

      //    LED_Display_Level(LED_MODE_FULL_LEFT, WP_Gym.Position[R], WP_Gym.Position[L], LED_Weight_R, LED_Weight_G, LED_Weight_B, 3, 3, 3);

    } else if(WP_Weight.Ctrl.OnOffScale[L] <= 0.1f && cnt_Off == 0){

 * This software is licensed under terms that can be found in the LICENSE file

 * If no LICENSE file comes with this software, it is provided AS-IS.

/* Includes ------------------------------------------------------------------*/

  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;

  if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)

  if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)

  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)

  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)

  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)

  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)

  sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;

  if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)

  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;

  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)

  htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;

  if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)

  htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;

  if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK)

  if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)

  htim8.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;

  if (HAL_TIM_ConfigClockSource(&htim8, &sClockSourceConfig) != HAL_OK)

  if (HAL_TIMEx_MasterConfigSynchronization(&htim8, &sMasterConfig) != HAL_OK)

  if (HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)

  if (HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)

  if (HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)

  if (HAL_TIMEx_ConfigBreakDeadTime(&htim8, &sBreakDeadTimeConfig) != HAL_OK)

  htim9.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;

  if (HAL_TIM_PWM_ConfigChannel(&htim9, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)

  if (HAL_TIM_PWM_ConfigChannel(&htim9, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)

void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle)

    GPIO_InitStruct.Pin = GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12

    HAL_GPIO_DeInit(GPIOE, GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12

     * Uncomment the line below to disable the "TIM1_BRK_TIM9_IRQn" interrupt

     * Be aware, disabling shared interrupt may affect other IPs

void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* tim_encoderHandle)

//#define     TIM9_PERIOD   (36000-1)      // 수행시간 감소에 도움될까? (2/27)

  TIM9_PWM_Start_IT(TIM_CHANNEL_1);         //include TIM9_Start()

  LPF_HSW(WP_RegenR.Vdc_filtered, WP_RegenR.Vdc_ref, WP_RegenR.alpha_Vdc);

  LPF_HSW(WP_RegenR.Idc_filtered, WP_RegenR.Idc_ref, WP_RegenR.alpha_Idc);

//  LPF_HSW(WP_RegenR.Idc_filtered, WP_RegenR.Idc_raw, WP_RegenR.alpha_Idc);

//  방전 구간 튜닝 시, 아래 함수 수시로 초기화 필요 : 원래는 Initialize_RegenR()에서 1회 수행

  WP_RegenR.Vdc_margin_Inv = 1.0f / (WP_RegenR.Vdc_Limit - WP_RegenR.Vdc_Off);

  WP_RegenR.Idc_margin_Inv = 1.0f / (WP_RegenR.Idc_Charge_Limit - WP_RegenR.Idc_Off);

    WP_RegenR.diff_Vdc = WP_RegenR.Vdc_filtered - WP_RegenR.Vdc_Off;

    WP_RegenR.PWM_Vdc = (int)(WP_RegenR.diff_Vdc * WP_RegenR.Vdc_margin_Inv * WP_RegenR.RegenScale_Vdc * TIM9_PERIOD);

    WP_RegenR.diff_Idc = - WP_RegenR.Idc_Off + WP_RegenR.Idc_filtered;   // Idc_diff 음수라서 PWM에는 반전시켜줘야 함(25.03.14)

    WP_RegenR.PWM_Idc = (int)(WP_RegenR.diff_Idc * WP_RegenR.Idc_margin_Inv * WP_RegenR.RegenScale_Idc * TIM9_PERIOD);

  WP_RegenR.PWM = (WP_RegenR.PWM_Vdc > WP_RegenR.PWM_Idc) ? WP_RegenR.PWM_Vdc : WP_RegenR.PWM_Idc;

  // 6. 안전 클램프 적용 (0 이상, TIM9_PERIOD의 95% 이하 - 5%는 그냥 본능적 마진 -> 900)

    WP_Derating.P_diss[side] = WP_Derating.Iq_x10_cont[side] * WP_Derating.Iq_x10_cont[side];

      case ON:   // live expression에서 계속 바꿔가면서 해야해서 상수들도 수시 업데이트 필요

        WP_Derating.P_loss[side] = (WP_Derating.Iq_x10_Debug[side] * WP_Derating.Iq_x10_Debug[side]);

    deltaP = WP_Derating.P_loss[side] - WP_Derating.P_diss[side];

    //    방열 시에는 온도 하강이 더 느릴 수 있음을 임의로 단순하게 구현 -> 추후 실험적으로 고도화 필요

    WP_Derating.deltaT[side] = deltaP + WP_Derating.deltaT[side];

    if(WP_Derating.deltaT[side] < 0) WP_Derating.deltaT[side] = 0 ;     //  이론적으론 필요 없지만 자료형, 오버플로우 등 혹시 모를 이슈를 위해 굳이 음수로 가는 건 막아놓기

    if(WP_Derating.deltaT[side] > WP_Derating.deltaT_max) WP_Derating.deltaT[side] = WP_Derating.deltaT_max;

    WP_Derating.DeratingScale[side] = (int) (100.0f / WP_Derating.deltaT_max * (WP_Derating.deltaT_max - WP_Derating.deltaT[side])) ;

    if(WP_Derating.DeratingScale[side] < 0)  WP_Derating.DeratingScale[side] = 0;

    if(WP_Derating.DeratingScale[side] > 100)  WP_Derating.DeratingScale[side] = 100;

//    WP_Derating.Iq_Lim[side] = 0.1f * ((WP_Machine.MaxCurrent * 10 - WP_Derating.Iq_x10_cont[side]) * (WP_Derating.DeratingScale[side] * 0.01f) + WP_Derating.Iq_x10_cont[side] * 0.5f);

//    WP_Derating.Iq_Lim[side] = 0.1f * (WP_Machine.MaxCurrent*10 - (WP_Derating.DeratingScale[side] * 0.01f) * (WP_Machine.MaxCurrent*10 - WP_Derating.Iq_x10_cont[side]));

//    WP_Derating.Iq_Lim[side] = 0.1f * (WP_Derating.Iq_x10_cont[side] + (WP_Derating.DeratingScale[side] * 0.01f) * (WP_Machine.MaxCurrent*10 - WP_Derating.Iq_x10_cont[side]));

    WP_Derating.Iq_Lim[side] = 0.1f * (WP_Derating.Iq_x10_Lim_Min + (WP_Derating.DeratingScale[side] * 0.01f) * (WP_Machine.MaxCurrent*10 - WP_Derating.Iq_x10_Lim_Min));

        if(WP_Derating.Iq_x10_Debug[side] > WP_Derating.Iq_x10_cont[side]){

          WP_Derating.Iq_x10_Debug[side] = (int) (WP_Derating.Iq_Lim[side] * 10.0f);

    //  UART3_printf("TIM2: %lu\r\n", TIM2_GetCounter());           //32bit

    UART3_printf("TIM2: %d\r\n", (int16_t)TIM2_GetCounter());   //16bit

    UART3_printf("TIM4: %d\r\n", (int16_t)TIM4_GetCounter());   //16bit

    VER_SUB,                        //uint8_t   ///somebp 2024.02.07

//__attribute__((section(".FlashData")))      //  (25.05.03) RDP 적용을 위해 FlashData를 bin으로 추출하기 위함

  if(HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError)!=HAL_OK){

  * @brief  Set the specified data holding register value for DAC channel.

      if(HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, (uint32_t)flash_data++, *ram_data++)!=HAL_OK) {

  * This software is licensed under terms that can be found in the LICENSE file

  * If no LICENSE file comes with this software, it is provided AS-IS.

#define EEPROM_SPI_Select()   (HAL_GPIO_WritePin(EEPROM_SPI_CS_Port, EEPROM_SPI_CS_Pin, GPIO_PIN_RESET))

#define EEPROM_SPI_Deselect() {HAL_GPIO_WritePin(EEPROM_SPI_CS_Port, EEPROM_SPI_CS_Pin, GPIO_PIN_SET);Delay_msec(1);}

    GPIO_InitStruct.Pin = GPIO_PIN_11|GPIO_PIN_12|GPIO_PIN_13|GPIO_PIN_14;

    HAL_GPIO_DeInit(GPIOG, GPIO_PIN_11|GPIO_PIN_12|GPIO_PIN_13|GPIO_PIN_14);

void EEPROM_WritePage(uint16_t address, uint8_t data[], uint8_t size)

  static uint8_t send[3+EEPROM_PAGE_SIZE] = {EEPROM_COM_WRITE, 0, 0};

uint8_t EEPROM_ReadPage(uint16_t address, uint8_t data[], uint8_t size)

//  WP_Machine.SensorGearRatioInv = 1.0f / WP_Machine.SensorGearRatio;

//  WP_Machine.MaxWeight = 10.0f;            // [kg]인데 현재는 A, 테스트 안전을 위해 막아둠

//  WP_Machine.Scale_Weight2Current = 0.5f;  // [kg/A] -> C샘플, BS-105-10 기준 약 2

//  WP_Machine.Scale_Current2Weight = 1.0f / WP_Machine.Scale_Weight2Current;

//  WP_Machine.DefaultWeight = 0.6;       // [A] -> 나중엔 [kg]로 바꿔야 함

  WP_Machine.SensorGearRatio = RAM_Data.Machine_SensorGearRatio;                          ///MACHINE_SENSOR_GEAR_RATIO

  WP_Machine.SensorGearRatioInv = 1.0f / WP_Machine.SensorGearRatio;

  WP_Machine.SpoolDiameter = RAM_Data.Machine_SpoolDiameter;                    // [mm]   ///MACHINE_SPOOL_DIAMETER

  WP_Machine.MaxWeight = RAM_Data.Machine_MaxWeight;                            // [kg]   ///MACHINE_MAX_WEIGHT

  WP_Machine.CableMaxLength = (uint16_t)RAM_Data.Machine_CableMaxLength;        // [mm]   ///MACHINE_CABLE_MAX_LENGTH

  WP_Machine.SoftWindingHeight = RAM_Data.Machine_SoftWindingHeight;            // [mm]   ///MACHINE_SOFT_WINDING_HEIGHT

  WP_Machine.Scale_Weight2Current = RAM_Data.Machine_Scale_Weight2Current;      // [kg/A] ///MACHINE_SCALE_WEIGHT2CURRENT

  WP_Machine.Scale_Current2Weight = 1.0f / WP_Machine.Scale_Weight2Current;

  WP_Machine.DefaultWeight = RAM_Data.Machine_DefaultWeight;                    // [A] -> 나중엔 [kg]로 바꿔야 함 ///MACHINE_DEFAULT_WEIGHT

  WP_Machine.FrictionWeight = RAM_Data.Machine_FrictionWeight;                  // [A] -> 0.7kg에 해당         ///MACHINE_FRICTION_WEIGHT

  WP_Machine.FrictionSpeedPlus = RAM_Data.Machine_FrictionSpeedPlus;                      ///MACHINE_FRICTION_SPEED_PLUS

  WP_Machine.FrictionSpeedMinus = RAM_Data.Machine_FrictionSpeedMinus;                    ///MACHINE_FRICTION_SPEED_MINUS

  WP_Gym.F_EccLimit = 0.5f * (WP_Machine.MaxCurrent * WP_Machine.Scale_Current2Weight);

  WP_Gym.Region.L_soft = 30;     //  가동범위 설정 시 불편함을 줄이기 위해  값을 줄여야 우리함.

  WP_Gym.Region.H_LoSoft[L] = WP_Gym.Region.H_shock;   // 초기값임 -> 가동범위 세팅시 갱신 필요!

  WP_Gym.Region.RangeLo[L] = WP_Gym.Region.H_LoSoft[L] + WP_Gym.Region.L_soft;                 // 초기값임 -> 가동범위 세팅시 갱신 필요!

  WP_Gym.Region.RangeLo[R] = WP_Gym.Region.H_LoSoft[L] + WP_Gym.Region.L_soft;            //  가동범위 위에서 무게가 가벼워지면, 방심하다가 조금 내려갈 때 무게가 증가하면서 위험해질 수 있으므로 일단 구현 xx

  WP_Gym.Region.RangeHi[L] = 1800.0f;             // 임의의 값임 -> 향후 수정 필요!!

  WP_Gym.Region.H_HiSoft[L] = WP_Gym.Region.RangeHi[L] + WP_Gym.Region.L_soft;

  WP_Gym.Region.H_HiSoft[R] = WP_Gym.Region.RangeHi[R] + WP_Gym.Region.L_soft;

  WP_Weight.Ecc_Step = 0.0010;     //  WeightController 주기(16÷2kHz) 당 변화량 -> 주기가 부정확 함.

  WP_Weight.Iso.Ki        = 1.0f;    // 적분 게인 — P가 즉각 잡고, I는 잔여 오차 처리

  WP_Weight.Iso.DecayRate = 20.0f;   // 하강 감쇠 속도 (kg/s), 20=50ms에 1kg

  WP_Weight.Iso.Fmin      = 0.0f;    // 최소 부하 (kg), 0=F_idle까지 풀림

  WP_Weight.Iso.Fmax      = 0.0f;    // 최대 부하 (kg), 0=TargetWeight 사용

  WP_Weight.Safety.SlewRate_kgPerSec = 50.0f;  // 50kg/s → 0→10kg에 200ms

  WP_Weight.Safety.IqLimit = 0.0f;             // 비활성 (MaxCurrent가 최종 보호)

  WP_Weight.Ctrl.MotionAutoActive = 0;         // 초기엔 MotionAutoOn 불가

  WP_Weight.Ctrl.OnOffStep = 0.00005;    // 적당한지 테스트 필요, 향후 스케쥴러로 변경!

  WP_Weight.FfScale.Step = 0.00016;   // 무게 단위 후처리 변동으로 2배처리함 (3/19)

  WP_RegenR.Vdc_Limit = 56.0f;    // 4/8 과충전(추정) 발생 이후로 타이트하게 수정 (기존 58)

//  WP_RegenR.alpha_Idc = 0.00001f;     // 25.02.25 최소한의 안정된 값을 갖기 위한 실험적 값

  WP_Derating.Iq_x10_cont[L] = 75;    // 7.5A 임의의 경험적 초기값 (3/22)

  WP_Derating.P_diss[L] = WP_Derating.Iq_x10_cont[L] * WP_Derating.Iq_x10_cont[L];

  WP_Derating.Iq_x10_Lim_Min = 50;    // Derating 최대 적용 전류 : 냉각에 영향

  WP_Derating.deltaT_max = 50000000;    // 일단 임의값 -> Iq^2과 시간 고려해야 함

/* Configure GPIO                                                             */

      {GPIO_MODE_OUTPUT_OD, GPIO_NOPULL},     //OUTPUT_OPENDRAIN

    return HAL_GPIO_ReadPin(GPIO_Pin[pin].port, GPIO_Pin[pin].pin);

    if(state<0)   HAL_GPIO_TogglePin(GPIO_Pin[pin].port, GPIO_Pin[pin].pin);

    else          HAL_GPIO_WritePin (GPIO_Pin[pin].port, GPIO_Pin[pin].pin, (state==0)?GPIO_PIN_RESET:GPIO_PIN_SET);

  HAL_GPIO_WritePin(GPIOF, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2, GPIO_PIN_RESET);

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)    // 엔코더 방식에서 Z펄스로 절대위치 초기화하는 함수!!

      WP_EncInit.Status[R] = Done;    //  (250703 추가 for Init_Encoder)

      WP_EncInit.Status[L] = Done;    //  (250703 추가 for Init_Encoder)

  if (HAL_DAC_ConfigChannel(&hdac, &sConfig, DAC_CHANNEL_1) != HAL_OK)

  if (HAL_DAC_ConfigChannel(&hdac, &sConfig, DAC_CHANNEL_2) != HAL_OK)

        UART3_printf("Main FW (Sector 1) erased successfully!\r\n");

            UART3_printf("RDP protection is active. Firmware is protected from reading.\r\n");

            UART3_printf("WARNING: RDP is not active! Firmware is not protected.\r\n");

//    volatile uint32_t *flash_cr = (uint32_t*)0x40023C10; // FLASH_CR 레지스터

//        UART3_printf("Main FW (Sector 1) erased successfully!\r\n");

//        g_debugger_detected = WSPN_SECURITY_IsDebuggerPresent();

  *          of all the requested memory to memory DMA transfers.

/* Configure DMA                                                              */

  /** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)

  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;

  if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK)

  /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.

  /** Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time

  sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES;

  sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING;

  sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_CC4;

  if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected) != HAL_OK)

  if (HAL_ADCEx_InjectedConfigChannel(&hadc2, &sConfigInjected) != HAL_OK)

  if (HAL_ADCEx_InjectedConfigChannel(&hadc3, &sConfigInjected) != HAL_OK)

    GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6

                          |GPIO_PIN_7|GPIO_PIN_8|GPIO_PIN_9|GPIO_PIN_10;

    hdma_adc3.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;

    * Uncomment the line below to disable the "ADC_IRQn" interrupt

    HAL_GPIO_DeInit(GPIOF, GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6

                          |GPIO_PIN_7|GPIO_PIN_8|GPIO_PIN_9|GPIO_PIN_10);

void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc)

    ADC_Inject[0][0]=HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);

    ADC_Inject[0][1]=HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);

    ADC_Inject[0][2]=HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_3);

    ADC_Inject[0][3]=HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_4);

    ADC_Inject[1][0]=HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);

    ADC_Inject[1][1]=HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_2);

    ADC_Inject[1][2]=HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_3);

    ADC_Inject[1][3]=HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_4);

    ADC_Inject[2][0]=HAL_ADCEx_InjectedGetValue(&hadc3, ADC_INJECTED_RANK_1);

    ADC_Inject[2][1]=HAL_ADCEx_InjectedGetValue(&hadc3, ADC_INJECTED_RANK_2);

    ADC_Inject[2][2]=HAL_ADCEx_InjectedGetValue(&hadc3, ADC_INJECTED_RANK_3);

    ADC_Inject[2][3]=HAL_ADCEx_InjectedGetValue(&hadc3, ADC_INJECTED_RANK_4);

    ConvAng2Pos();    // 이 타이밍도 모터 제어랑 더 붙일 필요가 있을지도? WeightManager, 정확하게는 WeightController (Region)랑도 밀접함.

      } else if(!Active_ForcedCalib) {    // 초기 부팅시 스킵할 경우. 250705 현재 이 방식.

        Debug_UART3_printf("Forced Calibration Inactive (Skip)\r\n");

//        LED_On_ALL(10, 11, 14);   //  이녀석이 웰컴라이트를 방해함 -> 일단 제외.

      if(cnt_LED_Calib_Err == 0 && WP_ForcedCali.Flag == Error) {  //  에러 LED

      if(MotorInitStart == 1 && WP_Ctrl.AdOffsetFlg == 1 && WP_Ctrl.Mode >= Inv_Ready && !Debug_SkipEncInit) {

      if(cnt_LED_EncInit_Err == 0 && WP_EncInit.Flag == Error) {  //  에러 LED

  WESPION_MotorControl();   // 이건 기본으로 실행해주고, 내부에서 상태 변수들로 구동 조건을 건드리는 게 적절 (250702)

      if(_isr_cycles > ISR_Profile.max) ISR_Profile.max = _isr_cycles;

      // 이동평균: avg = avg + (new - avg) / 16  — signed로 계산 (underflow 방지)

        ISR_Profile.avg = (uint32_t)((int32_t)ISR_Profile.avg + (((int32_t)_isr_cycles - (int32_t)ISR_Profile.avg) >> 4));

  //  Drive_Status == RunGym, Debug인 경우에만 수행하도록 위치 이동 (25.07.01)

//    ADC_Inject[1][0]=HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);

//    ADC_Inject[1][1]=HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_2);

//    ADC_Inject[1][2]=HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_3);

//    ADC_Inject[1][3]=HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_4);

//    ADC_Inject[0][0]=HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);

//    ADC_Inject[0][1]=HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);

//    ADC_Inject[0][2]=HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_3);

//    ADC_Inject[0][3]=HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_4);

//    ADC_Inject[2][0]=HAL_ADCEx_InjectedGetValue(&hadc3, ADC_INJECTED_RANK_1);

//    ADC_Inject[2][1]=HAL_ADCEx_InjectedGetValue(&hadc3, ADC_INJECTED_RANK_2);

//    ADC_Inject[2][2]=HAL_ADCEx_InjectedGetValue(&hadc3, ADC_INJECTED_RANK_3);

//    ADC_Inject[2][3]=HAL_ADCEx_InjectedGetValue(&hadc3, ADC_INJECTED_RANK_4);

      ntc_temp=-((ntc_adc-NTC_Value[i-1])/(NTC_Value[i-1]-NTC_Value[i])*5);

//UART3_printf("[%d] %d, NTC:%s'C\r\n", ch, ntc_adc, Float2String(ntc_temp, 3));

//#define     ADC_1A        ((0.04/3.3)*ADC_MAX)   //(0.04/3.3)*4095=49.6363..

#define     ADC_1A        (((ADC_GAIN_IDC*R_SHUNT_IDC)/3.3f)*ADC_MAX)

  float i = (ADC_Buffer[4] - ADC_OFFSET_IDC - ADC_MAX/2)/ADC_1A;

  float i = (ADC_Buffer[5] - ADC_OFFSET_I_CHG - ADC_MAX/2)/ADC_1A;

 *          Must update the version and date information in "flash.h"         *

 *     3.  F_xx들과 I_xx들을 실제 단위 kg, A에 맞게 바꿔야 하는 걸 임시로 했는데 완전히 처리된 지 모름

/* Private includes ----------------------------------------------------------*/

/* Private typedef -----------------------------------------------------------*/

/* Private define ------------------------------------------------------------*/

/* Private macro -------------------------------------------------------------*/

/* Private variables ---------------------------------------------------------*/

/* Private function prototypes -----------------------------------------------*/

/* Private user code ---------------------------------------------------------*/

  /* Configure the SysTick to have interrupt in 1ms time basis*/

  if (HAL_SYSTICK_Config(SystemCoreClock / (1000U / uwTickFreq)) > 0U)

WP_DriveStatus Drive_Status = EncInit; // 항상 EncInit -> Calibration -> RunGym으로 넘어가는 구조 - WESPION_Def.h 참고

uint8_t Debug_SkipEncInit = 0;  // [v1.0.3] 앱에서 EncInit 우회 (디버그용)

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */

  //System_Wakeup();                      //for Wake-up from Sleep mode     //not necessary

  ADC1_InjectedStart_IT();    // 중요 -> 전류 제어 관련 (전류 센싱, DC전압,위치), 3상 전류 하이 로우 6개 + 위치 2*2개 + DC전압 + 여분1개

  ADC3_InjectedStart_IT();    // 1,2와 달리 3은 인젝티트 모드가 아니라 폴링으로 도는 (덜 중요한) 채널이 같이 있어서, DMA로 도는... (이해 어려움)

  //  TIM1_PWM_Start_IT(TIM_CHANNEL_1);         //include TIM1_Start()

  //  TIM8_PWM_Start_IT(TIM_CHANNEL_1);         //include TIM8_Start()

//  ※※ 아래 구문이 시작되면서 핵심 수행 주기가 돌아감 !! - HAL_ADCEx_InjectedConvCpltCallback

  Flash_Init();                               //Backup or Restore

//  BLE_Rename(BLE_NAME);                       //MUST run only ONE time.

  LED_Send_All(60, 0,0,0, 0,0,0);             //Addressable LED turn OFF

  Delay_msec(110);                             //somebp 2024.02.08

//  LED_Send_All(60, 26, 13, 8, 10, 10, 10);   // 부팅 시 EncInit 단계를 시각화

    Debug_UART3_printf("\r\n---------- ---------- ----------\r\nRoomFit On!\r\n");

    Debug_UART3_printf("FW v%d.%d.%d (%04d.%02d.%02d)\r\n",WespionVer.VerMajer,WespionVer.VerMiner,WespionVer.VerSub,WespionVer.VerYear,WespionVer.VerMonth,WespionVer.VerDate);///somebp 2024.02.08

  ISR_Profile_Init();                    //  [v1.0.2] DWT Cycle Counter 활성화

  Task_Start_5ms(ISR_DeferredNotify);   //  [v1.0.2] ISR 지연 알림 처리 (UART/BLE 전송)

  //  [v1.0.4] Debug Report는 Task50ms()에서 100ms 주기로 호출 (스케줄러 슬롯 충돌 방지)

    //  Dac를 통해 수행 주기 체크 (Dac 세팅에 소요 시간을 최소화하기 위해 레지스터 직접 설정 방식 적용 by ChatGPT)

//    DAC->SWTRIGR |= DAC_SWTRIGR_SWTRIG1;  // 소프트웨어 트리거 (출력 반영)

    Regen_Resistor_Control_HSW();     // 수행시간 부족 유발로 수행 위치 이동 테스트 중 -> 스케쥴러 시도 (2/27)

    Protocol_Receive_Task_Uart3();    // BLE Protocol is more important than being in this location.... (24.02.09)

    /* 부팅 후 영점 조정이랑 부팅 후 첫 캘리를 통합하는 게 더 완벽하겠다 (너무 많이 풀려있으면 웰컴라이트가 먼저 끝나서 당길 수 있음)

    if(Active_ForcedCalib == 0 && InitialPosCali == 1 &&  Drive_Status == RunGym

        && Motor1_Ang.RpmFil < 5.0f && Motor2_Ang.RpmFil < 5.0f) {

    if(WP_ForcedCali.Flag == Done && cnt_LED_Welcome ==0 && status_LED_Welcome == 0) {

        Debug_UART3_printf("\r\n - Motor Ready, Let's move out! \r\n");

    //    Dac1 = (int32_t) WP_Gym.Speed[L] * 2047.0f / 330.0f + 2047;

    //    Dac2 = (int32_t) WP_Gym.Speed[R] * 2047.0f / 330.0f + 2047;

    Inverter_Comm();      // (25.05.05) 주요하지 않은 ADC 변수들 일괄로 불러오고 후처리 하는 코드로 보임

//        UART3_printf("[%d] %d, NTC:%s'C\t",   0, ADC_Buffer[2], Float2String(Get_Temperature(0), 3));

//        UART3_printf("[%d] %d, NTC:%s'C\r\n", 1, ADC_Buffer[3], Float2String(Get_Temperature(1), 3));

//        ATmacaddr[0] = UART_Getc_NoClear(&huart3,8);    // 리턴이 12자리이므로 9, 10, 11, 12번 째를 가져옴

//        UART_printf(&huart3, "AT+NAME%s%s", ATname, ATmacaddr);

//        ATmacaddr[0] = UART_Getc_NoClear(&huart3,8);    // 12자리이므로 9, 10, 11, 12번 째를 가져옴

//    LED는 진짜 제일 필요 없으니 맨 마지막. 배포 시엔 아예 끄는 게 지구에 도움될 지도 (250702)

  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters

  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)

  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)

 * @brief  This function is executed in case of error occurrence.

  /* User can add his own implementation to report the HAL error return state */

 * @brief  Reports the name of the source file and the source line number

  /* User can add his own implementation to report the file name and line number,

     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

////        UART_printf(&huart3, "%d tick=%d\r\n", MainCnt, tickDelta);

//        UART3_printf("%.1f, %.1f, tick=%dms\r\n", AngleMech, AngleMechAll, tickDelta);

  return_data[sizeof(return_data)-1]=-return_data[sizeof(return_data)-1];

void Return_2x2byte(uint8_t com, uint16_t data1, uint16_t data2)

void Return_2x1byte(uint8_t com, uint16_t data1, uint16_t data2)

void Return_4x1byte(uint8_t com, uint16_t data1, uint16_t data2, uint16_t data3, uint16_t data4)

  uint16_t report_cnt = (uint16_t)(Get_Time()/50);      //50msec

  //  Return_2x2byte(GET_POSITION, (uint16_t)WP_GymManager.CurrentPosition_L, (uint16_t)WP_GymManager.CurrentPosition_R);

  return_data[6]=(uint8_t) ((uint16_t)(WP_Gym.PositionFine[L]) >>8);    // PositionFine : 0.05mm 단위

  return_data[7]=(uint8_t) ((uint16_t) (WP_Gym.PositionFine[L]));

  return_data[8]=(uint8_t) ((uint16_t)(WP_Gym.PositionFine[R]) >>8);

  return_data[9]=(uint8_t) ((uint16_t) (WP_Gym.PositionFine[R]));

  //  return_data[6]=(uint8_t) ((uint16_t) (WP_Gym.Position[L] *0.1) >>8);    // 원래 mm 단위인데, App에 cm 단위로 보내주는 중

  //  return_data[8]=(uint8_t) ((uint16_t) (WP_Gym.Position[R] *0.1) >>8);

  return_data[10]=(uint8_t) ((uint16_t) (Motor1.Cmd.Icmd.q * WP_Machine.Scale_Current2Weight *100) >>8);

  return_data[11]=(uint8_t) (Motor1.Cmd.Icmd.q * WP_Machine.Scale_Current2Weight *100);

  return_data[12]=(uint8_t) ((uint16_t) (Motor2.Cmd.Icmd.q * WP_Machine.Scale_Current2Weight *100) >>8);

  return_data[13]=(uint8_t) (Motor2.Cmd.Icmd.q * WP_Machine.Scale_Current2Weight *100);

// ── Dev Report: v3 + Speed/Accel/Ifb/FLoad/Region/WeightSet (41바이트) ──

  return_data[14]=(uint8_t) ((uint16_t) (Inv.Ctrl.Vdc * 100) >>8);;

// (4/3) 아래는 언제적 기능인지 기억도 안남 -> void WeightBLEtoSetTask(float step)가 이 기능 수행 중

//void SetWeight_Start(uint8_t weight_L, uint8_t mode_L, uint8_t weight_R, uint8_t mode_R, uint16_t msec, uint16_t step);

//void SetWeight_Start(uint8_t weight_L, uint8_t mode_L, uint8_t weight_R, uint8_t mode_R, uint16_t msec, uint16_t step)

  Task_Start_50ms_1(Return_Debug_Report);   // Debug Report 20Hz (50ms_1 슬롯)

  if(__HAL_PWR_GET_FLAG(PWR_FLAG_SB) != RESET) { //when standby mode and press the button

  LED_Send_All(60, 0,0,0, 0,0,0);         //Addressable LED turn OFF while Sleeping

  //LED_Send_All(60, 1,0,0, 0,0,0);         //Addressable LED turn RED while Sleeping

  Motor1En(WP_Gate_DIS);                  //Disable Motor Gate Driver

  //digitalWrite(56, GPIO_PIN_RESET);       //On  while Sleeping

  HAL_PWR_EnterSLEEPMode(PWR_MAINREGULATOR_ON, PWR_SLEEPENTRY_WFI);

int check_GetPosition, check_Emergency, check_WeightPlus, check_WeightMinus;

          DEBUG_printf("\r\n[TIMEOUT] Buffer Clear:%d\r\n", available);

      if( UART3_Getc_NoClear(0)==0xFF && UART3_Getc_NoClear(1)==0xFF

          && UART3_Getc_NoClear(2)!=0xFF && UART3_Getc_NoClear(2)>0x01) {

        old_available=UART3_Available();                ///somebp 2024.02.19

        DEBUG_printf("\r\n1. Size:0x%02X\tChecksum:0x%02X\r\n", psize, checksum);

//      uint8_t pinst;    // 디버깅 편의를 위해 전역화 -> 상단 외부로 위치 이동 (25.05.01)

        DEBUG_printf("\r\nChecksum ERROR: 0x%02X\r\n", checksum);

        if(UART3_Getc_NoClear(0)!=0xFF)   UART3_Clear_Buffer(1);

          //          Return_2x2byte(GET_POSITION, 0x111, 0x222);

          //          Return_2x2byte(GET_POSITION, (uint16_t)Motor1_Ang.AngleMech, (uint16_t)Motor1_Ang.AngleMechAll);

          //          Return_2x2byte(GET_POSITION, (uint16_t)WP_GymManager.Position_L, (uint16_t)WP_GymManager.Position_R);

        case BLE_CONNECT:    // 모든 Gym setting (무게값, 모드, 가동 범위 등, 사용 시마다 설정하는 정보)을 초기화

          Return_4x1byte(WEIGHTMINUS, WP_Gym.WeightSet[L]*2, WP_Gym.WeightMode[L], WP_Gym.WeightSet[R]*2, WP_Gym.WeightMode[R]);

          Return_1byte(WEIGHTONOFF, WP_Weight.Ctrl.OnOffStatus[L]);

          //          if(WP_Gym.Position[L] > WP_Gym.Region.H_LoSoft[L] || WP_Gym.Position[R] > WP_Gym.Region.H_LoSoft[R]) {

          //            UART3_Puts("Err: Put down handles !! \r\n");

          //            WP_Gym.Region.H_LoSoft[L] = WP_Gym.Region.H_shock;   // 초기값임 -> 가동범위 세팅시 갱신 필요!

          //            WP_Gym.Region.H_LoSoft[R] = WP_Gym.Region.H_shock;

          //            WP_Gym.Region.RangeLo[L] = WP_Gym.Region.H_LoSoft[L] + WP_Gym.Region.L_soft;                 // 초기값임 -> 가동범위 세팅시 갱신 필요!

          //            WP_Gym.Region.RangeLo[R] = WP_Gym.Region.H_LoSoft[L] + WP_Gym.Region.L_soft;            //  가동범위 위에서 무게가 가벼워지면, 방심하다가 조금 내려갈 때 무게가 증가하면서 위험해질 수 있으므로 일단 구현 xx

          //            WP_Gym.Region.RangeHi[L] = 1800.0f;             // 임의의 값임 -> 향후 수정 필요!!

          //            WP_Gym.Region.H_HiSoft[L] = WP_Gym.Region.RangeHi[L] + WP_Gym.Region.L_soft;

          //            WP_Gym.Region.H_HiSoft[R] = WP_Gym.Region.RangeHi[R] + WP_Gym.Region.L_soft;

          UART3_printf("FW v%d.%d.%d R2 (%04d.%02d.%02d)\r\n",WespionVer.VerMajer,WespionVer.VerMiner,WespionVer.VerSub,WespionVer.VerYear,WespionVer.VerMonth,WespionVer.VerDate);///somebp 2024.02.08

          Return_4x1byte(CHECK_FWVER,WespionVer.VerMajer, WespionVer.VerMiner, WespionVer.VerSub, WespionVer.VerPre);

          UART3_printf("FW v%d.%d.%d-dev.%d (%04d.%02d.%02d)\r\n",WespionVer.VerMajer,WespionVer.VerMiner,WespionVer.VerSub,WespionVer.VerPre,WespionVer.VerYear,WespionVer.VerMonth,WespionVer.VerDate);

          ////          SetWeight(UART3_Getc_NoClear(1), UART3_Getc_NoClear(2), UART3_Getc_NoClear(3), UART3_Getc_NoClear(4));

          //            SetWeight_Start(UART3_Getc_NoClear(1), UART3_Getc_NoClear(2), UART3_Getc_NoClear(3), UART3_Getc_NoClear(4),UART3_Getc_NoClear(5),UART3_Getc_NoClear(6));

          //            Return_4x1byte(SET_WEIGHT, WP_Gym.WeightSet[L], WP_Gym.WeightMode[L], WP_Gym.WeightSet[R], WP_Gym.WeightMode[R]);

          if(UART3_Getc_NoClear(2) > 3 || UART3_Getc_NoClear(4) > 3 ) {

            if(UART3_Getc_NoClear(1) * 0.5 >= 60 || UART3_Getc_NoClear(1) * 0.5 < 0){    // 머신 제한 무게랑 연동해야 함!!

              WP_Gym.WeightBLE[L] = UART3_Getc_NoClear(1) * 0.5;    // 프로토콜 단위(0.5kg) 환산

            if(UART3_Getc_NoClear(3) * 0.5 >= 60 || UART3_Getc_NoClear(3) * 0.5 < 0){    // 머신 제한 무게랑 연동해야 함!!

              WP_Gym.WeightBLE[R] = UART3_Getc_NoClear(3) * 0.5;

          Return_4x1byte(SET_WEIGHT, WP_Gym.WeightSet[L]*2, WP_Gym.WeightMode[L], WP_Gym.WeightSet[R]*2, WP_Gym.WeightMode[R]);

        case SET_RANGE:   //  갑자기 구간을 바꿔버리면 무게가 확 변경되는 문제 해결해야 함! -> 스케쥴러?

          if(WP_Weight.Ctrl.OnOffScale[L] > 0.0f || WP_Weight.Ctrl.OnOffScale[R] > 0.0f) {

            //          if(WP_Gym.WeightSet[L] >= 5.5f || WP_Gym.WeightSet[R] >= 5.5f) {

                UART3_Puts(Float2String(WP_Gym.Region.RangeLo[L],0));

                UART3_Puts(Float2String(WP_Gym.Region.RangeHi[L],0));

                UART3_Puts(Float2String(WP_Gym.Region.RangeLo[R],0));

                UART3_Puts(Float2String(WP_Gym.Region.RangeHi[R],0));

            //  프로토콜은 0.05mm 단위(report task와 통일 목적) -> 근데 Range는 mm 단위로 다뤄짐 - WeightController()

            uint16_t position = (((uint16_t)UART3_Getc_NoClear(3) << 8) | UART3_Getc_NoClear(4))* 0.05f;

            SetRangeDigit(UART3_Getc_NoClear(1),UART3_Getc_NoClear(2), position);

          if(WP_Weight.Ctrl.OnOffStatus[L] == OFF && WP_Weight.Ctrl.OnOffStatus[R] == OFF) {

                Return_1byte(SET_RANGE_INIT,1);    // Not applied

        case EMERGENCY:                       //  지금은 단계적으로 변경하는 효과가 전혀 없음!! 스케쥴러로 돌려야 함!!

                Return_1byte(WEIGHTONOFF, WP_Weight.Ctrl.OnOffStatus[L]);

                      WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] + 0.5f;

                      if(WP_Gym.WeightBLE[L] >= WP_Machine.MaxCurrent * WP_Machine.Scale_Current2Weight){

                        WP_Gym.WeightBLE[L] = WP_Machine.MaxCurrent * WP_Machine.Scale_Current2Weight;

                      WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] + 5.0f;

                          WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] + 0.5f;

                          if(WP_Gym.WeightBLE[R] >= WP_Machine.MaxCurrent * WP_Machine.Scale_Current2Weight){

                            WP_Gym.WeightBLE[R] = WP_Machine.MaxCurrent * WP_Machine.Scale_Current2Weight;

                          WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] + 5.0f;

                              WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] + 0.5f;

                              WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] + 0.5f;

                              if(WP_Gym.WeightBLE[L] >= WP_Machine.MaxCurrent * WP_Machine.Scale_Current2Weight || WP_Gym.WeightBLE[R] >= WP_Machine.MaxCurrent * WP_Machine.Scale_Current2Weight){

                                WP_Gym.WeightBLE[L] = WP_Machine.MaxCurrent * WP_Machine.Scale_Current2Weight;

                                WP_Gym.WeightBLE[R] = WP_Machine.MaxCurrent * WP_Machine.Scale_Current2Weight;

                              WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] + 5.0f;

                              WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] + 5.0f;

                          WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 0.5f;

                          WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 5.0f;

                              WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 0.5f;

                              WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 5.0f;

                                  WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 0.5f;

                                  WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] - 0.5f;

                                  if(WP_Gym.WeightBLE[L] <= 0.0f || WP_Gym.WeightBLE[R] <= 0.0f){

                                    LED_Blink_ALL(150,10,10,50);

                                  Task_Start_10ms_1(Task10ms_1);

                                  WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 5.0f;

                                  WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] - 5.0f;

                    case MODE_CHANGE:   // 3/19 업데이트 : MODE_SET으로 명칭 바꿔야 할 듯?

                      if(UART3_Getc_NoClear(1) > 5){    // 0=Constant, 1=Negative, 2=Band, 3=Isokinetic, 4=Hydraulic, 5=Vibration

                        UART3_Puts("Err: Mode command number error\r\n");

                        if(!Debug_HilMode && (WP_Gym.Position[L] > WP_Gym.Region.H_LoSoft[L] || WP_Gym.Position[R] > WP_Gym.Region.H_LoSoft[R])) {

                          UART3_Puts("Err: Put down handles first\r\n");

                          WP_Gym.WeightMode[L] = UART3_Getc_NoClear(1);

                          WP_Gym.WeightMode[R] = UART3_Getc_NoClear(1);   // 일단 좌측 값을 우측에도 적용

                          //                        WP_Gym.WeightMode[R] = UART3_Getc_NoClear(2);

                          //                        if(WP_Gym.WeightMode[L] >= 2) {

                          //                          WP_Gym.WeightMode[L] = 0;

                          //                          WP_Gym.WeightMode[L]++;

                          //                        if(WP_Gym.WeightMode[R] >= 2) {

                          //                          WP_Gym.WeightMode[R] = 0;

                          //                          WP_Gym.WeightMode[R]++;

                              UART3_Puts("Mode 0 - Constant\r\n");

                              LED_Blink_ALL(WP_LEDCtrl.R, WP_LEDCtrl.G, WP_LEDCtrl.B,20);

                              UART3_Puts("Mode 1 - Eccentric: +");

                              UART3_Puts(Float2String(WP_Gym.F_EccSet*2,1));

                          UART3_Puts(Float2String(WP_Gym.WeightSet[L],2));

                          if(WP_Weight.Ctrl.OnOffStatus[L] == ON){

                        Return_4x1byte(MODE_CHANGE, WP_Gym.WeightMode[L], WP_Gym.WeightMode[R], WP_Gym.WeightModeError, WP_Gym.WeightModeError);

                        //                      Return_2x1byte(MODE_CHANGE, WP_Gym.WeightMode[L], WP_Gym.WeightMode[R]);

                      WP_Gym.F_EccSet_Temp[L] = UART3_Getc_NoClear(1) * 0.5f;  //  프로토콜 단위 환산 (정수1 = 0.5kg)

                      WP_Gym.F_EccSet_Temp[R] = UART3_Getc_NoClear(2) * 0.5f;

                      if(WP_Gym.F_EccSet_Temp[L] > WP_Gym.WeightSet[L] || WP_Gym.F_EccSet_Temp[R] > WP_Gym.WeightSet[R]){

                        UART3_Puts("Err: Over 100% is not allowed\r\n");

                      } else if(WP_Gym.F_EccSet_Temp[L] > WP_Gym.F_EccLimit || WP_Gym.F_EccSet_Temp[R] > WP_Gym.F_EccLimit){

                        UART3_Puts("Err: EccWeight is limited\r\n");

//                        WP_Gym.F_EccSet_Temp[L] = WP_Gym.F_Ecc_Percentage[L] * 0.01;

//                        WP_Gym.F_EccSet_Temp[R] = WP_Gym.F_Ecc_Percentage[R] * 0.01;   //  추후 좌우 구분시에 추가 (좌측도 같이 수정)

                        WP_Gym.F_EccSet = WP_Gym.F_EccSet_Temp[L];   // 임시 중복 코드 ㅠ : WeightController에는 이 공통 변수가 쓰이고 있어서..

//                      if (WP_Gym.F_EccSet > WP_Gym.F_EccLimit) {

//                        WP_Gym.F_EccSet = WP_Gym.F_EccSet + 0.5;

                      UART3_Puts(Float2String(WP_Gym.F_EccSet * 2,1));

                      Return_4x1byte(ECC_LEVEL, WP_Gym.F_EccSet_Temp[L] * 2, WP_Gym.F_EccSet_Temp[R] * 2, WP_Gym.F_Ecc_ErrorCode, 0);

//                      Return_1byte(ECC_LEVEL, EccSet20);   //  0.1kg 단위

                      WP_Weight.Ctrl.MotionAutoActive = UART3_Getc_NoClear(1);

                      Return_1byte(AUTOWEIGHT_ACTIVE, WP_Weight.Ctrl.MotionAutoActive);

                      UART3_Puts("User requested Forced Calibration \r\n");

                      Return_1byte(AUTOWEIGHT_ACTIVE, ReStart_ForcedCalib);

                          UART3_Puts("\r\n Debug Printing ON \r\n");

                          UART3_Puts("\r\n Debug Printing OFF \r\n");

                        uint16_t paddress=((uint16_t)UART3_Getc_NoClear(1)<<8)+(uint8_t)UART3_Getc_NoClear(2);

                        uint8_t data_size=UART3_Getc_NoClear(3);

                        UART3_printf(" paddress:0x%04X\tdata_size:%d\r\n", paddress, data_size);

                        if((paddress+data_size)>(sizeof(Data_Form_RAM)-4))  break;

                          *((uint8_t*)&RAM_Data+paddress+i)=UART3_Getc_NoClear(4+i);

                        MotorCtrl_Initialize();           ///Refresh Variables for Motor & !! Disable Gate Drivers !!

                        DEBUG_printf(" paddress:0x%04X\tdata_size:%d\r\n", paddress, data_size);

                        buffer[0] = 0xFF;             checksum  = 0xFF;

                        buffer[1] = 0xFF;             checksum += 0xFF;

                        buffer[2] = 2+data_size;      checksum += 2+data_size;

                        buffer[3] = GET_DATA;         checksum += GET_DATA;

                          buffer[4+i] = *((uint8_t*)&RAM_Data+paddress+i);

                      DEBUG_printf("\r\n Save Data to Flash\r\n");

                        Error_Flag_Enable = (UART3_Getc_NoClear(1)!=0);

                        if(Error_Flag_Enable)     Return_4byte(FAIL_SAFE_ERROR, Error_Flag);

                      MotorCtrl_Initialize();           ///Refresh Variables for Motor & !! Disable Gate Drivers !!

                      //        DEBUG_printf("\r\n System Standby\r\n");

                    case DEBUG_REPORT:                   //  [v1.0.2+] BLE 디버그 리포트 + 디버그 서브코맨드

                      if(psize >= 3) {                   //  FF FF 03 F5 [sub] [CHK] — 서브코맨드

                          case 0x01:                     //  F5 01: EncInit 스킵 + RunGym 강제 진입

                            Motor1En(WP_Gate_DIS);       //  게이트 OFF 유지 (안전)

                            WP_Weight.Ctrl.OnOffScale[L] = 1.0f;

                            WP_Weight.Ctrl.OnOffScale[R] = 1.0f;

                            Start_Report();              //  50ms Report 시작 (Debug Report도 여기서 호출됨)

                            Return_1byte(DEBUG_REPORT, 0x01);  //  ACK

                          case 0x02:                     //  F5 02: EncInit 복원 (원래 상태로)

                            Return_1byte(DEBUG_REPORT, 0x02);  //  ACK

                          case 0x03:                     //  F5 03: HIL Position+Speed 주입

                            //  FF FF 0B F5 03 posL_H posL_L posR_H posR_L spdL_H spdL_L spdR_H spdR_L CHK

                              int16_t injPosL = (int16_t)((UART3_Getc_NoClear(2) << 8) | UART3_Getc_NoClear(3));

                              int16_t injPosR = (int16_t)((UART3_Getc_NoClear(4) << 8) | UART3_Getc_NoClear(5));

                              WP_Gym.PositionFine[L] = injPosL;     // 0.05mm 단위

                              WP_Gym.PositionFine2[L] = injPosL / 2; // 0.1mm 단위 (PositionFine / 2)

                              WP_Gym.PositionFine2[R] = injPosR / 2;

                              // Speed: 확장 패킷이면 앱에서 직접 주입, 아니면 0 유지

                                int16_t injSpdL = (int16_t)((UART3_Getc_NoClear(6) << 8) | UART3_Getc_NoClear(7));

                                int16_t injSpdR = (int16_t)((UART3_Getc_NoClear(8) << 8) | UART3_Getc_NoClear(9));

                                WP_Gym.Speed[L] = (float)injSpdL;  // mm/s 단위 그대로

                                WP_Gym.Speed[R] = (float)injSpdR;

                          case 0x04:                     //  F5 04: HIL 모드 해제

                            Return_1byte(DEBUG_REPORT, 0x04);  //  ACK

                          case 0x05:                     //  F5 05: 무게 모드 파라미터 설정

                            uint8_t paramId = UART3_Getc_NoClear(2);

                            uint16_t val = (uint16_t)((UART3_Getc_NoClear(3) << 8) | UART3_Getc_NoClear(4));

                              case 0x01: WP_Weight.Hydro.Vmax = (float)val; break;  // v3.2: Vref→Vmax 통합 (하위호환)

                              case 0x02: WP_Weight.Hydro.Vmax = (float)val; break;

                              case 0x03: WP_Weight.Hydro.n = (float)val / 100.0f; break;

                              case 0x04: WP_Weight.Hydro.minRatio = (float)val / 100.0f; break;

                              case 0x10: WP_Weight.Iso.Vtarget = (float)val; break;

                              case 0x11: WP_Weight.Iso.Kp = (float)val / 100.0f; break;

                              case 0x12: WP_Weight.Iso.Ki = (float)val / 100.0f; break;

                              case 0x13: WP_Weight.Iso.DecayRate = (float)val / 10.0f; break; // ×10, kg/s (200→20.0)

                              case 0x14: WP_Weight.Iso.Fmin = (float)val / 10.0f; break;     // ×10, kg (30→3.0)

                              case 0x15: WP_Weight.Iso.Fmax = (float)val / 10.0f; break;     // ×10, kg (100→10.0), 0=TargetWeight

                              case 0x20: WP_Weight.Safety.SlewRate_kgPerSec = (float)val; break;

                              case 0x30: WP_Weight.Vib.Freq = (float)val / 100.0f; break;      // ×100

                              case 0x31: WP_Weight.Vib.AmplitudeKg = (float)val / 100.0f; break; // ×100, kg

                              case 0x32: WP_Weight.Vib.MaxRatio = (float)val / 100.0f; break;    // ×100, 비율

                              case 0x40: WP_Weight.Safety.IqLimit = (float)val / 100.0f; break;       // ×100, A 단위

                              case 0x50: Time_StopAutoOff = (int)val * 1000; break;    // val=0.1초 단위 (50→5초), ×1000(10kHz×0.1s)

                            Return_1byte(DEBUG_REPORT, 0x05);  //  ACK

                          case 0x10:                     //  F5 10: Debug Report 강제 ON

                            Return_1byte(DEBUG_REPORT, 0x10);  //  ACK

                          case 0x11:                     //  F5 11: Debug Report 강제 OFF

                            Return_1byte(DEBUG_REPORT, 0x11);  //  ACK

                          case 0x20:                     //  F5 20: Dev Report ON (v3→dev 전환)

                            Task_Start_50ms(Return_Report_Task_dev);  // 즉시 전환

                            Return_1byte(DEBUG_REPORT, 0x20);  //  ACK

                          case 0x21:                     //  F5 21: Dev Report OFF (dev→v3 원복)

                            Task_Start_50ms(Return_Report_Task_v3);  // 즉시 전환

                            Return_1byte(DEBUG_REPORT, 0x21);  //  ACK

                          default:                       //  F5 xx: 리포트 토글 (기존 동작)

                            Debug_Report_Active = !Debug_Report_Active;

                            Return_1byte(DEBUG_REPORT, Debug_Report_Active);

                      } else {                           //  FF FF 02 F5 [CHK] — 기존 토글

                        Debug_Report_Active = !Debug_Report_Active;

                        Return_1byte(DEBUG_REPORT, Debug_Report_Active);

                    case 0xFF:                          //for TEST   FF FF 03 FF ## CHK  (##:  [00]Default [01]RAM data  [02]Original Sector [03]Backup Sector [04]Variables)

                      DEBUG_printf("\r\n Memory Dump for TEST\r\n");

                              UART3_Puts("\r\nOriginal Sector\r\n");

                              UART3_Puts("\r\nBackup Sector\r\n");

                              UART3_Puts("\r\nDefault RAM Data\r\n");

                          for(int i=0; i<sizeof(Data_Form_RAM); i++) {

                            if(i%16==0)  UART3_printf("\r\n[%04X] ", i);

                          UART3_printf("\r\n\r\n[HW]\tModel: %d.%d\t",RAM_Data.model[0],RAM_Data.model[1]);

                          UART3_printf("FW Ver: %d.%d.%d\t",WespionVer.VerMajer,WespionVer.VerMiner,WespionVer.VerSub);

                          UART3_printf("Date: %04d.%02d.%02d\t",WespionVer.VerYear,WespionVer.VerMonth,WespionVer.VerDate);

                          UART3_printf("\r\n\tUpdate\tDate: %s\tTime: %s",__DATE__, __TIME__);      ///somebp 2024.02.08

                          UART3_printf("\r\n\r\n[Motor1]\tPole:%d\t",Motor1.Para.Pole);

                          UART3_printf("Ld:%s\t",Float2String(Motor1.Para.Ld, 2));

                          UART3_printf("Lq:%s\t",Float2String(Motor1.Para.Lq, 2));

                          UART3_printf("Rs:%s\t",Float2String(Motor1.Para.Rs, 2));

                          UART3_printf("PhaiF:%s\t",Float2String(Motor1.Para.PhaiF, 3));

                          UART3_printf("\r\nGain\t(CurD) Kp:%s\t",Float2String(Motor1.GainCurD.Kp, 2));

                          UART3_printf("Ki:%s\t",Float2String(Motor1.GainCurD.Ki, 2));

                          UART3_printf("Kaw:%s\t",Float2String(Motor1.GainCurD.Kaw, 2));

                          UART3_printf("(CurQ) Kp:%s\t",Float2String(Motor1.GainCurQ.Kp, 2));

                          UART3_printf("Ki:%s\t",Float2String(Motor1.GainCurQ.Ki, 2));

                          UART3_printf("Kaw:%s\t",Float2String(Motor1.GainCurQ.Kaw, 2));

                          UART3_printf("\r\n\t(Spd)  Kp:%s\t",Float2String(Motor1.GainSpd.Kp, 2));

                          UART3_printf("Ki:%s\t",Float2String(Motor1.GainSpd.Ki, 2));

                          UART3_printf("Kaw:%s\t",Float2String(Motor1.GainSpd.Kaw, 2));

                          UART3_printf("(Fwk)  Kp:%s\t",Float2String(Motor1.GainFweak.Kp, 2));

                          UART3_printf("Ki:%s\t",Float2String(Motor1.GainFweak.Ki, 2));

                          UART3_printf("\r\nM_Ang\tPolePair:%d\t",Motor1_Ang.PolePair);

                          UART3_printf("Enc:%d\t",Motor1_Ang.EncPulse);

                          UART3_printf("AngSc:%s\t",Float2String(Motor1_Ang.AngleScale, 2));

                          UART3_printf("SpdSc:%s\t",Float2String(Motor1_Ang.SpeedScale, 2));

                          UART3_printf("\r\n\tOffs:%s\t",Float2String(Motor1_Ang.AngleElecOffset, 1));

                          UART3_printf("RpmLim:%s\t",Float2String(Motor1_Ang.RpmLim, 1));

                          UART3_printf("SpdLim:%s\t",Float2String(Motor1_Ang.SpdCtrlOutLim, 1));

                          UART3_printf("\r\nMR\tMaxS:%d\t",Motor1_Ang.MR.Max.Sin);

                          UART3_printf("MaxC:%d\t",Motor1_Ang.MR.Max.Cos);

                          UART3_printf("MinS:%d\t",Motor1_Ang.MR.Min.Sin);

                          UART3_printf("MinC:%d\t",Motor1_Ang.MR.Min.Cos);

                          UART3_printf("MidS:%d\t",Motor1_Ang.MR.Mid.Sin);

                          UART3_printf("MidC:%d\t",Motor1_Ang.MR.Mid.Cos);

                          UART3_printf("GainS:%s\t",Float2String(Motor1_Ang.MR.Gain.Sin, 1));

                          UART3_printf("GainC:%s\t",Float2String(Motor1_Ang.MR.Gain.Cos, 1));

                          UART3_printf("\r\n\r\n[Motor2]\tPole:%d\t",Motor2.Para.Pole);

                          UART3_printf("Ld:%s\t",Float2String(Motor2.Para.Ld, 2));

                          UART3_printf("Lq:%s\t",Float2String(Motor2.Para.Lq, 2));

                          UART3_printf("Rs:%s\t",Float2String(Motor2.Para.Rs, 2));

                          UART3_printf("PhaiF:%s\t",Float2String(Motor2.Para.PhaiF, 3));

                          UART3_printf("\r\nGain\t(CurD) Kp:%s\t",Float2String(Motor2.GainCurD.Kp, 2));

                          UART3_printf("Ki:%s\t",Float2String(Motor2.GainCurD.Ki, 2));

                          UART3_printf("Kaw:%s\t",Float2String(Motor2.GainCurD.Kaw, 2));

                          UART3_printf("(CurQ) Kp:%s\t",Float2String(Motor2.GainCurQ.Kp, 2));

                          UART3_printf("Ki:%s\t",Float2String(Motor2.GainCurQ.Ki, 2));

                          UART3_printf("Kaw:%s\t",Float2String(Motor2.GainCurQ.Kaw, 2));

                          UART3_printf("\r\n\t(Spd)  Kp:%s\t",Float2String(Motor2.GainSpd.Kp, 2));

                          UART3_printf("Ki:%s\t",Float2String(Motor2.GainSpd.Ki, 2));

                          UART3_printf("Kaw:%s\t",Float2String(Motor2.GainSpd.Kaw, 2));

                          UART3_printf("(Fwk)  Kp:%s\t",Float2String(Motor2.GainFweak.Kp, 2));

                          UART3_printf("Ki:%s\t",Float2String(Motor2.GainFweak.Ki, 2));

                          UART3_printf("\r\nM_Ang\tPolePair:%d\t",Motor2_Ang.PolePair);

                          UART3_printf("Enc:%d\t",Motor2_Ang.EncPulse);

                          UART3_printf("AngSc:%s\t",Float2String(Motor2_Ang.AngleScale, 2));

                          UART3_printf("SpdSc:%s\t",Float2String(Motor2_Ang.SpeedScale, 2));

                          UART3_printf("\r\n\tOffs:%s\t",Float2String(Motor2_Ang.AngleElecOffset, 1));

                          UART3_printf("RpmLim:%s\t",Float2String(Motor2_Ang.RpmLim, 1));

                          UART3_printf("SpdLim:%s\t",Float2String(Motor2_Ang.SpdCtrlOutLim, 1));

                          UART3_printf("\r\nMR\tMaxS:%d\t",Motor2_Ang.MR.Max.Sin);

                          UART3_printf("MaxC:%d\t",Motor2_Ang.MR.Max.Cos);

                          UART3_printf("MinS:%d\t",Motor2_Ang.MR.Min.Sin);

                          UART3_printf("MinC:%d\t",Motor2_Ang.MR.Min.Cos);

                          UART3_printf("MidS:%d\t",Motor2_Ang.MR.Mid.Sin);

                          UART3_printf("MidC:%d\t",Motor2_Ang.MR.Mid.Cos);

                          UART3_printf("GainS:%s\t",Float2String(Motor2_Ang.MR.Gain.Sin, 1));

                          UART3_printf("GainC:%s\t",Float2String(Motor2_Ang.MR.Gain.Cos, 1));

                          UART3_printf("\r\n\r\n[Inv]\tHWAdcSc:%s\t",Float2String(Inv.Para.HwAdcScale, 4));

                          UART3_printf("HWVdcSc:%s\t",Float2String(Inv.Para.HwVdcScale, 4));

                          UART3_printf("HWiSc:%s\t",Float2String(Inv.Para.HwiScale, 4));

                          UART3_printf("VdcSc:%s\t",Float2String(Inv.Para.VdcScale, 4));

                          UART3_printf("iSc:%s\t",Float2String(Inv.Para.iScale, 4));

                          UART3_printf("\r\n\tTsmp:%s\t",Float2String(Inv.Para.Tsamp, 4));

                          UART3_printf("VdcMg:%s\t",Float2String(Inv.Para.VdcMargin, 4));

                          UART3_printf("PWMmax:%s\t",Float2String(Inv.Para.PWM_Max, 2));

                          UART3_printf("PWMmid:%s\t",Float2String(Inv.Para.PWM_Mid, 2));

                          UART3_printf("VLiFw:%s\t",Float2String(Inv.Ctrl.VdcLimFweak, 1));

                          UART3_printf("ILiFw:%s\t",Float2String(Inv.Ctrl.IlimFweak, 1));

                          UART3_printf("\r\n\r\n[LPF]\t(Vdc)fc:%s\t",Float2String(LpfVdc.fc, 1));

                          UART3_printf("fs:%s\t",Float2String(LpfVdc.fs, 1));

                          UART3_printf("(Cur)fc:%s\t",Float2String(LpfCur.fc, 1));

                          UART3_printf("fs:%s\t",Float2String(LpfCur.fs, 1));

                          UART3_printf("(Spd)fc:%s\t",Float2String(LpfSpd.fc, 1));

                          UART3_printf("fs:%s\t",Float2String(LpfSpd.fs, 1));

                          UART3_printf("\r\n\r\n[Err]\t(Is)Open:%s\t",Float2String(WP_ErrRef.IsensorOpen, 1));

                          UART3_printf("Short:%s\t",Float2String(WP_ErrRef.IsensorShort, 1));

                          UART3_printf("OFFmax:%s\t",Float2String(WP_ErrRef.IsensorOffsetMax, 1));

                          UART3_printf("OFFmin:%s\t",Float2String(WP_ErrRef.IsensorOffsetMin, 1));

                          UART3_printf("\r\n\tOverCur:%s\t",Float2String(WP_ErrRef.OverCurrent, 1));

                          UART3_printf("OverSpd:%s\t",Float2String(WP_ErrRef.OverSpeed, 1));

                          UART3_printf("OverV:%s\t",Float2String(WP_ErrRef.OverVoltage, 1));

                          UART3_printf("UnderV:%s\t",Float2String(WP_ErrRef.UnderVoltage, 1));

                          UART3_printf("\r\n\r\n[Spec]\tGear:%s\t",Float2String(WP_Machine.SensorGearRatio, 1));

                          UART3_printf("GearInv:%s\t",Float2String(WP_Machine.SensorGearRatioInv, 1));

                          UART3_printf("SpoolD:%s\t",Float2String(WP_Machine.SpoolDiameter, 1));

                          UART3_printf("MaxWeight:%s\t",Float2String(WP_Machine.MaxWeight, 1));

                          UART3_printf("CableMax:%s\t",Float2String(WP_Machine.CableMaxLength, 1));

                          UART3_printf("ScW2Cur:%s\t",Float2String(WP_Machine.Scale_Weight2Current, 1));

                          UART3_printf("\r\n\tSoftWind:%s\t",Float2String(WP_Machine.SoftWindingHeight, 1));

                          UART3_printf("Def.W%s\t",Float2String(WP_Machine.DefaultWeight, 1));

                          UART3_printf("Fric.W:%s\t",Float2String(WP_Machine.FrictionWeight, 1));

                          UART3_printf("Fric.Spd+:%s\t",Float2String(WP_Machine.FrictionSpeedPlus, 1));

                          UART3_printf("Fric.Spd-:%s\t",Float2String(WP_Machine.FrictionSpeedMinus, 1));

                          UART3_printf("[ISR Profile] (180MHz, cycles/180=us)\r\n");

                          UART3_printf("  Last: %lu cycles (%s us)\r\n", ISR_Profile.last, Float2String((float)ISR_Profile.last/180.0f, 1));

                          UART3_printf("  Max:  %lu cycles (%s us)\r\n", ISR_Profile.max, Float2String((float)ISR_Profile.max/180.0f, 1));

                          UART3_printf("  Avg:  %lu cycles (%s us)\r\n", ISR_Profile.avg, Float2String((float)ISR_Profile.avg/180.0f, 1));

                          UART3_printf("  Count: %lu\r\n", ISR_Profile.count);

                          UART3_printf("  Budget: 200us (5kHz), Usage: %s%%\r\n", Float2String((float)ISR_Profile.max/36000.0f*100.0f, 1));

      old_available=UART3_Available();                ///somebp 2024.02.19

 * @brief  Inaccurate Time Delay but NOT use Systick Timer (can be used in Interrupt Service Routine)

void    Rx_Interrupt_IT(UART_HandleTypeDef *huart,uint8_t rx_data);

#define   MAX_TXBUFFER_SIZE_UART2   64    //must be 2,4,8,16,32,64,128,256(max)

#define   MAX_RXBUFFER_SIZE_UART2   256   //must be 2,4,8,16,32,64,128,256,...

#define   MAX_TXBUFFER_SIZE_UART3   64    //must be 2,4,8,16,32,64,128,256(max)

#define   MAX_RXBUFFER_SIZE_UART3   256   //must be 2,4,8,16,32,64,128,256,...

//    HAL_NVIC_SetPriority(USART3_IRQn, 10, 0);   // 0으로 낮춰보기 (HSW 24.02.20)

void UART_PutData(UART_HandleTypeDef *huart, uint8_t *data, uint8_t size )  //in queue

    if( uart_tx_uart2.head!=((uart_tx_uart2.tail+1)&(MAX_TXBUFFER_SIZE_UART2-1)) )  //not full

      uart_tx_uart2.tail &= MAX_TXBUFFER_SIZE_UART2-1;    //because MAX_TXBUFFER_SIZE != 0x100

    if(huart2.gState==HAL_UART_STATE_READY) {    //uart ready check

      uart_tx_uart2.head &= MAX_TXBUFFER_SIZE_UART2-1;    //because MAX_TXBUFFER_SIZE != 0x100

    if( uart_tx_uart3.head!=((uart_tx_uart3.tail+1)&(MAX_TXBUFFER_SIZE_UART3-1)) )  //not full

      uart_tx_uart3.tail &= MAX_TXBUFFER_SIZE_UART3-1;    //because MAX_TXBUFFER_SIZE != 0x100

    if(huart3.gState==HAL_UART_STATE_READY) {    //uart ready check

      uart_tx_uart3.head &= MAX_TXBUFFER_SIZE_UART3-1;    //because MAX_TXBUFFER_SIZE != 0x100

//-------interrupt tx, pull out data in queue buffer(dequeue)---------

    if(uart_tx_uart2.head!=uart_tx_uart2.tail) {      //not empty

    if(uart_tx_uart3.head!=uart_tx_uart3.tail) {      //not empty

//------interrupt rx, receive data-------------------------------

//    you must change the "UART_Receive_IT" function (see below)

//    the "UART_Receive_IT" function is in the "stm32f7xx_hal_uart.c" file

static HAL_StatusTypeDef UART_Receive_IT(UART_HandleTypeDef *huart)

//---------interrupt rx, data in queue buffer(enqueue) -------------

    if(uart_rx_uart2.head!=((uart_rx_uart2.tail+1)&(MAX_RXBUFFER_SIZE_UART2-1)) ) {     //if not full

      uart_rx_uart2.buffer[uart_rx_uart2.tail]=(uint8_t) READ_REG(huart->Instance->DR); //enqueue

    if(uart_rx_uart3.head!=((uart_rx_uart3.tail+1)&(MAX_RXBUFFER_SIZE_UART3-1)) ) {     //if not full

      uart_rx_uart3.buffer[uart_rx_uart3.tail]=(uint8_t) READ_REG(huart->Instance->DR); //enqueue

    if(uart_rx_uart2.head!=uart_rx_uart2.tail) {          //if not empty

      rcv_data=uart_rx_uart2.buffer[uart_rx_uart2.head++];  //dequeue

    if(uart_rx_uart3.head!=uart_rx_uart3.tail) {          //if not empty

      rcv_data=uart_rx_uart3.buffer[uart_rx_uart3.head++];  //dequeue

void Debug_UART3_printf(char* inbuf, ...) // 충돌방지를 위해 선택적 출력용 (250705)

int16_t UART_Getc_NoClear(UART_HandleTypeDef *huart,uint16_t pos)

      rcv_data=uart_rx_uart2.buffer[(uart_rx_uart2.head+pos)&(MAX_RXBUFFER_SIZE_UART2-1)];

      rcv_data=uart_rx_uart3.buffer[(uart_rx_uart3.head+pos)&(MAX_RXBUFFER_SIZE_UART3-1)];

void UART_Clear_Buffer(UART_HandleTypeDef *huart, uint16_t size)

  for(; point>1; point--)   divider *= 10;    //divider = pow(10, point);

    if(WP_Weight.Ctrl.OnOffScale[L] < 0.1f && WP_Weight.Ctrl.OnOffScale[R] < 0.1f ) {

  if((UART_Available(huart)<2) || !(UART_Getc_NoClear(huart,0)=='O' && UART_Getc_NoClear(huart,1)=='K')) {

  UART_printf(huart, "AT+LADDR\r\n");     //Return : "+LADDR=00:15:8C:96:04:FE\r\n"

  if(UART_Getc_NoClear(huart,0)=='+' && UART_Getc_NoClear(huart,1)=='L' && UART_Getc_NoClear(huart,2)=='A'

  && UART_Getc_NoClear(huart,3)=='D' && UART_Getc_NoClear(huart,4)=='D' && UART_Getc_NoClear(huart,5)=='R' && UART_Getc_NoClear(huart,6)=='=') {

  UART_printf(huart, "AT+NAME\r\n");      //Return : "+NAME=BT05\r\n"

  if(UART_Getc_NoClear(huart,0)=='+' && UART_Getc_NoClear(huart,1)=='N' && UART_Getc_NoClear(huart,2)=='A'

  && UART_Getc_NoClear(huart,3)=='M' && UART_Getc_NoClear(huart,4)=='E' && UART_Getc_NoClear(huart,5)=='=') {

 *                             WESPION Motor Control Function                         *

void WESPION_MotorControl(void) // It's in ADC_IRQHandler Function

			MotorControl_ErrChk(&WP_CtrlErr.Motor1, &WP_ErrRef, &Motor1.Fb, &Motor1_Ang, &Inv.Ctrl.AD1);

			MotorControl_ErrChk(&WP_CtrlErr.Motor2, &WP_ErrRef, &Motor2.Fb, &Motor2_Ang, &Inv.Ctrl.AD2);

  Motor1.Fb.VphaseRef = sqrtf(Motor1.Fb.VeRef.d * Motor1.Fb.VeRef.d + Motor1.Fb.VeRef.q * Motor1.Fb.VeRef.q);

  Motor2.Fb.VphaseRef = sqrtf(Motor2.Fb.VeRef.d * Motor2.Fb.VeRef.d + Motor2.Fb.VeRef.q * Motor2.Fb.VeRef.q);

//      WP_CtrlErr.Motor1.PWM = WP_PWMout(MOTOR1, Motor1.Fb.PWMcnt);

//      WP_CtrlErr.Motor2.PWM = WP_PWMout(MOTOR2, Motor2.Fb.PWMcnt);

    case Inv_TorqueWeakCtrl:  // 6번 모드 - FOC with field weak control !!

inline void MotorControl_ErrChk(WP_MotorControllerErrorTypeDef *_err, WP_ErrorValueTypeDef *_errRef, WP_MotorAxisTypeDef *_fb, WP_MotorAngleTypeDef *_ang, WP_InverterAdOffsetTypeDef *_off)

  if(_off->iRaw.U > _errRef->IsensorOpen)     //3908.0f;  // 50A에 해당하는 값

  if(_off->iRaw.U < _errRef->IsensorShort)     // 186.0f;  // -50A에 해당하는 값

    if(Error_Flag_Enable)     Return_4byte(FAIL_SAFE_ERROR, Error_Flag);

inline void Controller_ErrChk(WP_ControllerErrorTypeDef *_err, WP_ErrorValueTypeDef *_errRef, WP_InverterControlTypeDef *_inv)

  WP_RampUpDn(&Motor1.Cmd.Icmd.q, &Motor1.Cmd.Iref.q, 1.0f, 1.0f);

    Motor1.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VlimFweak, Motor1.Fb.VphaseFil, &Motor1.PiFweak, &Motor1.GainFweak, Inv.Ctrl.IlimFweak);

    Motor1.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VdcLimFweak, Inv.Ctrl.VdcFil, &Motor1.PiFweak, &Motor1.GainFweak, Inv.Ctrl.IlimFweak);

    WP_RampUpDn(&Motor1.Cmd.Icmd.d, &Motor1.Cmd.Iref.d, 1.0f, 1.0f);

  Motor1.PiCurQ.Ff = Motor1.Para.PhaiF * Motor1_Ang.RpmFil * WP_Weight.FfScale.Temp[L];

//  Motor1.PiCurQ.Ff = Motor1.Para.PhaiF * Motor1_Ang.RpmFil * WP_Weight.FfScale.Temp[MOTOR_L];// + Motor1.Para.Ld * Motor1.Fb.Ie.d * Motor1_Ang.WeFil;

//  Motor1.PiCurQ.Ff = Motor1.Para.PhaiF * Motor1_Ang.RpmFil * WP_WeightFeel.FfScale_L;

//  Motor1.PiCurD.Ff = - Motor1.Para.Lq * Motor1.Fb.Ie.q * Motor1_Ang.WeFil;

//  Motor1.Fb.VeRef.q = WP_PIcontrol_Spdlim(Motor1.Cmd.Iref.q, Motor1.Fb.Ie.q, &Motor1.PiCurQ, &Motor1.GainCurQ, Inv.Ctrl.VphaseLim, Motor1_Ang.RpmFil);///somebp 2024.02.02

  Motor1.Fb.VeRef.q = WP_PIcontrol(Motor1.Cmd.Iref.q, Motor1.Fb.Ie.q, &Motor1.PiCurQ, &Motor1.GainCurQ, Inv.Ctrl.VphaseLim);                          ///origin

  Motor1.Fb.VeRef.d = WP_PIcontrol(Motor1.Cmd.Iref.d, Motor1.Fb.Ie.d, &Motor1.PiCurD, &Motor1.GainCurD, Inv.Ctrl.VphaseLim);

  Motor1.Fb.VphaseRef = sqrtf(Motor1.Fb.VeRef.d * Motor1.Fb.VeRef.d + Motor1.Fb.VeRef.q * Motor1.Fb.VeRef.q); // 700ns

  Motor1.Fb.VphaseSet = 1.0f / Motor1.Fb.VphaseFil;   // 역수 취한 값

      WP_InvClarkeParkTrans(&Motor1.Fb.V3Set, &Motor1.Fb.VsSet, &Motor1.Cmd.Vfcmd, &Motor1_Ang);

        Motor1.Fb.VeSet.d = Motor1.Fb.VeRef.d * Inv.Ctrl.VphaseLim * Motor1.Fb.VphaseSet;

        Motor1.Fb.VeSet.q = Motor1.Fb.VeRef.q * Inv.Ctrl.VphaseLim * Motor1.Fb.VphaseSet;

      WP_InvClarkeParkTrans(&Motor1.Fb.V3Set, &Motor1.Fb.VsSet, &Motor1.Fb.VeSet, &Motor1_Ang);

  WP_InvMinMax(&Motor1.Fb.V3Set, &Motor1.Fb.V3, Inv.Ctrl.VphLim);

  WP_ClarkeParkTrans(&Motor1.Fb.V3, &Motor1.Fb.Vs, &Motor1.Fb.Ve, &Motor1_Ang);

  WP_PwmScale(Motor1.Fb.PWMcnt, &Motor1.Fb.V3, Motor1.Fb.Dutyratio, &Inv);

  WP_RampUpDn(&Motor2.Cmd.Icmd.q, &Motor2.Cmd.Iref.q, 1.0f, 1.0f);

	  Motor2.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VlimFweak, Motor2.Fb.VphaseFil, &Motor2.PiFweak, &Motor2.GainFweak, Inv.Ctrl.IlimFweak);

	  Motor2.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VdcLimFweak, Inv.Ctrl.VdcFil, &Motor2.PiFweak, &Motor2.GainFweak, Inv.Ctrl.IlimFweak);

    WP_RampUpDn(&Motor2.Cmd.Icmd.d, &Motor2.Cmd.Iref.d, 1.0f, 1.0f);

  Motor2.PiCurQ.Ff = Motor2.Para.PhaiF * Motor2_Ang.RpmFil * WP_Weight.FfScale.Temp[R];

//  Motor2.PiCurQ.Ff = Motor2.Para.PhaiF * Motor2_Ang.RpmFil * WP_Weight.FfScale.Temp[MOTOR_R];// + Motor2.Para.Ld * Motor2.Fb.Ie.d * Motor2_Ang.WeFil;

//  Motor2.PiCurQ.Ff = Motor2.Para.PhaiF * Motor2_Ang.RpmFil * WP_WeightFeel.FfScale_R;

//  Motor2.PiCurD.Ff = - Motor2.Para.Lq * Motor2.Fb.Ie.q * Motor2_Ang.WeFil;

//  Motor2.Fb.VeRef.q = WP_PIcontrol_Spdlim(Motor2.Cmd.Iref.q, Motor2.Fb.Ie.q, &Motor2.PiCurQ, &Motor2.GainCurQ, Inv.Ctrl.VphaseLim, Motor2_Ang.RpmFil);///somebp 2024.02.02

  Motor2.Fb.VeRef.q = WP_PIcontrol(Motor2.Cmd.Iref.q, Motor2.Fb.Ie.q, &Motor2.PiCurQ, &Motor2.GainCurQ, Inv.Ctrl.VphaseLim);                          ///origin

  Motor2.Fb.VeRef.d = WP_PIcontrol(Motor2.Cmd.Iref.d, Motor2.Fb.Ie.d, &Motor2.PiCurD, &Motor2.GainCurD, Inv.Ctrl.VphaseLim);

  Motor2.Fb.VphaseRef = sqrtf(Motor2.Fb.VeRef.d * Motor2.Fb.VeRef.d + Motor2.Fb.VeRef.q * Motor2.Fb.VeRef.q); // 700ns

      WP_InvClarkeParkTrans(&Motor2.Fb.V3Set, &Motor2.Fb.VsSet, &Motor2.Cmd.Vfcmd, &Motor2_Ang);

    	  Motor2.Fb.VeSet.d = Motor2.Fb.VeRef.d * Inv.Ctrl.VphaseLim * Motor2.Fb.VphaseSet;

          Motor2.Fb.VeSet.q = Motor2.Fb.VeRef.q * Inv.Ctrl.VphaseLim * Motor2.Fb.VphaseSet;

      WP_InvClarkeParkTrans(&Motor2.Fb.V3Set, &Motor2.Fb.VsSet, &Motor2.Fb.VeSet, &Motor2_Ang);

  WP_InvMinMax(&Motor2.Fb.V3Set, &Motor2.Fb.V3, Inv.Ctrl.VphLim);

  WP_ClarkeParkTrans(&Motor2.Fb.V3, &Motor2.Fb.Vs, &Motor2.Fb.Ve, &Motor2_Ang);

  WP_PwmScale(Motor2.Fb.PWMcnt, &Motor2.Fb.V3, Motor2.Fb.Dutyratio, &Inv);

  WP_RampUpDn(&Motor1.Cmd.RpmCmd, &Motor1.Cmd.RpmRef, 10.0f, 10.0f);

  Motor1.Cmd.Icmd.q = WP_PIcontrol(Motor1.Cmd.RpmRef, Motor1_Ang.RpmFil, &Motor1.PiSpd, &Motor1.GainSpd, Motor1_Ang.SpdCtrlOutLim);

  WP_RampUpDn(&Motor2.Cmd.RpmCmd, &Motor2.Cmd.RpmRef, 10.0f, 10.0f);

  Motor2.Cmd.Icmd.q = WP_PIcontrol(Motor2.Cmd.RpmRef, Motor2_Ang.RpmFil, &Motor2.PiSpd, &Motor2.GainSpd, Motor2_Ang.SpdCtrlOutLim);

        LPF(Inv.Ctrl.AD1.iOffset.U, Inv.Ctrl.AD1.iRaw.U, LpfCur.fct);

        LPF(Inv.Ctrl.AD1.iOffset.V, Inv.Ctrl.AD1.iRaw.V, LpfCur.fct);

        LPF(Inv.Ctrl.AD1.iOffset.W, Inv.Ctrl.AD1.iRaw.W, LpfCur.fct);

        LPF(Inv.Ctrl.AD2.iOffset.U, Inv.Ctrl.AD2.iRaw.U, LpfCur.fct);

        LPF(Inv.Ctrl.AD2.iOffset.V, Inv.Ctrl.AD2.iRaw.V, LpfCur.fct);

        LPF(Inv.Ctrl.AD2.iOffset.W, Inv.Ctrl.AD2.iRaw.W, LpfCur.fct);

  Motor1.Fb.I3.U = (Inv.Ctrl.AD1.iRaw.U - Inv.Ctrl.AD1.iOffset.U) * Inv.Para.iScale;

  Motor1.Fb.I3.V = (Inv.Ctrl.AD1.iRaw.V - Inv.Ctrl.AD1.iOffset.V) * Inv.Para.iScale;

  Motor1.Fb.I3.W = (Inv.Ctrl.AD1.iRaw.W - Inv.Ctrl.AD1.iOffset.W) * Inv.Para.iScale;

  Motor2.Fb.I3.U = (Inv.Ctrl.AD2.iRaw.U - Inv.Ctrl.AD2.iOffset.U) * Inv.Para.iScale;

  Motor2.Fb.I3.V = (Inv.Ctrl.AD2.iRaw.V - Inv.Ctrl.AD2.iOffset.V) * Inv.Para.iScale;

  Motor2.Fb.I3.W = (Inv.Ctrl.AD2.iRaw.W - Inv.Ctrl.AD2.iOffset.W) * Inv.Para.iScale;

  Inv.Ctrl.VphaseLim = Inv.Ctrl.VdcFil * MT_SQ3_OVR_2 * Inv.Para.VdcMargin;

  MotorCtrl_TransformAngle(&Motor1_Ang, Motor1.Cmd.XbyF_Freq, Inv.Para.Tsamp);

  WP_ClarkeParkTrans(&Motor1.Fb.I3, &Motor1.Fb.Is, &Motor1.Fb.Ie, &Motor1_Ang);

  MotorCtrl_TransformAngle(&Motor2_Ang, Motor2.Cmd.XbyF_Freq, Inv.Para.Tsamp);

  WP_ClarkeParkTrans(&Motor2.Fb.I3, &Motor2.Fb.Is, &Motor2.Fb.Ie, &Motor2_Ang);

inline void MotorCtrl_TransformAngle(WP_MotorAngleTypeDef *angle, float cmd, float tsamp)

      array_theta = (uint32_t)(2048*(angle->XbyFAngle)*MT_1_OVR_360);

      array_theta = (uint32_t)(2048*(angle->AngleElec)*MT_1_OVR_360);

inline void MotorCtrl_XbyF_Ctrl(float fc, float *angle, float SampT)

  //Motor1.Para.Pole = 10;     // SAT : 4, JKONG : 8, BS-105-10 : 30

  Motor1.Para.Pole = RAM_Data.Motor_Para_Pole;        ///MOTOR_PARA_POLE

  //Motor1.Para.PhaiF = 72.2f * 0.001f * MT_1_OVR_SQ3;  // V/krpm

  Motor1.Para.Ld = RAM_Data.Motor_Para_Ld;            ///MOTOR_PARA_Ld

  Motor1.Para.Lq = RAM_Data.Motor_Para_Lq;            ///MOTOR_PARA_Lq

  Motor1.Para.Rs = RAM_Data.Motor_Para_Rs;            ///MOTOR_PARA_Rs

  Motor1.Para.PhaiF = RAM_Data.Motor_Para_Phaif;      ///MOTOR_PARA_PhaiF

  Motor1.GainCurD.Kp = RAM_Data.Motor_GainCurD_Kp;    ///MOTOR_GAINCURD_Kp

  Motor1.GainCurD.Ki = RAM_Data.Motor_GainCurD_Ki;    ///MOTOR_GAINCURD_Ki

  Motor1.GainCurD.Kaw = RAM_Data.Motor_GainCurD_Kaw;  ///MOTOR_GAINCURD_Kaw

  Motor1.GainCurQ.Kp = RAM_Data.Motor_GainCurQ_Kp;    ///MOTOR_GAINCURQ_Kp

  Motor1.GainCurQ.Ki = RAM_Data.Motor_GainCurQ_Ki;    ///MOTOR_GAINCURQ_Ki

  Motor1.GainCurQ.Kaw = RAM_Data.Motor_GainCurQ_Kaw;  ///MOTOR_GAINCURQ_Kaw

  Motor1.GainCurQ.Kd = 0.008f;                            //somebp 2024.02.02

  Motor1.GainSpd.Kp = RAM_Data.Motor_GainSpd_Kp;      ///MOTOR_GAINSPD_Kp

  Motor1.GainSpd.Ki = RAM_Data.Motor_GainSpd_Ki;      ///MOTOR_GAINSPD_Ki

  Motor1.GainSpd.Kaw = RAM_Data.Motor_Gainspd_Kaw;    ///MOTOR_GAINSPD_Kaw

  Motor1.GainFweak.Kp = RAM_Data.Motor_GainFweak_Kp;  ///MOTOR_GAINFWEAK_Kp

  Motor1.GainFweak.Ki = RAM_Data.Motor_GainFweak_Ki;  ///MOTOR_GAINFWEAK_Ki

  //Motor1_Ang.EncPulse = 4096*4;     //  TLE5012B : 4096, JKONG : 1000

  //Motor1_Ang.AngleScale = 360.0f / (float)Motor1_Ang.EncPulse;

  //Motor1_Ang.SpeedScale = 1000.0f * 60.0f / (float)Motor1_Ang.EncPulse;

  //Motor1_Ang.AngleElecOffset = 10.0f;       // SAT : 12, JKONG : #1 257 / #2 197 -> inaccurate, check every time!

  //Motor1_Ang.AngleElecOffset = 194.0f;       // 24.01.05 -> Can be changed by sample modification, check every time!

  Motor1_Ang.EncPulse = RAM_Data.Motor_Ang_EncPulse;    ///MOTOR_ANG_ENCPULSE

  Motor1_Ang.AngleScale = RAM_Data.Motor_Ang_AngleScale;///MOTOR_ANG_ANGLESCALE

  Motor1_Ang.SpeedScale = RAM_Data.Motor_Ang_SpeedScale;///MOTOR_ANG_SPEEDSCALE

  Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset; ///MOTOR1_ANG_ANGLEELCOFFSET

  Motor1_Ang.MR.Max.Sin = RAM_Data.Motor_Ang_MR_Max_Sin;  ///MOTOR_ANG_MR_MAX_SIN

  Motor1_Ang.MR.Max.Cos = RAM_Data.Motor_Ang_MR_Max_Cos;  ///MOTOR_ANG_MR_MAX_COS

  Motor1_Ang.MR.Min.Sin = RAM_Data.Motor_Ang_MR_Min_Sin;  ///MOTOR_ANG_MR_MIN_SIN

  Motor1_Ang.MR.Min.Cos = RAM_Data.Motor_Ang_MR_Min_Cos;  ///MOTOR_ANG_MR_MIN_COS

  Motor1_Ang.MR.Mid.Sin = RAM_Data.Motor_Ang_MR_Mid_Sin;  ///MOTOR_ANG_MR_MID_SIN

  Motor1_Ang.MR.Mid.Cos = RAM_Data.Motor_Ang_MR_Mid_Cos;  ///MOTOR_ANG_MR_MID_COS

  Motor1_Ang.MR.Gain.Sin = RAM_Data.Motor_Ang_MR_Gain_Sin;///MOTOR_ANG_MR_GAIN_SIN

  Motor1_Ang.MR.Gain.Cos = RAM_Data.Motor_Ang_MR_Gain_Cos;///MOTOR_ANG_MR_GAIN_COS

  Motor2.Para.Pole = Motor1.Para.Pole;          // SAT : 4, JKONG : 8, BS-105-10 : 30

  Motor2_Ang.EncPulse = Motor1_Ang.EncPulse;     //  TLE5012B : 4096, JKONG : 1000

  //Motor2_Ang.AngleElecOffset = 147.0f;       // SAT : 12, JKONG : #1 77 / #2 198.759857f -> inaccurate, check every time!

  Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset; ///MOTOR2_ANG_ANGLEELCOFFSET// SAT : 12, JKONG : #1 77 / #2 198.759857f -> inaccurate, check every time!

  Inv.Para.HwAdcScale = RAM_Data.Inv_Para_HwAdcScale; ///INV_PARA_HWADCSCALE

  Inv.Para.HwVdcScale = RAM_Data.Inv_Para_HwVdcScale; ///INV_PARA_HWVDCSCALE

  Inv.Para.HwiScale = RAM_Data.Inv_Para_HwiScale;     ///INV_PARA_HWISCALE

  Inv.Para.VdcScale = Inv.Para.HwAdcScale / Inv.Para.HwVdcScale;

  Inv.Para.Tsamp = RAM_Data.Inv_Para_Tsamp;         ///INV_PARA_TSAMP

  Inv.Para.PWM_Max = RAM_Data.Inv_Para_PWM_Max;     ///INV_PARA_PWM_MAX

  Inv.Para.PWM_Mid = RAM_Data.Inv_Para_PWM_Mid;     ///INV_PARA_PWM_MID

  Inv.Para.VdcMargin = RAM_Data.Inv_Para_VdcMargin; ///INV_PARA_VDCMARGIN

  LpfVdc.fc = RAM_Data.LPFVdc_fc;                   ///LPFVDC_FC

  LpfVdc.fs = RAM_Data.LPFVdc_fs;                   ///LPFVDC_FS

  LpfCur.fc = RAM_Data.LPFCur_fc;                   ///LPFCUR_FC

  LpfCur.fs = RAM_Data.LPFCur_fs;                   ///LPFCUR_FS

  LpfSpd.fc = RAM_Data.LPFSpd_fc;                   ///LPFSPD_FC

  LpfSpd.fs = RAM_Data.LPFSpd_fs;                   ///LPFSPD_FS

  WP_ErrRef.IsensorOpen = RAM_Data.ErrRef_IsensorOpen;          ///ERRREF_ISENSOROPEN

  WP_ErrRef.IsensorShort = RAM_Data.ErrRef_IsensorShort;        ///ERRREF_ISENSORSHORT

  WP_ErrRef.OverCurrent = RAM_Data.ErrRef_OverCurrent;          ///ERRREF_OVERCURRENT

  WP_ErrRef.OverSpeed = RAM_Data.ErrRef_OverSpeed;              ///ERRREF_OVERSPEED

  WP_ErrRef.OverVoltage = RAM_Data.ErrRef_OverVoltage;          ///ERRREF_OVERVOLTAGE

  WP_ErrRef.UnderVoltage = RAM_Data.ErrRef_UnderVoltage;        ///ERRREF_UNDERVOLTAGE

  WP_ErrRef.IsensorOffsetMax = RAM_Data.ErrRef_IsensorOffsetMax;///ERRREF_ISENSOROFFSETMAX

  WP_ErrRef.IsensorOffsetMin = RAM_Data.ErrRef_IsensorOffsetMin;///ERRREF_ISENSOROFFSETMIN

//  Inv.Ctrl.VdcLimFweak = 49.0f;                               // Generation Field weak Pi Controller Output Limit

//  Inv.Ctrl.IlimFweak = WP_ErrRef.OverCurrent * 0.8f;          // Motoring Field weak Pi controller Output Limit

  Inv.Ctrl.VdcLimFweak = RAM_Data.Inv_Ctrl_VdcLimFweak; ///INV_CTRL_VDCLIMFWEAK // Generation Field weak Pi Controller Output Limit

  Inv.Ctrl.IlimFweak = RAM_Data.Inv_Ctrl_IlimFweak;     ///INV_CTRL_ILIMFWEAK   // Motoring Field weak Pi controller Output Limit

//  Motor1_Ang.SpdCtrlOutLim = WP_ErrRef.OverCurrent * 0.8f;    // Speed Control Pi Controller Output Limit

//  Motor2_Ang.SpdCtrlOutLim = WP_ErrRef.OverCurrent * 0.8f;    // Speed Control Pi Controller Output Limit

  Motor1_Ang.RpmLim = RAM_Data.Motor_Ang_RpmLim;              ///MOTOR_ANG_RPMLIM

  Motor1_Ang.SpdCtrlOutLim = RAM_Data.Motor_Ang_SpdCtrlOutLim;///MOTOR_ANG_SPDCTRLOUTLIM  // Speed Control Pi Controller Output Limit

  Motor2_Ang.RpmLim = RAM_Data.Motor_Ang_RpmLim;              ///MOTOR_ANG_RPMLIM

  Motor2_Ang.SpdCtrlOutLim = RAM_Data.Motor_Ang_SpdCtrlOutLim;///MOTOR_ANG_SPDCTRLOUTLIM  // Speed Control Pi Controller Output Limit

//  Motor1.PiCurQ.Dterm = 0.0f;             ///somebp 2024.02.02

 *                             Motor Control Basic API                                *

inline void WP_ClarkeParkTrans(WP_Motor3PhaseTypeDef *_3, WP_MotorDQTypeDef *s, WP_MotorDQTypeDef *e, WP_MotorAngleTypeDef *Angle)

inline void WP_InvClarkeParkTrans(WP_Motor3PhaseTypeDef *_3, WP_MotorDQTypeDef *s, WP_MotorDQTypeDef *e, WP_MotorAngleTypeDef *Angle)

inline void WP_InvMinMax(WP_Motor3PhaseTypeDef *_3in, WP_Motor3PhaseTypeDef *_3out, float Vlim)

  b = ( (_3in->U - _3in->V) * a > 0 ) ? ( _3in->U ): ( (_3in->V - _3in->W) * a >=0  ? _3in->W : _3in->V );

inline void WP_InvDeadTimeComp(WP_Motor3PhaseTypeDef *_3I, WP_Motor3PhaseTypeDef *_3V, WP_Motor3PhaseTypeDef *_3dead, float VT_DEAD, float Vlim)

inline float WP_PIcontrol(float Cmd, float Fedb, WP_PITypeDef *PiCur, WP_GainTypeDef *Gain, float VRefLim)

  PiCur->PIterm = PiCur->Pterm + Gain->Ki*(PiCur->Err - PiCur->AW * Gain->Kaw) + PiCur->Iterm + PiCur->Ff;  //piout=Ki*e(k)+x(k-1)+Kp*e(k)

  PiCur->Iterm += Gain->Ki * (PiCur->Err - PiCur->AW * Gain->Kaw);  //x(k)=x(k-1)+Ki*e(k)

//#define   DTERM_SCALE         2.0f            //1(Low) ~ 100(High)

//inline float WP_PIcontrol_Spdlim(float Cmd, float Fedb, WP_PITypeDef *PiCur, WP_GainTypeDef *Gain, float VRefLim, float rpm)

//  if(rpm > LIMIT_RPM) {                 //  add Dterm, operate  when rpm is -lim

//    PiCur->Dterm = (Gain->Kd* PiCur->Err)/0.0000625;  // Ang->RpmFil enable

//      float dterm  = 1-PiCur->Dterm/(1000.0f/DTERM_SCALE);              //>=1.0

//          UART3_printf("[Dterm]%s\t",Float2String(PiCur->Dterm, 2));    //Negative

//  PiCur->Pterm = Gain->Kp * PiCur->Err;// Kp*e(k) //PiCur->Aw*Hain->kaw :error가 너무 쌓였을 때 안티와인드업 / 현재의 적분텀과 과거의 적분텀을 더함

//  PiCur->PIterm = PiCur->Pterm + Gain->Ki*(PiCur->Err - PiCur->AW * Gain->Kaw) + PiCur->Iterm + PiCur->Ff;  //piout=Ki*e(k)+x(k-1)+Kp*e(k)

//  PiCur->Out = SAT(PiCur->PIterm, VRefLim);       //SAT: Cut out of range

//  PiCur->Iterm += Gain->Ki * (PiCur->Err - PiCur->AW * Gain->Kaw);  //x(k)=x(k-1)+Ki*e(k)

//  PiCur->Iterm = SAT(PiCur->Iterm, VRefLim);      // VRefLim: currently x

inline float WP_PIcontrolForFieldWeak(float Cmd, float Fedb, WP_PITypeDef *PiCur, WP_GainTypeDef *Gain, float VRefLim)

  PiCur->PIterm = PiCur->Pterm + Gain->Ki*(PiCur->Err) + PiCur->Iterm;  //piout=Ki*e(k)+x(k-1)+Kp*e(k)

  PiCur->Iterm += Gain->Ki * (PiCur->Err);  //x(k)=x(k-1)+Ki*e(k)

inline void WP_PwmScale(uint32_t* Duty, WP_Motor3PhaseTypeDef *_3V, float *Rt, WP_InverterTypeDef *inv)

  *(Duty)   = (uint32_t) SAT_U((Du[0] + inv->Para.PWM_Mid), inv->Para.PWM_Max);

  *(Duty+1) = (uint32_t) SAT_U((Du[1] + inv->Para.PWM_Mid), inv->Para.PWM_Max);

  *(Duty+2) = (uint32_t) SAT_U((Du[2] + inv->Para.PWM_Mid), inv->Para.PWM_Max);

inline void WP_RampUpDn(float *Cmd, float *Ref, float RampU, float RampD) // 단위 : 1A / 스위칭

    if(Ang->MR.Raw.Sin >= Ang->MR.Max.Sin) Ang->MR.Max.Sin = Ang->MR.Raw.Sin;

    if(Ang->MR.Raw.Sin <= Ang->MR.Min.Sin) Ang->MR.Min.Sin = Ang->MR.Raw.Sin;

    if(Ang->MR.Raw.Cos >= Ang->MR.Max.Cos) Ang->MR.Max.Cos = Ang->MR.Raw.Cos;

    if(Ang->MR.Raw.Cos <= Ang->MR.Min.Cos) Ang->MR.Min.Cos = Ang->MR.Raw.Cos;

    Ang->MR.Gain.Cos = (((float)Ang->MR.Max.Sin - Ang->MR.Mid.Sin))/(((float)Ang->MR.Max.Cos - Ang->MR.Mid.Cos));

  Ang->MR.Scaled.Sin = ((float)Ang->MR.Raw.Sin - (float)Ang->MR.Mid.Sin) * Ang->MR.Gain.Sin;

  Ang->MR.Scaled.Cos = ((float)Ang->MR.Raw.Cos - (float)Ang->MR.Mid.Cos) * Ang->MR.Gain.Cos;

  Ang->AngleMech = TR_RAD2ANG * (atan2f(Ang->MR.Scaled.Sin, Ang->MR.Scaled.Cos)+PI);

  Ang->AngleElec = Ang->AngleElec - (float)((uint16_t)(Ang->AngleElec * MT_1_OVR_360))*360.0f + Ang->AngleElecOffset;

inline WP_ErrorStatus WP_EncPos(uint8_t Mot, WP_MotorAngleTypeDef *Ang)  // this function in 'stm32f4xx_it.c'

//    Ang->diffF += (float)(((Ang->Pulse - Ang->PulseOld)<<16)>>16);

    Ang->diffF += (float)(((Ang->PulseOld - Ang->Pulse)<<16)>>16);

    Ang->AngleElec = Ang->AngleElec - (float)((uint16_t)(Ang->AngleElec * MT_1_OVR_360))*360.0f + Ang->AngleElecOffset;

inline WP_ErrorStatus WP_EncSpd(uint8_t Mot, WP_MotorAngleTypeDef *Ang)

      Ang->Rpm = (float)Ang->diffM * Ang->SpeedScale * 0.3333; // 기어비 반영

//    you must change the "HAL_GPIO_EXTI_Callback" function (see below)

//    the "HAL_GPIO_EXTI_Callback" function is in the "gpio.c" file

// *                             WESPION Motor Control Function                         *

//void WESPION_MotorControl(void) // It's in ADC_IRQHandler Function

//      MotorControl_ErrChk(&WP_CtrlErr.Motor1, &WP_ErrRef, &Motor1.Fb, &Motor1_Ang, &Inv.Ctrl.AD1);

//      MotorControl_ErrChk(&WP_CtrlErr.Motor2, &WP_ErrRef, &Motor2.Fb, &Motor2_Ang, &Inv.Ctrl.AD2);

//inline void WESPION_MotorAddon(void) // It's in main while loop

//  Motor1.Fb.VphaseRef = sqrtf(Motor1.Fb.VeRef.d * Motor1.Fb.VeRef.d + Motor1.Fb.VeRef.q * Motor1.Fb.VeRef.q);

//  Motor2.Fb.VphaseRef = sqrtf(Motor2.Fb.VeRef.d * Motor2.Fb.VeRef.d + Motor2.Fb.VeRef.q * Motor2.Fb.VeRef.q);

////      WP_CtrlErr.Motor1.PWM = WP_PWMout(MOTOR1, Motor1.Fb.PWMcnt);

////      WP_CtrlErr.Motor2.PWM = WP_PWMout(MOTOR2, Motor2.Fb.PWMcnt);

//    case Inv_TorqueWeakCtrl:  // 6번 모드 - FOC with field weak control !!

//inline void MotorControl_ErrChk(WP_MotorControllerErrorTypeDef *_err, WP_ErrorValueTypeDef *_errRef, WP_MotorAxisTypeDef *_fb, WP_MotorAngleTypeDef *_ang, WP_InverterAdOffsetTypeDef *_off)

//inline void Controller_ErrChk(WP_ControllerErrorTypeDef *_err, WP_ErrorValueTypeDef *_errRef, WP_InverterControlTypeDef *_inv)

//  WP_RampUpDn(&Motor1.Cmd.Icmd.q, &Motor1.Cmd.Iref.q, 1.0f, 1.0f);

//    Motor1.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VlimFweak, Motor1.Fb.VphaseFil, &Motor1.PiFweak, &Motor1.GainFweak, Inv.Ctrl.IlimFweak);

//    Motor1.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VdcLimFweak, Inv.Ctrl.VdcFil, &Motor1.PiFweak, &Motor1.GainFweak, Inv.Ctrl.IlimFweak);

//    WP_RampUpDn(&Motor1.Cmd.Icmd.d, &Motor1.Cmd.Iref.d, 1.0f, 1.0f);

////  Motor1.PiCurQ.Ff = Motor1.Para.PhaiF * Motor1_Ang.RpmFil * WP_WeightFeel.FfScale_L;

////  Motor1.PiCurD.Ff = - Motor1.Para.Lq * Motor1.Fb.Ie.q * Motor1_Ang.WeFil;

//  Motor1.Fb.VeRef.q = WP_PIcontrol(Motor1.Cmd.Iref.q, Motor1.Fb.Ie.q, &Motor1.PiCurQ, &Motor1.GainCurQ, Inv.Ctrl.VphaseLim);

//  Motor1.Fb.VeRef.d = WP_PIcontrol(Motor1.Cmd.Iref.d, Motor1.Fb.Ie.d, &Motor1.PiCurD, &Motor1.GainCurD, Inv.Ctrl.VphaseLim);

//  Motor1.Fb.VphaseRef = sqrtf(Motor1.Fb.VeRef.d * Motor1.Fb.VeRef.d + Motor1.Fb.VeRef.q * Motor1.Fb.VeRef.q); // 700ns

//  Motor1.Fb.VphaseSet = 1.0f / Motor1.Fb.VphaseFil;   // 역수 취한 값

//      WP_InvClarkeParkTrans(&Motor1.Fb.V3Set, &Motor1.Fb.VsSet, &Motor1.Cmd.Vfcmd, &Motor1_Ang);

//        Motor1.Fb.VeSet.d = Motor1.Fb.VeRef.d * Inv.Ctrl.VphaseLim * Motor1.Fb.VphaseSet;

//        Motor1.Fb.VeSet.q = Motor1.Fb.VeRef.q * Inv.Ctrl.VphaseLim * Motor1.Fb.VphaseSet;

//      WP_InvClarkeParkTrans(&Motor1.Fb.V3Set, &Motor1.Fb.VsSet, &Motor1.Fb.VeSet, &Motor1_Ang);

//  WP_InvMinMax(&Motor1.Fb.V3Set, &Motor1.Fb.V3, Inv.Ctrl.VphLim);

//  WP_ClarkeParkTrans(&Motor1.Fb.V3, &Motor1.Fb.Vs, &Motor1.Fb.Ve, &Motor1_Ang);

//  WP_PwmScale(Motor1.Fb.PWMcnt, &Motor1.Fb.V3, Motor1.Fb.Dutyratio, &Inv);

//  WP_CtrlErr.Motor1.PWM = WP_PWMout(MOTOR1, Motor1.Fb.PWMcnt);

//  WP_RampUpDn(&Motor2.Cmd.Icmd.q, &Motor2.Cmd.Iref.q, 1.0f, 1.0f);

//    Motor2.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VlimFweak, Motor2.Fb.VphaseFil, &Motor2.PiFweak, &Motor2.GainFweak, Inv.Ctrl.IlimFweak);

//    Motor2.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VdcLimFweak, Inv.Ctrl.VdcFil, &Motor2.PiFweak, &Motor2.GainFweak, Inv.Ctrl.IlimFweak);

//    WP_RampUpDn(&Motor2.Cmd.Icmd.d, &Motor2.Cmd.Iref.d, 1.0f, 1.0f);

////  Motor2.PiCurQ.Ff = Motor2.Para.PhaiF * Motor2_Ang.RpmFil * WP_WeightFeel.FfScale_R;

////  Motor2.PiCurD.Ff = - Motor2.Para.Lq * Motor2.Fb.Ie.q * Motor2_Ang.WeFil;

//  Motor2.Fb.VeRef.q = WP_PIcontrol(Motor2.Cmd.Iref.q, Motor2.Fb.Ie.q, &Motor2.PiCurQ, &Motor2.GainCurQ, Inv.Ctrl.VphaseLim);

//  Motor2.Fb.VeRef.d = WP_PIcontrol(Motor2.Cmd.Iref.d, Motor2.Fb.Ie.d, &Motor2.PiCurD, &Motor2.GainCurD, Inv.Ctrl.VphaseLim);

//  Motor2.Fb.VphaseRef = sqrtf(Motor2.Fb.VeRef.d * Motor2.Fb.VeRef.d + Motor2.Fb.VeRef.q * Motor2.Fb.VeRef.q); // 700ns

//      WP_InvClarkeParkTrans(&Motor2.Fb.V3Set, &Motor2.Fb.VsSet, &Motor2.Cmd.Vfcmd, &Motor2_Ang);

//        Motor2.Fb.VeSet.d = Motor2.Fb.VeRef.d * Inv.Ctrl.VphaseLim * Motor2.Fb.VphaseSet;

//          Motor2.Fb.VeSet.q = Motor2.Fb.VeRef.q * Inv.Ctrl.VphaseLim * Motor2.Fb.VphaseSet;

//      WP_InvClarkeParkTrans(&Motor2.Fb.V3Set, &Motor2.Fb.VsSet, &Motor2.Fb.VeSet, &Motor2_Ang);

//  WP_InvMinMax(&Motor2.Fb.V3Set, &Motor2.Fb.V3, Inv.Ctrl.VphLim);

//  WP_ClarkeParkTrans(&Motor2.Fb.V3, &Motor2.Fb.Vs, &Motor2.Fb.Ve, &Motor2_Ang);

//  WP_PwmScale(Motor2.Fb.PWMcnt, &Motor2.Fb.V3, Motor2.Fb.Dutyratio, &Inv);

//  WP_CtrlErr.Motor2.PWM = WP_PWMout(MOTOR2, Motor2.Fb.PWMcnt);

//  WP_RampUpDn(&Motor1.Cmd.RpmCmd, &Motor1.Cmd.RpmRef, 10.0f, 10.0f);

//  Motor1.Cmd.Icmd.q = WP_PIcontrol(Motor1.Cmd.RpmRef, Motor1_Ang.RpmFil, &Motor1.PiSpd, &Motor1.GainSpd, Motor1_Ang.SpdCtrlOutLim);

//  WP_RampUpDn(&Motor2.Cmd.RpmCmd, &Motor2.Cmd.RpmRef, 10.0f, 10.0f);

//  Motor2.Cmd.Icmd.q = WP_PIcontrol(Motor2.Cmd.RpmRef, Motor2_Ang.RpmFil, &Motor2.PiSpd, &Motor2.GainSpd, Motor2_Ang.SpdCtrlOutLim);

//        LPF(Inv.Ctrl.AD1.iOffset.U, Inv.Ctrl.AD1.iRaw.U, LpfCur.fct);

//        LPF(Inv.Ctrl.AD1.iOffset.V, Inv.Ctrl.AD1.iRaw.V, LpfCur.fct);

//        LPF(Inv.Ctrl.AD1.iOffset.W, Inv.Ctrl.AD1.iRaw.W, LpfCur.fct);

//        LPF(Inv.Ctrl.AD2.iOffset.U, Inv.Ctrl.AD2.iRaw.U, LpfCur.fct);

//        LPF(Inv.Ctrl.AD2.iOffset.V, Inv.Ctrl.AD2.iRaw.V, LpfCur.fct);

//        LPF(Inv.Ctrl.AD2.iOffset.W, Inv.Ctrl.AD2.iRaw.W, LpfCur.fct);

//  Motor1.Fb.I3.U = (Inv.Ctrl.AD1.iRaw.U - Inv.Ctrl.AD1.iOffset.U) * Inv.Para.iScale;

//  Motor1.Fb.I3.V = (Inv.Ctrl.AD1.iRaw.V - Inv.Ctrl.AD1.iOffset.V) * Inv.Para.iScale;

//  Motor1.Fb.I3.W = (Inv.Ctrl.AD1.iRaw.W - Inv.Ctrl.AD1.iOffset.W) * Inv.Para.iScale;

//  Motor2.Fb.I3.U = (Inv.Ctrl.AD2.iRaw.U - Inv.Ctrl.AD2.iOffset.U) * Inv.Para.iScale;

//  Motor2.Fb.I3.V = (Inv.Ctrl.AD2.iRaw.V - Inv.Ctrl.AD2.iOffset.V) * Inv.Para.iScale;

//  Motor2.Fb.I3.W = (Inv.Ctrl.AD2.iRaw.W - Inv.Ctrl.AD2.iOffset.W) * Inv.Para.iScale;

//  Inv.Ctrl.VphaseLim = Inv.Ctrl.VdcFil * MT_SQ3_OVR_2 * Inv.Para.VdcMargin;

//  MotorCtrl_TransformAngle(&Motor1_Ang, Motor1.Cmd.XbyF_Freq, Inv.Para.Tsamp);

//  WP_ClarkeParkTrans(&Motor1.Fb.I3, &Motor1.Fb.Is, &Motor1.Fb.Ie, &Motor1_Ang);

//  MotorCtrl_TransformAngle(&Motor2_Ang, Motor2.Cmd.XbyF_Freq, Inv.Para.Tsamp);

//  WP_ClarkeParkTrans(&Motor2.Fb.I3, &Motor2.Fb.Is, &Motor2.Fb.Ie, &Motor2_Ang);

//inline void MotorCtrl_TransformAngle(WP_MotorAngleTypeDef *angle, float cmd, float tsamp)

//      array_theta = (uint32_t)(2048*(angle->XbyFAngle)*MT_1_OVR_360);

//      array_theta = (uint32_t)(2048*(angle->AngleElec)*MT_1_OVR_360);

//inline void MotorCtrl_XbyF_Ctrl(float fc, float *angle, float SampT)

//  Motor1.Para.Pole = 10;     // SAT : 4, JKONG : 8, BS-105-10 : 30

//  Motor1.Para.PhaiF = 72.2f * 0.001f * MT_1_OVR_SQ3;  // V/krpm

//  Motor1_Ang.EncPulse = 4096*4;     //  TLE5012B : 4096, JKONG : 1000

//  Motor1_Ang.AngleScale = 360.0f / (float)Motor1_Ang.EncPulse;

//  Motor1_Ang.SpeedScale = 1000.0f * 60.0f / (float)Motor1_Ang.EncPulse;

////  Motor1_Ang.AngleElecOffset = 37.0f;       // 24.01.05 -> Can be changed by sample modification, check every time!

//  Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset; ///MOTOR1_ANG_ANGLEELCOFFSET

//  Motor2.Para.Pole = Motor1.Para.Pole;     // SAT : 4, JKONG : 8, BS-105-10 : 30

//  Motor2_Ang.EncPulse = Motor1_Ang.EncPulse;     //  TLE5012B : 4096, JKONG : 1000

////  Motor2_Ang.AngleElecOffset = 120.0f;       // SAT : 12, JKONG : #1 77 / #2 198.759857f -> inaccurate, check every time!

//  Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset;

//  Inv.Para.VdcScale = Inv.Para.HwAdcScale / Inv.Para.HwVdcScale;

//  Inv.Para.iScale = - Inv.Para.HwAdcScale / Inv.Para.HwiScale;

// *                             Motor Control Basic API                                *

//inline void WP_ClarkeParkTrans(WP_Motor3PhaseTypeDef *_3, WP_MotorDQTypeDef *s, WP_MotorDQTypeDef *e, WP_MotorAngleTypeDef *Angle)

//inline void WP_InvClarkeParkTrans(WP_Motor3PhaseTypeDef *_3, WP_MotorDQTypeDef *s, WP_MotorDQTypeDef *e, WP_MotorAngleTypeDef *Angle)

//inline void WP_InvMinMax(WP_Motor3PhaseTypeDef *_3in, WP_Motor3PhaseTypeDef *_3out, float Vlim)

//  b = ( (_3in->U - _3in->V) * a > 0 ) ? ( _3in->U ): ( (_3in->V - _3in->W) * a >=0  ? _3in->W : _3in->V );

//inline void WP_InvDeadTimeComp(WP_Motor3PhaseTypeDef *_3I, WP_Motor3PhaseTypeDef *_3V, WP_Motor3PhaseTypeDef *_3dead, float VT_DEAD, float Vlim)

//inline float WP_PIcontrol(float Cmd, float Fedb, WP_PITypeDef *PiCur, WP_GainTypeDef *Gain, float VRefLim)

//inline float WP_PIcontrolForFieldWeak(float Cmd, float Fedb, WP_PITypeDef *PiCur, WP_GainTypeDef *Gain, float VRefLim)

//  PiCur->PIterm = PiCur->Pterm + Gain->Ki*(PiCur->Err) + PiCur->Iterm;  //piout=Ki*e(k)+x(k-1)+Kp*e(k)

//  PiCur->Iterm += Gain->Ki * (PiCur->Err);  //x(k)=x(k-1)+Ki*e(k)

//inline void WP_PwmScale(uint32_t* Duty, WP_Motor3PhaseTypeDef *_3V, float *Rt, WP_InverterTypeDef *inv)

//  Du[0] = SAT(_3V->U * inv->Ctrl.PWM_Scale, inv->Para.PWM_Mid);

//  Du[1] = SAT(_3V->V * inv->Ctrl.PWM_Scale, inv->Para.PWM_Mid);

//  Du[2] = SAT(_3V->W * inv->Ctrl.PWM_Scale, inv->Para.PWM_Mid);

//  *(Duty)   = (uint32_t) SAT_U((Du[0] + inv->Para.PWM_Mid), inv->Para.PWM_Max);

//  *(Duty+1) = (uint32_t) SAT_U((Du[1] + inv->Para.PWM_Mid), inv->Para.PWM_Max);

//  *(Duty+2) = (uint32_t) SAT_U((Du[2] + inv->Para.PWM_Mid), inv->Para.PWM_Max);

//inline void WP_RampUpDn(float *Cmd, float *Ref, float RampU, float RampD) // 단위 : 1A / 스위칭

//    if(Ang->MR.Raw.Sin >= Ang->MR.Max.Sin) Ang->MR.Max.Sin = Ang->MR.Raw.Sin;

//    if(Ang->MR.Raw.Sin <= Ang->MR.Min.Sin) Ang->MR.Min.Sin = Ang->MR.Raw.Sin;

//    if(Ang->MR.Raw.Cos >= Ang->MR.Max.Cos) Ang->MR.Max.Cos = Ang->MR.Raw.Cos;

//    if(Ang->MR.Raw.Cos <= Ang->MR.Min.Cos) Ang->MR.Min.Cos = Ang->MR.Raw.Cos;

//    Ang->MR.Gain.Cos = (((float)Ang->MR.Max.Sin - Ang->MR.Mid.Sin))/(((float)Ang->MR.Max.Cos - Ang->MR.Mid.Cos));

//  Ang->MR.Scaled.Sin = ((float)Ang->MR.Raw.Sin - (float)Ang->MR.Mid.Sin) * Ang->MR.Gain.Sin;

//  Ang->MR.Scaled.Cos = ((float)Ang->MR.Raw.Cos - (float)Ang->MR.Mid.Cos) * Ang->MR.Gain.Cos;

//  Ang->AngleMech = TR_RAD2ANG * (atan2f(Ang->MR.Scaled.Sin, Ang->MR.Scaled.Cos)+PI);

//  Ang->AngleElec = Ang->AngleElec - (float)((uint16_t)(Ang->AngleElec * MT_1_OVR_360))*360.0f + Ang->AngleElecOffset;

//inline WP_ErrorStatus WP_EncPos(uint8_t Mot, WP_MotorAngleTypeDef *Ang)  // this function in 'stm32f4xx_it.c'

////    Ang->diffF += (float)(((Ang->Pulse - Ang->PulseOld)<<16)>>16);

//    Ang->diffF += (float)(((Ang->PulseOld - Ang->Pulse)<<16)>>16);

//    Ang->AngleElec = Ang->AngleElec - (float)((uint16_t)(Ang->AngleElec * MT_1_OVR_360))*360.0f + Ang->AngleElecOffset;

//inline WP_ErrorStatus WP_EncSpd(uint8_t Mot, WP_MotorAngleTypeDef *Ang)

//      Ang->Rpm = (float)Ang->diffM * Ang->SpeedScale * 0.3333; // 기어비 반영

////    you must change the "HAL_GPIO_EXTI_Callback" function (see below)

////    the "HAL_GPIO_EXTI_Callback" function is in the "gpio.c" file

 * @brief _sbrk() allocates memory to the newlib heap and is used by malloc

 * #  .data  #  .bss  #       newlib heap       #          MSP stack          #

 * #         #        #                         # Reserved by _Min_Stack_Size #

 * ^-- RAM start      ^-- _end                             _estack, RAM end --^

 * This implementation starts allocating at the '_end' linker symbol

 * The '_Min_Stack_Size' linker symbol reserves a memory for the MSP stack

 * The implementation considers '_estack' linker symbol to be RAM end

 * NOTE: If the MSP stack, at any point during execution, grows larger than the

  extern uint8_t _end; /* Symbol defined in the linker script */

  extern uint8_t _estack; /* Symbol defined in the linker script */

  extern uint32_t _Min_Stack_Size; /* Symbol defined in the linker script */

  const uint32_t stack_limit = (uint32_t)&_estack - (uint32_t)&_Min_Stack_Size;

  * @brief   CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.

  *   This file provides two functions and one global variable to be called from 

  *      - SystemInit(): This function is called at startup just after reset and 

  *                      before branch to main program. This call is made inside

  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used

  *                                  by the user application to setup the SysTick 

  *                                  timer or configure other parameters.

  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must

  *                                 be called whenever the core clock is changed

  #define HSE_VALUE    ((uint32_t)25000000) /*!< Default value of the External oscillator in Hz */

  #define HSI_VALUE    ((uint32_t)16000000) /*!< Value of the Internal oscillator in Hz*/

/************************* Miscellaneous Configuration ************************/

/*!< Uncomment the following line if you need to use external SRAM or SDRAM as data memory  */

#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\

 || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\

 || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx)

#endif /* STM32F40xxx || STM32F41xxx || STM32F42xxx || STM32F43xxx || STM32F469xx || STM32F479xx ||\

#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\

 || defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx)

#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx ||\

/* Note: Following vector table addresses must be defined in line with linker

/*!< Uncomment the following line if you need to relocate the vector table

     anywhere in Flash or Sram, else the vector table is kept at the automatic

/*!< Uncomment the following line if you need to relocate your vector Table

#define VECT_TAB_BASE_ADDRESS   SRAM_BASE       /*!< Vector Table base address field.

                                                     This value must be a multiple of 0x200. */

#define VECT_TAB_OFFSET         0x00000000U     /*!< Vector Table base offset field.

#define VECT_TAB_BASE_ADDRESS   FLASH_BASE      /*!< Vector Table base address field.

#define VECT_TAB_OFFSET         0x00004000U     /*!< Vector Table base offset field.

      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency 

         Note: If you use this function to configure the system clock; then there

               is no need to call the 2 first functions listed above, since SystemCoreClock

const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};

  *         Initialize the FPU setting, vector table location and External memory 

  /* FPU settings ------------------------------------------------------------*/

    SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  /* set CP10 and CP11 Full Access */

  /* Configure the Vector Table location -------------------------------------*/

  SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */

   * @brief  Update SystemCoreClock variable according to Clock Register Values.

  *         The SystemCoreClock variable contains the core clock (HCLK), it can

  *         be used by the user application to setup the SysTick timer or configure

  * @note   Each time the core clock (HCLK) changes, this function must be called

  *         to update SystemCoreClock variable value. Otherwise, any configuration

  * @note   - The system frequency computed by this function is not the real 

  *           frequency in the chip. It is calculated based on the predefined 

  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)

  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)

  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) 

  *             or HSI_VALUE(*) multiplied/divided by the PLL factors.

  *         (*) HSI_VALUE is a constant defined in stm32f4xx_hal_conf.h file (default value

  *             16 MHz) but the real value may vary depending on the variations

  *         (**) HSE_VALUE is a constant defined in stm32f4xx_hal_conf.h file (its value

  *              depends on the application requirements), user has to ensure that HSE_VALUE

  *              is same as the real frequency of the crystal used. Otherwise, this function

  *         - The result of this function could be not correct when using fractional

  uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2;

  /* Get SYSCLK source -------------------------------------------------------*/

        pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);

        pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);

      pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;

  /* Compute HCLK frequency --------------------------------------------------*/

  *         This function configures the external memories (SRAM/SDRAM)

  *         This SRAM/SDRAM will be used as program data memory (including heap and stack).

  /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface clock */

/*-- FMC Configuration -------------------------------------------------------*/

#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)

#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ 

#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */

  /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface 

#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx || STM32F479xx */

/*-- GPIOs Configuration -----------------------------------------------------*/

/*-- FMC/FSMC Configuration --------------------------------------------------*/

#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx)|| defined(STM32F417xx)\

#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F412Zx || STM32F412Vx */

#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F427xx || STM32F437xx ||\

          STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx || STM32F412Zx || STM32F412Vx  */ 

  * @brief        This file provides code for the MSP Initialization

/* External functions --------------------------------------------------------*/

/* External variables --------------------------------------------------------*/

/*           Cortex-M4 Processor Interruption and Exception Handlers          */

  * @brief This function handles Pre-fetch fault, memory access fault.

  * @brief This function handles Undefined instruction or illegal state.

  * @brief This function handles System service call via SWI instruction.

  * @brief This function handles Pendable request for system service.

/* STM32F4xx Peripheral Interrupt Handlers                                    */

/* Add here the Interrupt Handlers for the used peripherals.                  */

/* For the available peripheral interrupt handler names,                      */

/* please refer to the startup file (startup_stm32f4xx.s).                    */

  * @brief This function handles ADC1, ADC2 and ADC3 interrupts.

//  ITcnt.adc++;  // Below 3 handler function execution time is about 10.20us

///somebp 2024.02.07  //move to HAL_ADCEx_InjectedConvCpltCallback() in adc.c

  ITcnt.adc++;  // Below 3 handler function execution time is about 10.20us

  * @brief This function handles TIM1 break interrupt and TIM9 global interrupt.

  * @brief This function handles TIM1 update interrupt and TIM10 global interrupt.

  ITcnt.tim1_up_tim10++;  // Below handler function + 2 Encoder function execution time is about 5us

  * @brief This function handles TIM1 trigger and commutation interrupts and TIM11 global interrupt.

  * @brief This function handles TIM8 break interrupt and TIM12 global interrupt.

  * @brief This function handles TIM8 update interrupt and TIM13 global interrupt.

  * @brief This function handles TIM8 trigger and commutation interrupts and TIM14 global interrupt.

// Private function prototypes -----------------------------------------------

  if((void *)User_Task_Every_1ms!=(void *)0)                                      (*User_Task_Every_1ms)();

  if((void *)User_Task_Every_5ms!=(void *)0    && (Get_Time()%    5)==0)          (*User_Task_Every_5ms)();

  if((void *)User_Task_Every_10ms!=(void *)0   && (Get_Time()%   10)==1)          (*User_Task_Every_10ms)();

  if((void *)User_Task_Every_10ms_1!=(void *)0 && (Get_Time()%   10)==2)          (*User_Task_Every_10ms_1)();

  if((void *)User_Task_Every_50ms!=(void *)0   && (Get_Time()%   50)==3)          (*User_Task_Every_50ms)();

  if((void *)User_Task_Every_50ms_1!=(void *)0 && (Get_Time()%   50)==13)         (*User_Task_Every_50ms_1)();

  if((void *)User_Task_Every_100ms!=(void *)0  && (Get_Time()%  100)==4)          (*User_Task_Every_100ms)();

  if((void *)User_Task_Every_250ms!=(void *)0  && (Get_Time()%  250)==5)          (*User_Task_Every_250ms)();

  if((void *)User_Task_Every_500ms!=(void *)0  && (Get_Time()%  500)==6)          (*User_Task_Every_500ms)();

  if((void *)User_Task_Every_1s!=(void *)0     && (Get_Time()% 1000)==7)          (*User_Task_Every_1s)();

  if((void *)User_Task_Every_10s!=(void *)0    && (Get_Time()%10000)==8)          (*User_Task_Every_10s)();

  if((void *)User_Task_Every_60s!=(void *)0    && (Get_Time()%60000)==9)          (*User_Task_Every_60s)();

  if((void *)User_Task_Every_User!=(void *)0   && (Get_Time()%User_Time)==User_Time-1)    (*User_Task_Every_User)();

///////////////////////   Private Function  ///////////////////////////

  if(WP_Weight.Ctrl.OnOffScale[L] == 0 && WP_Weight.Ctrl.OnOffScale[R] == 0) {

//  Return_4x1byte(WEIGHTPLUS, WP_Gym.WeightSet[L]*4, WP_Gym.WeightMode[L], WP_Gym.WeightSet[R]*4, WP_Gym.WeightMode[R]);

 *  [FF FF] [09] [F5] [avg_H avg_L] [max_H max_L] [usage] [state] [skip] [CHK]

  uint8_t usage = (uint8_t)((float)ISR_Profile.max / 36000.0f * 100.0f);

    Return_2x1byte(AUTOWEIGHT_STATUS, ISR_Deferred.MotionAuto_StatusL, ISR_Deferred.MotionAuto_StatusR);

    Return_4x1byte(ECC_LEVEL, ISR_Deferred.EccClamp_L, ISR_Deferred.EccClamp_R, ISR_Deferred.EccClamp_Err, 0);

void WeightController (int side) {        //  ScaleWeight2Current가 반영 안된 상태임!!

  // 굳이 좌우 변수를 지역변수에 할당해야만 하는 건 아니지만, 로직 부분 함수 가독성 개선을 위해 짧게 수정하려고 살려둠

  else if(WP_Gym.Region.H_gnd*10 <= Pos && Pos < WP_Gym.Region.H_base*10) {

  else if(WP_Gym.Region.H_base*10 <= Pos && Pos < WP_Gym.Region.H_shock*10) {

    Iq_Temp = LINEAR_INTERPOLATION(Pos, WP_Gym.Region.H_base*10, WP_Gym.Region.H_shock*10, WP_Gym.F_min, WP_Gym.F_idle) * WP_Machine.Scale_Weight2Current;

  else if(WP_Gym.Region.H_shock*10 <= Pos && Pos < H_LoSoft*10) {

        F_Load = TargetWeight * WP_Weight.Ctrl.OnOffScale[side];

          Iq_Temp = (WP_Gym.F_idle + (F_Load - WP_Gym.F_idle) * (Pos - H_LoSoft*10) / (RangeLo*10 - H_LoSoft*10)) * WP_Machine.Scale_Weight2Current;

        if (WP_Gym.EccSpdUp < Spd) {   //  속도가 상한 이상인 경우 = 올라감 -> 기본 무게 (올라가는 방향이 +)

        } else if (Spd <= WP_Gym.EccSpdDown) {   //  속도가 하한 이하인 경우 = 내려감 -> 더해진 무게

        F_Load = (TargetWeight + FeccTemp) * WP_Weight.Ctrl.OnOffScale[side];

        //        if (WP_Gym.EccSpdUp < Spd) {   // 속도가 상한 이상인 경우 = 올라감 -> 더해진 무게

        //          if (FeccTemp >= WP_Gym.F_EccSet) {   // FeccTemp가 너무 커지지 않도록 제어

        //        } else if (Spd <= WP_Gym.EccSpdDown) {   // 속도가 하한 이하인 경우 = 내려감 -> 기본 무게로 복귀

        //          if (FeccTemp <= 0) {   // FeccTemp가 음수로 내려가지 않도록 제어

        //        F_Load = (TargetWeight + FeccTemp) * WP_Weight.Ctrl.OnOffScale[side];

        //        if (F_Load <= WP_Gym.F_idle) F_Load = WP_Gym.F_idle;

        //          Iq_Temp = (WP_Gym.F_idle + (F_Load - WP_Gym.F_idle) * (Pos - H_LoSoft*10) / (RangeLo*10 - H_LoSoft*10)) * WP_Machine.Scale_Weight2Current;

        //          Iq_Temp = F_Load * WP_Machine.Scale_Weight2Current;

          Iq_Temp = (WP_Gym.F_idle + (F_Load - WP_Gym.F_idle) * (Pos - H_LoSoft*10) / (RangeHi*10 - H_LoSoft*10)) * WP_Machine.Scale_Weight2Current;

          WP_LEDCtrl.R = 10 + 240 * (Pos - H_LoSoft*10) / (RangeHi*10 - H_LoSoft*10);

          //            LED_Weight_G = 120 - 120 * (Pos - H_LoSoft - 600) / (RangeHi - H_LoSoft - 600);

          WP_LEDCtrl.G = 40 - 35 * (Pos - H_LoSoft*10) / (RangeHi*10 - H_LoSoft*10);

          //          if(LED_Weight_G <= 10)  LED_Weight_G = 10;

          WP_LEDCtrl.B = 40 - 40 * (Pos - H_LoSoft*10) / (RangeHi*10 - H_LoSoft*10);

          //          if(LED_Weight_B <= 10)  LED_Weight_B = 10;

          float fmin = WP_Weight.Iso.Fmin > WP_Gym.F_idle ? WP_Weight.Iso.Fmin : WP_Gym.F_idle;

          float neutral = fmin;  // 최소 텐션 (Fmin or F_idle 중 큰 쪽)

                F_Load_iso[side] += ki * 3.0f * error * dt;  // 브레이킹 3배

                F_Load_iso[side] += ki * error * dt;          // 풀기 1배

            float fmax = (WP_Weight.Iso.Fmax > 0) ? WP_Weight.Iso.Fmax : TargetWeight;

              if (F_Load_iso[side] < neutral) F_Load_iso[side] = neutral;

              if (F_Load_iso[side] > neutral) F_Load_iso[side] = neutral;

            F_Load = F_Load_iso[side] * WP_Weight.Ctrl.OnOffScale[side];

            float decayRate = (WP_Weight.Iso.DecayRate * 0.5f) * dt;  // 하강의 절반 속도

            float fmax_clamp = (WP_Weight.Iso.Fmax > 0) ? WP_Weight.Iso.Fmax : TargetWeight;

            if (F_Load_iso[side] > fmax_clamp) F_Load_iso[side] = fmax_clamp;

          float maxDelta = WP_Weight.Safety.SlewRate_kgPerSec * dt;

          if (delta > maxDelta) F_Load = F_Load_prev[side] + maxDelta;

          else if (delta < -maxDelta) F_Load = F_Load_prev[side] - maxDelta;

          if (cappedSpd > WP_Weight.Hydro.Vmax) cappedSpd = WP_Weight.Hydro.Vmax;

          F_Load = TargetWeight * (minR + (1.0f - minR) * spdRatio) * WP_Weight.Ctrl.OnOffScale[side];

        // F = TargetWeight + clamp(vibKg, absMax) × sin(2π × freq × t)

          float osc = sinf(2.0f * 3.14159265f * WP_Weight.Vib.Freq * t_sec);

          float maxByRatio = TargetWeight * WP_Weight.Vib.MaxRatio;

          F_Load = (TargetWeight + vibKg * osc) * WP_Weight.Ctrl.OnOffScale[side];

   * 저점 이하에서 토크 제어 이상한 원인 찾았다!!! 단위 바뀌면서 *10해줘야 하는데 위에만 하고 여기 빼먹었었음!! (250705)

   * 가 아니라 왜 저점 아래에선 전향보상을 안 하는 거지? *10을 빼먹는 바람에 이 구분에 따른 아래 영역이, 실제 저점을 꽤 높게 설정하지 않으면 체감되지 어려워서 다행이었던 것 같음!

   *  - H_Shock 이하는 항상 0으로 둬서 타겟 전류가 0(거의)인데도 불구 전류 지령이 흔들리는 걸 방지하고

        //      Ff_Scale = -LINEAR_INTERPOLATION(F_Load, 1.5, 2.5, WP_Weight.FfScale.Max, WP_Weight.FfScale.Min);

        Ff_Scale = (WP_Weight.FfScale.Max + WP_Weight.FfScale.Min)*0.5;

      Ff_Scale = (WP_Weight.FfScale.Max + WP_Weight.FfScale.Min)*0.5;

      //      Ff_Scale = -LINEAR_INTERPOLATION(F_Load, 1.5, 2.5, WP_Weight.FfScale.Max, WP_Weight.FfScale.Min);

    Ff_Scale = (WP_Weight.FfScale.Max + WP_Weight.FfScale.Min)*0.5;

#if 0   // R1 : 미완이지만 이게 더 안정해보이긴 함. 그래도 검증 안 됐으니 미적용하고 배포하는 게 안전?? (250705)

  if(WP_Weight.Safety.IqLimit > 0 && Iq_Temp > WP_Weight.Safety.IqLimit)

  if(Iq_Temp >= WP_Machine.MaxCurrent)  Iq_Temp = WP_Machine.MaxCurrent;

  DevReport_FLoad[side] = Iq_Temp / WP_Machine.Scale_Weight2Current;  // Iq→kg 역변환

//  } else if(-WP_Weight.RefSpdMove < Spd && Spd <= WP_Weight.RefSpdMove){

    //        if(FfScale_new >= WP_Weight.FfScale_Max) FfScale_new = WP_Weight.FfScale_Max;

  if(-WP_Weight.FfScale.Step < FfScaleDiff && FfScaleDiff < WP_Weight.FfScale.Step) {

        if(WP_Gym.Speed[L] > -WP_Weight.RefSpdStop && WP_Gym.Speed[R] > -WP_Weight.RefSpdStop) {    // 정지 판단 속도 이상으로 내려가지 않는 경우

          WP_Weight.Ctrl.OnOffScale[L] = WP_Weight.Ctrl.OnOffScale[L] + WP_Weight.Ctrl.OnOffStep;

          WP_Weight.Ctrl.OnOffScale[R] = WP_Weight.Ctrl.OnOffScale[R] + WP_Weight.Ctrl.OnOffStep;

            WP_Weight.Ctrl.MotionAutoStatus = STOP;    // 앱과 통신하는 변수 업데이트

            ISR_Deferred.WeightOnOff_Status = WP_Weight.Ctrl.OnOffStatus[side];

        if(WP_Gym.Speed[L] < WP_Weight.RefSpdStop && WP_Gym.Speed[R] < WP_Weight.RefSpdStop) {    // 정지 판단 속도 이상으로 올라가지 않는 경우

          WP_Weight.Ctrl.OnOffScale[L] = WP_Weight.Ctrl.OnOffScale[L] - WP_Weight.Ctrl.OnOffStep;

          WP_Weight.Ctrl.OnOffScale[R] = WP_Weight.Ctrl.OnOffScale[R] - WP_Weight.Ctrl.OnOffStep;

    if(WP_Gym.Speed[L] > -WP_Weight.RefSpdStop && WP_Gym.Speed[R] > -WP_Weight.RefSpdStop) {    // 정지 판단 속도 이상으로 내려가지 않는 경우

      Return_4x1byte(WEIGHTPLUS, WP_Gym.WeightSet[L]*2, WP_Gym.WeightMode[L], WP_Gym.WeightSet[R]*2, WP_Gym.WeightMode[R]);

    if(WP_Gym.Speed[L] < WP_Weight.RefSpdStop && WP_Gym.Speed[R] < WP_Weight.RefSpdStop) {    // 정지 판단 속도 이상으로 올라가지 않는 경우

      WP_Gym.WeightSet[L] = WP_Gym.WeightSet[L] - 3.0f*step;    // 빠지는 속도는 2에서 3으로 올림 -> 계속 개선 필요(25.05.05)

  if(-step <= WP_Gym.WeightSet[L] - WP_Gym.WeightBLE[L] && WP_Gym.WeightSet[L] - WP_Gym.WeightBLE[L] <= step){

    if(-step <= WP_Gym.WeightSet[R] - WP_Gym.WeightBLE[R] && WP_Gym.WeightSet[R] - WP_Gym.WeightBLE[R] <= step){

      //      0.25A 단위로 변경하는 현재 기준에서, 4를 곱해서 1 LSB 당 500g 단위로 보내줌

uint8_t Debug_HilMode = 0;          // [v1.0.4] HIL 모드: 0=실측, 1=position 주입

  // 단위 : Position [1mm], PositionFine [0.05mm], PositionFine2 [0.1mm]

  WP_Gym.Position[L] = -WP_Machine.SpoolDiameter *0.5f *(Motor1_Ang.AngleMechAll - WP_Gym.CalibDeg[L]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv;

  WP_Gym.PositionFine[L] = -20 * WP_Machine.SpoolDiameter *0.5f *(Motor1_Ang.AngleMechAll - WP_Gym.CalibDeg[L]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv;

  WP_Gym.PositionFine2[L] = -10 * WP_Machine.SpoolDiameter *0.5f *(Motor1_Ang.AngleMechAll - WP_Gym.CalibDeg[L]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv;

  WP_Gym.Position[R] = -WP_Machine.SpoolDiameter *0.5f *(Motor2_Ang.AngleMechAll - WP_Gym.CalibDeg[R]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv;

  WP_Gym.PositionFine[R] = -20 * WP_Machine.SpoolDiameter *0.5f *(Motor2_Ang.AngleMechAll - WP_Gym.CalibDeg[R]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv;

  WP_Gym.PositionFine2[R] = -10 * WP_Machine.SpoolDiameter *0.5f *(Motor2_Ang.AngleMechAll - WP_Gym.CalibDeg[R]) * TR_ANG2RAD * WP_Machine.SensorGearRatioInv;

  WP_Gym.Speed[L] = -WP_Machine.SpoolDiameter *0.5f * (Motor1_Ang.RpmFil *TR_RPM2RAD) * WP_Machine.SensorGearRatioInv;

  WP_Gym.Speed[R] = -WP_Machine.SpoolDiameter *0.5f * (Motor2_Ang.RpmFil *TR_RPM2RAD) * WP_Machine.SensorGearRatioInv;

    float rawAcc = (WP_Gym.Speed[s] - DevReport_PrevSpd[s]) * dt_inv;

    DevReport_Accel[s] += alpha * (rawAcc - DevReport_Accel[s]);  // 1차 LPF

  WP_Gym.Position[L]      = WP_Gym.PositionFine[L] / 20;      // 1mm

  WP_Gym.PositionFine2[L] = WP_Gym.PositionFine[L] / 2;       // 0.1mm

void PositionCalibrate (uint8_t side)   //  가동범위 세팅 완료 후 '바닥면' 캘리브레이션 기능으로 변경해야 함!! (23.11.22)

      if (WP_Gym.Position[side] > WP_Gym.Region.H_shock + WP_Gym.Region.L_soft) {

        WP_Gym.Region.RangeLo[side] = WP_Gym.Position[side] + WP_Gym.Region.L_soft;

        WP_Gym.Region.H_LoSoft[side] = WP_Gym.Region.RangeLo[side] - WP_Gym.Region.L_soft;

        WP_Gym.Region.RangeLo[side] = WP_Gym.Region.H_shock + WP_Gym.Region.L_soft;

      if (WP_Gym.Position[side] > WP_Gym.Region.RangeLo[side] + WP_Gym.Region.L_RangeMin) {

        WP_Gym.Region.H_HiSoft[side] = WP_Gym.Region.RangeHi[side] + WP_Gym.Region.L_soft;

void SetRangeDigit(uint8_t side, uint8_t Point, uint16_t Position){

      if (WP_Gym.Region.H_shock + WP_Gym.Region.L_soft < Position && Position < WP_Gym.Region.RangeHi[side] - WP_Gym.Region.L_RangeMin) {

      if (Position > WP_Gym.Region.RangeLo[side] + WP_Gym.Region.L_RangeMin) {

int Status_MotionAutoWeight = 0;    //  0: None, -1: temporary to Off, +1: temporary to On

  if(WP_Weight.Ctrl.OnOffStatus[L] == ON || WP_Weight.Ctrl.OnOffStatus[R] == ON) {

  } else if(WP_Weight.Ctrl.OnOffStatus[L] == OFF || WP_Weight.Ctrl.OnOffStatus[R] == OFF) {

  if(WP_Weight.Ctrl.MotionAutoActive == 1 && WP_Gym.Speed[L] > Spd_PullAutoOn && WP_Gym.Speed[R] > Spd_PullAutoOn) {

      if(WP_Weight.Ctrl.OnOffStatus[L] == OFF || WP_Weight.Ctrl.OnOffStatus[R] == OFF) {

        ISR_Deferred.MotionAuto_StatusL = WP_Weight.Ctrl.MotionAutoStatus;

        ISR_Deferred.MotionAuto_StatusR = WP_Weight.Ctrl.MotionAutoStatus;

  else if(WP_Gym.Speed[L] < Spd_DropAutoOff || WP_Gym.Speed[R] < Spd_DropAutoOff) {

    if(Cnt_MotionAutoWeight < -Time_DropAutoOff) {   // 디버깅 시 구분감을 위해 부호를 반대로 처리함

      if(WP_Weight.Ctrl.OnOffStatus[L] == ON || WP_Weight.Ctrl.OnOffStatus[R] == ON) {

        WP_Weight.Ctrl.MotionAutoStatus = DOWN;    // 앱에 알려주기 위함 (나중엔 좌우 구분할 예정)

  } else if ((WP_Gym.Speed[L] < Spd_StopAutoOff && WP_Gym.Speed[L] > -Spd_StopAutoOff ) && (WP_Gym.Speed[R] < Spd_StopAutoOff && WP_Gym.Speed[R] > -Spd_StopAutoOff)) {

    if(WP_Weight.Ctrl.OnOffStatus[L] == ON || WP_Weight.Ctrl.OnOffStatus[R] == ON) {

      if(Cnt_MotionAutoWeight < -Time_StopAutoOff) {   // 디버깅 시 구분감을 위해 부호를 반대로 처리함

        WP_Weight.Ctrl.MotionAutoStatus = DOWN;    // 앱에 알려주기 위함

    } else if (WP_Weight.Ctrl.OnOffStatus[L] == OFF || WP_Weight.Ctrl.OnOffStatus[R] == OFF) {

    WP_Gym.F_EccSet = WP_Gym.WeightSet[side] * 0.5f;   //  실제 적용되는 변수

    //    WP_Gym.F_EccSet_Temp[side] = WP_Gym.F_EccSet;      //  프로토콜용 임시 변수 -> 향후 이렇게 좌우 구분해야 함

    WP_Gym.F_EccSet_Temp[L] = WP_Gym.F_EccSet;      //  구조가 꼬여서 각각 바꿔줘야 함..

//float current_sequence[] = {-0.8f, 0.8f, -1.2f, 1.2f, -1.5f, 1.5f};

//const uint32_t MAX_STEP = sizeof(current_sequence)/sizeof(current_sequence[0]);

//  // 에러 상태에서는 아예 함수 진입 중단 -> 일단 둘 중 하나라도 에러면 무조건 동작 중지 처리. 나중엔 하나라도 쓸 수 있게..?

//  if (WP_EncInit.Status[L] == Error || WP_EncInit.Status[R] == Error) {

//  // 양쪽 모두 초기화 성공 시 (초기화 여부는 외부 HAL_GPIO_EXTI_Callback에서 업데이트함)

//  if (WP_EncInit.Status[L] == Done && WP_EncInit.Status[R] == Done) {

//    WP_EncInit.StartTime[L] = WP_EncInit.StartTime[R] = currentTime;

//    Motor1.Cmd.Icmd.q = current_sequence[WP_EncInit.Step[L] - 1];

//    Motor2.Cmd.Icmd.q = current_sequence[WP_EncInit.Step[L] - 1];

//      WP_EncInit.StepTime[L] = currentTime - WP_EncInit.StartTime[L];

//        // HAL_GPIO_EXTI_Callback에서 WP_EncInit.Status[ ] = Done으로 설정될 예정

//          Motor1.Cmd.Icmd.q = current_sequence[WP_EncInit.Step[L] - 1];

//      WP_EncInit.StepTime[R] = currentTime - WP_EncInit.StartTime[R];

//          Motor2.Cmd.Icmd.q = current_sequence[WP_EncInit.Step[R] - 1];

//    Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset;

//    Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset;

//    Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + 0.0f;

//    Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + 0.0f;

//    const float AngleSequence[5] = {0.0f, 30.0f, 60.0f, 90.0f, 120.0f};

//    const float CurrentSequence[4] = {-0.8f, 0.8f, -1.2f, 1.2f};

//    static uint8_t CurrentLevel_L = 0, CurrentLevel_R = 0;  // 0:-0.8A, 1:0.8A, 2:-1.2A, 3:1.2A

//                           Float2String(CurrentSequence[CurrentLevel_L], 1));

//          Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[AngleStep_L];

//          Motor1.Cmd.Icmd.q = CurrentSequence[CurrentLevel_L];

//        UART3_printf("좌측 회전 안됨 - 다음 각도 시도 (각도:%d, 전류:%d)\r\n",

//                         Float2String(CurrentSequence[CurrentLevel_L], 1));

//        Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[AngleStep_L];

//                           Float2String(CurrentSequence[CurrentLevel_R], 1));

//          Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[AngleStep_R];

//          Motor2.Cmd.Icmd.q = CurrentSequence[CurrentLevel_R];

//        UART3_printf("우측 회전 안됨 - 다음 각도 시도 (각도:%d, 전류:%d)\r\n",

//                         Float2String(CurrentSequence[CurrentLevel_R], 1));

//        Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[AngleStep_R];

  //const float AngleSequence[10] = {0.0f, 30.0f, 60.0f, 90.0f, 120.0f, 150.0f, 180.0f, 210.0f, 240.0f, 270.0f};

  //const float AngleSequence[10] = {0.0f, 180.0f, 90.0f, 270.0f, 45.0f, 225.0f, 135.0f, 315.0f, 30.0f, 210.0f};

  const float AngleSequence[10] = {0.0f, 90.0f, 180.0f, 270.0f, 45.0f, 135.0f, 225.0f, 315.0f, 30.0f, 60.0f};

  uint8_t num_Ang = sizeof(AngleSequence) / sizeof(AngleSequence[0]);

  uint8_t num_Curr = sizeof(CurrentSequence) / sizeof(CurrentSequence[0]);

  if (WP_EncInit.Status[L] == Error || WP_EncInit.Status[R] == Error) {

  if (WP_EncInit.Status[L] == Done && WP_EncInit.Status[R] == Done) {

    UART3_Puts("Encoder Init Success\r\n");  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

    Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset;

    Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset;

    WP_EncInit.Step[L] = WP_EncInit.Step[R] = 0;  // NOTE: 구조체에서 Step[2] 제거됨 - AngleStep[2]로 대체

    // WP_EncInit.AngleStep[L] = WP_EncInit.AngleStep[R] = 0;  // 구조체 초기화 시 자동으로 0

  if (WP_EncInit.Step[L] == 0 && WP_EncInit.Step[R] == 0) {  // NOTE: 구조체에서 Step[2] 제거 예정 - AngleStep 사용 고려

    WP_EncInit.StartTime[L] = WP_EncInit.StartTime[R] = currentTime;

    Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[0];  // +0도

    Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[0];  // +0도

    WP_EncInit.Step[L] = WP_EncInit.Step[R] = 1;  // NOTE: 구조체에서 Step[2] 제거 예정

    UART3_Puts("Encoder Init Started (TorqueCtrl)\r\n");  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

      WP_EncInit.StepTime[L] = currentTime - WP_EncInit.StartTime[L];

        float rotationAmount = Abs(Motor1_Ang.AngleElec - WP_EncInit.StartAngle[L]);

        if (rotationAmount > 180.0f) rotationAmount = 360.0f - rotationAmount; // 최단 경로로 계산

          Debug_UART3_printf("좌측 풀림 제한: 360도 초과 회전 감지 - 다음 시도\r\n");  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

          WP_EncInit.StartAngle[L] = Motor1_Ang.AngleElec; // 새 시작 각도 저장

              Debug_UART3_printf("좌측 초기화 실패 (모든 시도 완료 - 회전 제한)\r\n");  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

              Debug_UART3_printf("좌측 전류 변경: %s A\r\n",  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

                  Float2String(CurrentSequence[WP_EncInit.CurrentLevel[L]], 1));

          Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[WP_EncInit.AngleStep[L]];

          Motor1.Cmd.Icmd.q = CurrentSequence[WP_EncInit.CurrentLevel[L]];

          Debug_UART3_printf("좌측 Z펄스 대기 타임아웃 (각도:%d, 전류:%d)\r\n",  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

              WP_EncInit.AngleStep[L], WP_EncInit.CurrentLevel[L]);

              Debug_UART3_printf("좌측 초기화 실패 (모든 시도 완료 - 타임아웃)\r\n");  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

        Debug_UART3_printf("좌측 회전 안됨 - 다음 각도 시도 (각도:%d, 전류:%d)\r\n",  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

            WP_EncInit.AngleStep[L], WP_EncInit.CurrentLevel[L]);

        WP_EncInit.StartAngle[L] = Motor1_Ang.AngleElec; // 새 시작 각도 저장

            Debug_UART3_printf("좌측 초기화 실패 (모든 시도 완료 - 회전 없음)\r\n");  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

            Debug_UART3_printf("좌측 전류 변경: %s A\r\n",  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

                Float2String(CurrentSequence[WP_EncInit.CurrentLevel[L]], 1));

        Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset + AngleSequence[WP_EncInit.AngleStep[L]];

        Motor1.Cmd.Icmd.q = CurrentSequence[WP_EncInit.CurrentLevel[L]];

      WP_EncInit.StepTime[R] = currentTime - WP_EncInit.StartTime[R];

        float rotationAmount = Abs(Motor2_Ang.AngleElec - WP_EncInit.StartAngle[R]);

          Debug_UART3_printf("우측 안전 제한: 360도 초과 회전 감지 - 다음 시도\r\n");  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

          WP_EncInit.StartAngle[R] = Motor2_Ang.AngleElec; // 새 시작 각도 저장

              Debug_UART3_printf("우측 초기화 실패 (모든 시도 완료 - 회전 제한)\r\n");  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

              Debug_UART3_printf("우측 전류 변경: %s A\r\n",  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

                  Float2String(CurrentSequence[WP_EncInit.CurrentLevel[R]], 1));

          Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[WP_EncInit.AngleStep[R]];

          Motor2.Cmd.Icmd.q = CurrentSequence[WP_EncInit.CurrentLevel[R]];

          Debug_UART3_printf("우측 Z펄스 대기 타임아웃 (각도:%d, 전류:%d)\r\n",  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

              WP_EncInit.AngleStep[R], WP_EncInit.CurrentLevel[R]);

              Debug_UART3_printf("우측 초기화 실패 (모든 시도 완료 - 타임아웃)\r\n");  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

        Debug_UART3_printf("우측 회전 안됨 - 다음 각도 시도 (각도:%d, 전류:%d)\r\n",  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

            WP_EncInit.AngleStep[R], WP_EncInit.CurrentLevel[R]);

        WP_EncInit.StartAngle[R] = Motor2_Ang.AngleElec; // 새 시작 각도 저장

            Debug_UART3_printf("우측 초기화 실패 (모든 시도 완료 - 회전 없음)\r\n");  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

            Debug_UART3_printf("우측 전류 변경: %s A\r\n",  // TODO: 배포시 제거 (인터럽트에서 UART 출력 위험)

                Float2String(CurrentSequence[WP_EncInit.CurrentLevel[R]], 1));

        Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset + AngleSequence[WP_EncInit.AngleStep[R]];

        Motor2.Cmd.Icmd.q = CurrentSequence[WP_EncInit.CurrentLevel[R]];

int cnt_LED_EncInit, cnt_LED_EncInit_Err, cnt_LED_Calib, cnt_LED_Calib_Err;

  const float MAX_REVERSE_ROTATION = 180.0f;      // 역방향 회전 제한 (도)

  const float MAX_WINDING_SPEED = 250.0f;         // 최대 감는 속도 (RPM)

//  const uint32_t REVERSE_RETRY_WAIT = 5000;       // 5초 역방향 재시도 대기

//  const uint8_t MAX_REVERSE_RETRY = 3;            // 최대 3회 재시도

  if (WP_ForcedCali.Status[L] == Error || WP_ForcedCali.Status[R] == Error) {

  if (WP_ForcedCali.Status[L] == Done && WP_ForcedCali.Status[R] == Done) {

  if (WP_ForcedCali.Step[L] == 0 && WP_ForcedCali.Step[R] == 0) {

    WP_ForcedCali.StartTime[L] = WP_ForcedCali.StartTime[R] = currentTime;

    WP_ForcedCali.CurrentLevel[L] = WP_ForcedCali.CurrentLevel[R] = 2; // 0.8A에서 시작

    Motor1.Cmd.Icmd.q = Motor2.Cmd.Icmd.q = ForcedCaliCurrents[2]; // 0.8A

    Debug_UART3_printf("Forced Calibration Started (TorqueCtrl)\r\n");

    float rpm = (motor == L) ? Motor1_Ang.RpmFil : Motor2_Ang.RpmFil;

    float angle = (motor == L) ? Motor1_Ang.AngleMechAll : Motor2_Ang.AngleMechAll;

    float position = (motor == L) ? WP_Gym.Position[L] : WP_Gym.Position[R];

        float newCurrent = ForcedCaliCurrents[WP_ForcedCali.CurrentLevel[motor]];

        Debug_UART3_printf("%s 과속 감지 - 전류 감소: %s A\r\n", (motor == L) ? "좌측" : "우측",

      float reverseRotation = WP_ForcedCali.StartAngle[motor] - angle; // 역방향은 감소

        Debug_UART3_printf("%s 역방향 180도 초과 회전 감지 - 에러\r\n", (motor == L) ? "좌측" : "우측");

    if (currentTime - WP_ForcedCali.StartTime[motor] > 20000) { //  FORCED_CALI_TIMEOUT

      Debug_UART3_printf("%s 강제 캘리브레이션 타임아웃 (20초 초과)\r\n", (motor == L) ? "좌측" : "우측");

          if ((angle - WP_ForcedCali.StartAngle[motor]) > MAX_ROTATION_TURNS) {

              UART3_printf("%s 과도한 회전량 감지 (18바퀴 초과)\r\n", (motor == L) ? "좌측" : "우측");

    if ((position - WP_ForcedCali.StartPosition[motor]) > MAX_POSITION_MM) {

      Debug_UART3_printf("%s 과도한 이동거리 감지 (3.5m 초과)\r\n", (motor == L) ? "좌측" : "우측");

    if (!isRotating && (currentTime - WP_ForcedCali.StopTime[motor]) > 750) {   //  STOP_WAIT_TIME

        Debug_UART3_printf("%s 강제 캘리브레이션 완료\r\n", (motor == L) ? "좌측" : "우측");

        Debug_UART3_printf("PositionCalibrate(%d) 호출 전\r\n", motor);

        Debug_UART3_printf("PositionCalibrate(%d) 호출 후\r\n", motor);

          float newCurrent = ForcedCaliCurrents[WP_ForcedCali.CurrentLevel[motor]];

          Debug_UART3_printf("%s 회전 안됨 - 전류 증가: %s A\r\n", (motor == L) ? "좌측" : "우측",

          Debug_UART3_printf("%s 최대 전류에서 회전 불가 - 감기 완료로 판단\r\n", (motor == L) ? "좌측" : "우측");

          Debug_UART3_printf("PositionCalibrate(%d) 호출 전\r\n", motor);

          Debug_UART3_printf("PositionCalibrate(%d) 호출 후\r\n", motor);