/*
 * ****************************************************
 * @file    : protocol.c
 * @brief   : Protocol Program Body
 * @author  : sohee kim / Something by People
 * @data    : 2023. 06. 13
 * ****************************************************
 */
#include "protocol.h"
#include "WESPION.h"

//#define __DEBUG__

#ifdef  __DEBUG__
#define DEBUG_printf                UART3_printf
#else
#define DEBUG_printf                Dummy_printf
#endif

uint32_t  Error_Flag = 0;
uint8_t   Error_Flag_Enable = 0;        //0:Disable, 1:Enable

// Ȳ���� �߰� �� ���� (Uart2, Uart3 ���� �۽� �� ���ο���)
void Return_LineEnding(void) {
  uint8_t MessageEnd[2] = {13,10};
  UART3_PutData(MessageEnd,2);
}

void Return_0byte(uint8_t com)
{
  uint8_t return_data[3+2];

  return_data[0]=0xFF;
  return_data[1]=0xFF;
  return_data[2]=sizeof(return_data)-3;           //size
  return_data[3]=com;

  return_data[sizeof(return_data)-1]=0;           //checksum
  for(uint8_t i=0; i<sizeof(return_data)-1; i++) {
    return_data[sizeof(return_data)-1]+=return_data[i];
  }
  return_data[sizeof(return_data)-1]=-return_data[sizeof(return_data)-1];

  UART3_PutData(return_data, sizeof(return_data));
  Return_LineEnding();
}

void Return_1byte(uint8_t com, uint8_t data)
{
  uint8_t return_data[3+3];

  return_data[0]=0xFF;
  return_data[1]=0xFF;
  return_data[2]=sizeof(return_data)-3;           //size
  return_data[3]=com;
  return_data[4]=data;

  return_data[sizeof(return_data)-1]=0;           //checksum
  for(uint8_t i=0; i<sizeof(return_data)-1; i++) {
    return_data[sizeof(return_data)-1]+=return_data[i];
  }
  return_data[sizeof(return_data)-1]=-return_data[sizeof(return_data)-1];

  UART3_PutData(return_data, sizeof(return_data));
  Return_LineEnding();
}

void Return_2byte(uint8_t com, uint16_t data)
{
  uint8_t return_data[3+4];

  return_data[0]=0xFF;
  return_data[1]=0xFF;
  return_data[2]=sizeof(return_data)-3;           //size
  return_data[3]=com;
  return_data[4]=(uint8_t)(data>>8);              //data
  return_data[5]=(uint8_t)data;

  return_data[sizeof(return_data)-1]=0;           //checksum
  for(uint8_t i=0; i<sizeof(return_data)-1; i++) {
    return_data[sizeof(return_data)-1]+=return_data[i];
  }
  return_data[sizeof(return_data)-1]=-return_data[sizeof(return_data)-1];

  UART3_PutData(return_data, sizeof(return_data));
  Return_LineEnding();
}

void Return_4byte(uint8_t com, uint32_t data)
{
  uint8_t return_data[3+6];

  return_data[0]=0xFF;
  return_data[1]=0xFF;
  return_data[2]=sizeof(return_data)-3;           //size
  return_data[3]=com;
  return_data[4]=(uint8_t)(data>>24);             //data
  return_data[5]=(uint8_t)(data>>16);
  return_data[6]=(uint8_t)(data>>8);
  return_data[7]=(uint8_t)data;

  return_data[sizeof(return_data)-1]=0;           //checksum
  for(uint8_t i=0; i<sizeof(return_data)-1; i++) {
    return_data[sizeof(return_data)-1]+=return_data[i];
  }
  return_data[sizeof(return_data)-1]=-return_data[sizeof(return_data)-1];

  UART3_PutData(return_data, sizeof(return_data));
  Return_LineEnding();
}

void Return_2x2byte(uint8_t com, uint16_t data1, uint16_t data2)
{
  uint8_t return_data[3+6];

  return_data[0]=0xFF;
  return_data[1]=0xFF;
  return_data[2]=sizeof(return_data)-3;           //size
  return_data[3]=com;
  return_data[4]=(uint8_t)(data1>>8);             //data1
  return_data[5]=(uint8_t)data1;
  return_data[6]=(uint8_t)(data2>>8);             //data2
  return_data[7]=(uint8_t)data2;

  return_data[sizeof(return_data)-1]=0;           //checksum
  for(uint8_t i=0; i<sizeof(return_data)-1; i++) {
    return_data[sizeof(return_data)-1]+=return_data[i];
  }
  return_data[sizeof(return_data)-1]=-return_data[sizeof(return_data)-1];

  UART3_PutData(return_data, sizeof(return_data));
  Return_LineEnding();
}

void Return_2x1byte(uint8_t com, uint16_t data1, uint16_t data2)
{
  uint8_t return_data[3+6];

  return_data[0]=0xFF;
  return_data[1]=0xFF;
  return_data[2]=sizeof(return_data)-3;           //size
  return_data[3]=com;
  return_data[4]=(uint8_t)data1;                 //data1
  return_data[5]=(uint8_t)data2;

  return_data[sizeof(return_data)-1]=0;           //checksum
  for(uint8_t i=0; i<sizeof(return_data)-1; i++) {
    return_data[sizeof(return_data)-1]+=return_data[i];
  }
  return_data[sizeof(return_data)-1]=-return_data[sizeof(return_data)-1];

  UART3_PutData(return_data, sizeof(return_data));
  Return_LineEnding();
}

void Return_4x1byte(uint8_t com, uint16_t data1, uint16_t data2, uint16_t data3, uint16_t data4)
{
  uint8_t return_data[3+8];   //  데이터 사이즈 틀린듯? - 3/19 발견

  return_data[0]=0xFF;
  return_data[1]=0xFF;
  return_data[2]=sizeof(return_data)-3;           //size
  return_data[3]=com;
  return_data[4]=(uint8_t)data1;                 //data1
  return_data[5]=(uint8_t)data2;
  return_data[6]=(uint8_t)data3;
  return_data[7]=(uint8_t)data4;

  return_data[sizeof(return_data)-1]=0;           //checksum
  for(uint8_t i=0; i<sizeof(return_data)-1; i++) {
    return_data[sizeof(return_data)-1]+=return_data[i];
  }
  return_data[sizeof(return_data)-1]=-return_data[sizeof(return_data)-1];

  UART3_PutData(return_data, sizeof(return_data));
  Return_LineEnding();
}
int check_Report;
void Return_Report_Task(void)
{
  //UART3_Puts("Report\r\n");
  check_Report++;
  uint8_t return_data[3+8];
  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[0]=0xFF;
  return_data[1]=0xFF;
  return_data[2]=sizeof(return_data)-3;           //size
  return_data[3]=START_REPORT;                    //res
  return_data[4]=(uint8_t)(report_cnt>>8);
  return_data[5]=(uint8_t)report_cnt;
  return_data[6]=(uint8_t) ((uint16_t) (WP_Gym.Position[L] *0.1) >>8);
  return_data[7]=(uint8_t) (WP_Gym.Position[L] *0.1);
  return_data[8]=(uint8_t) ((uint16_t) (WP_Gym.Position[R] *0.1) >>8);
  return_data[9]=(uint8_t) (WP_Gym.Position[R] *0.1);
  return_data[10]=0;
  for(uint8_t i=0; i<sizeof(return_data)-1; i++){
    return_data[10]+=return_data[i];
  }
  return_data[10]=-return_data[10];
  UART3_PutData(return_data, sizeof(return_data));
  Return_LineEnding();
}

int check_ReportV2;
void Return_Report_Task_v2(void)
{
  //  UART3_Puts("Reportv2\r\n");
  check_ReportV2++;
  uint8_t return_data[3+12];
  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[0]=0xFF;
  return_data[1]=0xFF;
  return_data[2]=sizeof(return_data)-3;           //size
  return_data[3]=START_REPORT;                    //res
  return_data[4]=(uint8_t) (report_cnt>>8);
  return_data[5]=(uint8_t) report_cnt;
  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[7]=(uint8_t) (WP_Gym.Position[L] *0.1);
  //  return_data[8]=(uint8_t) ((uint16_t) (WP_Gym.Position[R] *0.1) >>8);
  //  return_data[9]=(uint8_t) (WP_Gym.Position[R] *0.1);
  //(아래) 원래 A 단위인데, kg/A 환산하고, 0.01단위로 보내주는 중
  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);
  return_data[14]=0;
  for(uint8_t i=0; i<sizeof(return_data)-1; i++){
    return_data[14]+=return_data[i];
  }
  return_data[14]=-return_data[14];
  UART3_PutData(return_data, sizeof(return_data));
  Return_LineEnding();
}

int check_ReportV3;
void Return_Report_Task_v3(void)
{
  //  UART3_Puts("Reportv2\r\n");
  check_ReportV3++;
  uint8_t return_data[3+14];
  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[0]=0xFF;
  return_data[1]=0xFF;
  return_data[2]=sizeof(return_data)-3;           //size
  return_data[3]=START_REPORT;                    //res
  return_data[4]=(uint8_t) (report_cnt>>8);
  return_data[5]=(uint8_t) report_cnt;
  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[7]=(uint8_t) (WP_Gym.Position[L] *0.1);
  //  return_data[8]=(uint8_t) ((uint16_t) (WP_Gym.Position[R] *0.1) >>8);
  //  return_data[9]=(uint8_t) (WP_Gym.Position[R] *0.1);
  //(아래) 원래 A 단위인데, kg/A 환산하고, 0.01단위로 보내주는 중
  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);
  return_data[14]=(uint8_t) ((uint16_t) (Inv.Ctrl.Vdc * 100) >>8);;
  return_data[15]=(uint8_t) (Inv.Ctrl.Vdc * 100);;
  return_data[16]=0;
  for(uint8_t i=0; i<sizeof(return_data)-1; i++){
    return_data[16]+=return_data[i];
  }
  return_data[16]=-return_data[16];
  UART3_PutData(return_data, sizeof(return_data));
  Return_LineEnding();
}

uint16_t Vdc100;
void Report_Vdc(void)
{
  UART3_Puts("Voltage: ");
  UART3_Puts(Float2String(Inv.Ctrl.Vdc,2));
  UART3_Puts(" [Vdc]\r\n");
  Vdc100 = Inv.Ctrl.Vdc * 100;
  Return_2byte(GET_VOLTAGE, Vdc100);
}
//----------------
uint8_t Flag_SetWeight = 0;
float TargetWeight_L, TargetWeight_R;
float  Step_L, Step_R;
uint16_t Step_Count=0;

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_Stop(void);
void SetWeight_Task(void);

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)
{
  if(Flag_SetWeight)    SetWeight_Stop();

  //check_SetWeight++;
  // 입력인자를 전류지령으로 환산하는 구문
  // 입력된 무게값의 단위는 0.5kg     ex)입력값 255(최대) = 127.5kg

  if(weight_L > 0 && weight_R > 0) {    // 양쪽 구동
    Debug_DriveMotor = 3;
    WP_Ctrl.Mode = 5;
    WP_Ctrl.ActiveMotor = 3;
  } else if(weight_L > 0 && weight_R <= 0) {    // 좌측 구동 (1번)
    Debug_DriveMotor = 1;
    WP_Ctrl.Mode = 5;
    WP_Ctrl.ActiveMotor = 1;
  } else if(weight_L <= 0 && weight_R > 0) {    // 우측 구동 (2번)
    Debug_DriveMotor = 2;
    WP_Ctrl.Mode = 5;
    WP_Ctrl.ActiveMotor = 2;
  } else {                            // 양쪽 off
    Debug_DriveMotor = 0;
    WP_Ctrl.Mode = 0;
    WP_Ctrl.ActiveMotor = 0;
  }

  WP_Gym.WeightMode[L] = mode_L;
  WP_Gym.WeightMode[R] = mode_R;

  Step_L  = TargetWeight_L = weight_L;
  Step_R  = TargetWeight_R = weight_R;
  //  Step_L -= WP_GymManager.TargetWeight_L;
  //  Step_R -= WP_GymManager.TargetWeight_R;
  Step_L -= WP_Gym.WeightSet[L];
  Step_R -= WP_Gym.WeightSet[R];

  Step_L /= step;
  Step_R /= step;
  Step_Count = step;
  Flag_SetWeight=1;
  Task_Start_User(SetWeight_Task, msec);
}

void SetWeight_Stop(void)
{
  if(Flag_SetWeight) {
    Flag_SetWeight = 0;
    Task_Stop_User();
  }
}

void SetWeight_Task(void)
{
  if(Flag_SetWeight) {
    WP_Gym.WeightSet[L] += Step_L;
    WP_Gym.WeightSet[R] += Step_R;

    if(--Step_Count ==0) {
      WP_Gym.WeightSet[L] = TargetWeight_L;
      WP_Gym.WeightSet[R] = TargetWeight_R;
      SetWeight_Stop();
    }
  }
}


//---------------------

void Start_Report(void)
{
  //Task_Start_100ms(Return_Report_Task);

  //  Task_Start_50ms(Return_Report_Task);
//  Task_Start_50ms(Return_Report_Task_v2);
  Task_Start_50ms(Return_Report_Task_v3);

  //  Task_Start_10ms(Return_Report_Task);
}

void Stop_Report(void)
{
  //Task_Stop_100ms();
  Task_Stop_50ms();
}

int Status_VoltRprt;
void VoltRprtToggle(void)
{
  switch (Status_VoltRprt){
    case ON:
      UART3_Puts("\r\n Volt Report Stop \r\n");
      Status_VoltRprt = OFF;
      break;
    case OFF:
      UART3_Puts("\r\n Volt Report Start \r\n");
      Status_VoltRprt = ON;
      break;
  }
}

void Save_Data_to_Flash(void)
{
  Flash_Write(FLASH_ORIGIN_SECTOR);
  Flash_Write(FLASH_ORIGIN_SECTOR);
}

void Factory_Reset(void)
{
  uint8_t i;
  uint32_t *ram_data,*source_data;

  ram_data=(uint32_t*)&RAM_Data;
  source_data=(uint32_t*)&Defualt_RAM_Data;
  for(i=0; i<sizeof(RAM_Data)/4;i++) {
    *ram_data++=*source_data++;
  }
  Save_Data_to_Flash();
}

void System_Reboot(void)
{
  Save_Data_to_Flash();
  Delay_sec(0.1);

  //////////////////////////////////////////////////////////////////////
  DEBUG_printf("REBOOT MODE is ON\r\n");
  for(uint8_t i=0; i<5; i++) {
    HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_2);
    HAL_Delay(200);
  }
  //////////////////////////////////////////////////////////////////////
  HAL_NVIC_SystemReset();
}

void System_Standby(void)
{
  Save_Data_to_Flash();
  Delay_sec(0.1);

  HAL_PWR_EnableWakeUpPin(PWR_WAKEUP_PIN2);
  __HAL_PWR_CLEAR_FLAG(PWR_FLAG_WU);

  //////////////////////////////////////////////////////////////////////
  DEBUG_printf("STANDBY MODE is ON\r\n");
  for(uint8_t i=0; i<5; i++) {
    HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_2);
    HAL_Delay(200);
  }
  //////////////////////////////////////////////////////////////////////
  HAL_PWR_EnterSTANDBYMode();
}

void System_Wakeup(void)
{
  if(__HAL_PWR_GET_FLAG(PWR_FLAG_SB) != RESET) { //when standby mode and press the button
    if(digitalRead(7)) {                //Standby Button
      HAL_PWR_EnterSTANDBYMode();
    }
    __HAL_PWR_CLEAR_FLAG(PWR_FLAG_WU);
    __HAL_PWR_CLEAR_FLAG(PWR_FLAG_SB);
    //////////////////////////////////////////////////////////////////////
    DEBUG_printf("Wake-up from the STANBY MODE\r\n");
    for(uint8_t i=0; i<5; i++) {
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_1);
      HAL_Delay(200);
    }
    //////////////////////////////////////////////////////////////////////
    HAL_PWR_DisableWakeUpPin(PWR_WAKEUP_PIN2);
  }
}

///somebp 2024.02.02
void System_Sleep(void)
{
  Save_Data_to_Flash();

  //////////////////////////////////////////////////////////////////////
  DEBUG_printf("SLEEP MODE is ON\r\n");

  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
  Motor2En(WP_Gate_DIS);                  //

  for(int i=0; i<6; i++) {
    digitalWrite(56, GPIO_PIN_TOGGLE);    //LED1
    Delay_sec(0.3);
  }
  digitalWrite(56, GPIO_PIN_SET);         //Off while Sleeping
  //digitalWrite(56, GPIO_PIN_RESET);       //On  while Sleeping
  //
  // To Do
  //
  //////////////////////////////////////////////////////////////////////
  Delay_sec(0.1);
  HAL_SuspendTick();

  HAL_ADC_DeInit(&hadc1);
  HAL_ADC_DeInit(&hadc2);
  HAL_ADC_DeInit(&hadc3);
  HAL_ADC_MspDeInit(&hadc1);
  HAL_ADC_MspDeInit(&hadc2);
  HAL_ADC_MspDeInit(&hadc3);

  HAL_DAC_DeInit(&hdac);
  HAL_DAC_MspDeInit(&hdac);

  HAL_TIM_PWM_DeInit(&htim1);
  HAL_TIM_PWM_DeInit(&htim8);
  HAL_TIM_PWM_DeInit(&htim5);
  HAL_TIM_PWM_DeInit(&htim9);
  HAL_TIM_Encoder_DeInit(&htim2);
  HAL_TIM_Encoder_DeInit(&htim4);
  HAL_TIM_Base_MspDeInit(&htim1);
  HAL_TIM_Base_MspDeInit(&htim8);
  HAL_TIM_PWM_MspDeInit(&htim5);
  HAL_TIM_PWM_MspDeInit(&htim9);
  HAL_TIM_Encoder_MspDeInit(&htim2);
  HAL_TIM_Encoder_MspDeInit(&htim4);

  HAL_UART_DeInit(&huart2);
  HAL_UART_MspDeInit(&huart2);

  __HAL_RCC_PWR_CLK_ENABLE();

  HAL_PWR_EnterSLEEPMode(PWR_MAINREGULATOR_ON, PWR_SLEEPENTRY_WFI);

  // Wake-up state
  HAL_ResumeTick();
  if(__HAL_UART_GET_IT_SOURCE(&huart3,UART_IT_RXNE))
  {
    //////////////////////////////////////////////////////////////////////
    DEBUG_printf("Wake-up from the SLEEP MODE\r\n");

    for(int i=0; i<6; i++) {
      digitalWrite(56, GPIO_PIN_TOGGLE);  //LED1
      Delay_sec(0.1);
    }
    //
    // To Do
    //
    //////////////////////////////////////////////////////////////////////
  }
  HAL_NVIC_SystemReset();
}
///somebp 2024.02.02


int check_ProtocolRx = 0;
int check_GetPosition, check_Emergency, check_WeightPlus, check_WeightMinus;
int check_Chksum = 0;
int check_Pinst = 0;

void Protocol_Receive_Task_Uart3(void)
{
  check_ProtocolRx++;
  static uint8_t psize=0;
  static uint8_t checksum=0;

  ///somebp 2024.02.19
#define   TIMEOUT         100
  static uint16_t old_available = 0;
  {
    uint16_t available = UART3_Available();

    if(available==0) {
      old_available = 0;
    }
    else {
      static uint32_t old_time = 0;

      if(old_available != available) {
        old_available = available;
        old_time = Get_Time();
      }
      else {
        uint32_t new_time=Get_Time();
        if(new_time - old_time > TIMEOUT) {
          UART3_Clear_Buffer(available);
          old_available = 0;
          psize = 0;
          DEBUG_printf("\r\n[TIMEOUT] Buffer Clear:%d\r\n", available);
        }
      }
    }
  }
  ///somebp 2024.02.19

  if(psize==0) {
    if(UART3_Available()>=3) {
      if( UART3_Getc_NoClear(0)==0xFF && UART3_Getc_NoClear(1)==0xFF
          && UART3_Getc_NoClear(2)!=0xFF && UART3_Getc_NoClear(2)>0x01) {
        psize=UART3_Getc_NoClear(2);
        checksum=(uint8_t)(0xFF + 0xFF + psize);
        UART3_Clear_Buffer(3);
        old_available=UART3_Available();                ///somebp 2024.02.19
        DEBUG_printf("\r\n1. Size:0x%02X\tChecksum:0x%02X\r\n", psize, checksum);
      }
      else {
        UART3_Clear_Buffer(1);
        old_available=UART3_Available();                ///somebp 2024.02.19
      }
    }
  }
  else {
    if(UART3_Available()>=psize) {
      uint8_t pinst;

      for(int i=0; i<psize; i++) {
        checksum += UART3_Getc_NoClear(i);
        DEBUG_printf("%02X\t", UART3_Getc_NoClear(i));
      }
      if(checksum != 0) {
        DEBUG_printf("\r\nChecksum ERROR: 0x%02X\r\n", checksum);
        check_Chksum++;
        UART3_Puts("\r\nChecksum Error");
        if(UART3_Getc_NoClear(0)!=0xFF)   UART3_Clear_Buffer(1);
        old_available=UART3_Available();                ///somebp 2024.02.19
        psize = 0;
        return;
      }

      pinst=UART3_Getc_NoClear(0);
      DEBUG_printf("\r\n2. pinst:0x%02X\r\n",pinst);

      switch(UART3_Getc_NoClear(0)) {   // �ν�Ʈ���ǿ� ���� �ش��ϴ� ��� ����
        case GET_POSITION:
          DEBUG_printf("\r\n Get Position\r\n");
          float send_Pos_L = WP_Gym.Position[L];
          float send_Pos_R = WP_Gym.Position[R];
          UART3_Puts("Position(L/R): ");
          UART3_Puts(Float2String(send_Pos_L,0));
          UART3_Puts(" / ");
          UART3_Puts(Float2String(send_Pos_R,0));
          UART3_Puts(" [mm]\r\n");
          //          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);
          break;
        case GET_VOLTAGE:
          DEBUG_printf("\r\n Get Voltage\r\n");
          //          Return_1byte(GET_VOLTAGE, 0xAA);
          UART3_Puts("Voltage: ");
          UART3_Puts(Float2String(Inv.Ctrl.Vdc,2));
          UART3_Puts(" [Vdc]\r\n");

          Report_Vdc();
          break;
        case BLE_CONNECT:    // 모든 Gym setting (무게값, 모드, 가동 범위 등, 사용 시마다 설정하는 정보)을 초기화
//          무게 관련 상태 업데이트
          Return_4x1byte(WEIGHTMINUS, WP_Gym.WeightSet[L]*4, WP_Gym.WeightMode[L], WP_Gym.WeightSet[R]*4, WP_Gym.WeightMode[R]);
//          전압 정보 업데이트
          Vdc100 = Inv.Ctrl.Vdc * 100;
          Return_2byte(GET_VOLTAGE, Vdc100);
//          무게 On/Off 여부 업데이트
          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");
          //            break;
          //          } else {
          //            WP_Weight.Ctrl.OnOffStatus[L] = OFF;
          //            WP_Weight.Ctrl.OnOffStatus[R] = OFF;
          //            WP_Gym.WeightSet[L] = 0;
          //            WP_Gym.WeightSet[R] = 0;
          //            WP_Gym.WeightMode[L] = 0;
          //            WP_Gym.WeightMode[R] = 0;
          //            WP_Gym.F_EccSet = 0.5;
          //            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.L_RangeMin = 50.0f;
          //            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.RangeHi[R] = 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
          break;
        case START_REPORT:
          DEBUG_printf("\r\n Start Report\r\n");
          Start_Report();
          break;
        case STOP_REPORT:
          DEBUG_printf("\r\n Stop Report\r\n");
          Stop_Report();
          break;
          //        case SET_WEIGHT:      // 썸띵 제공 버전 (HW_v3)
          //          DEBUG_printf("\r\n Set Weight\r\n");
          //          if(psize>=8) {
          //          // �ش� ������ ���Ű��� �����ϴ� ����
          ////          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]);
          //          // ���� ���θ� �����ϴ� ���� �߰�
          //          }
          //          break;
        case VOLT_RPRT_TOGGLE:
          VoltRprtToggle();
          break;
        case SET_WEIGHT:    //  모바일앱 테스트용 임시

          //  모드 변경 : 없는 넘버일 때 에러만 만듦)
          if(UART3_Getc_NoClear(2) > 3 || UART3_Getc_NoClear(4) > 3 ) {
            UART3_Puts("Err: Mode number error\r\n");
//            WP_Gym.WeightModeError = 2;
            break;
          } else{
            //  좌측 무게 적용
            if(UART3_Getc_NoClear(1) * 0.5 >= 60 || UART3_Getc_NoClear(1) * 0.5 < 0){    // 머신 제한 무게랑 연동해야 함!!
              delay_msec(15);
              LED_Blink_ALL(150,10,10,50);
              //            기존 무게가 유지되도록 아무 설정 변경 안 함
              UART3_Puts("Err: Weight_Left\r\n");
              break;
            } else {
              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){    // 머신 제한 무게랑 연동해야 함!!
              delay_msec(15);
              LED_Blink_ALL(150,10,10,50);
              //            기존 무게가 유지되도록 아무 설정 변경 안 함
              UART3_Puts("Err: Weight_Right\r\n");
              break;
            } else {
              WP_Gym.WeightBLE[R] = UART3_Getc_NoClear(3) * 0.5;
            }
            Task_Stop_10ms_1();
            Task_Start_10ms_1(Task10ms_1);
            WP_Gym.WeightMode[L] = UART3_Getc_NoClear(2);
            WP_Gym.WeightMode[R] = UART3_Getc_NoClear(4);
          }

          // 변경 이후 현재 적용 값을 리턴하는 구문
          Return_4x1byte(SET_WEIGHT, WP_Gym.WeightBLE[L], WP_Gym.WeightMode[L], WP_Gym.WeightBLE[R], WP_Gym.WeightMode[R]);

          break;
        case SET_RANGE:   //  갑자기 구간을 바꿔버리면 무게가 확 변경되는 문제 해결해야 함! -> 스케쥴러?
          DEBUG_printf("\r\n Set Range\r\n");
          UART3_Puts("\r\n");
          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("Err: Off Weight First\r\n");
            Return_1byte(SET_RANGE,0);    // Not applied
          } else{
            switch(UART3_Getc_NoClear(1)){
              case L:   // 좌측만
                SetRange(L,UART3_Getc_NoClear(2));
                UART3_Puts("[Range of Motion - L] ");
                UART3_Puts(Float2String(WP_Gym.Region.RangeLo[L],0));
                UART3_Puts("~ ");
                UART3_Puts(Float2String(WP_Gym.Region.RangeHi[L],0));
                UART3_Puts(" (mm)\r\n");
                Return_1byte(SET_RANGE,1);    // applied
                break;
              case R:   // 우측만
                SetRange(R,UART3_Getc_NoClear(2));
                UART3_Puts("[Range of Motion - R] ");
                UART3_Puts(Float2String(WP_Gym.Region.RangeLo[R],0));
                UART3_Puts("~ ");
                UART3_Puts(Float2String(WP_Gym.Region.RangeHi[R],0));
                UART3_Puts(" (mm)\r\n");
                Return_1byte(SET_RANGE,1);    // applied
                break;
              case LR:   // 양쪽
                SetRange(L,UART3_Getc_NoClear(2));
                SetRange(R,UART3_Getc_NoClear(2));
                UART3_Puts("[Range of Motion - L] ");
                UART3_Puts(Float2String(WP_Gym.Region.RangeLo[L],0));
                UART3_Puts("~ ");
                UART3_Puts(Float2String(WP_Gym.Region.RangeHi[L],0));
                UART3_Puts(" (mm)\r\n");
                UART3_Puts("[Range of Motion - R] ");
                UART3_Puts(Float2String(WP_Gym.Region.RangeLo[R],0));
                UART3_Puts("~ ");
                UART3_Puts(Float2String(WP_Gym.Region.RangeHi[R],0));
                UART3_Puts(" (mm)\r\n");
                Return_1byte(SET_RANGE,1);    // applied
                break;
            }
          }
          break;
        case POS_CALIB:
          DEBUG_printf("\r\n Position Calibrate\r\n");
          // �ش� ������ ���Ű��� �����ϴ� ����
          PositionCalibrate(UART3_Getc_NoClear(1));
          //          Return_0byte(POS_CALIB);
          UART3_Puts("Position Calibrated\r\n");
          break;
        case EMERGENCY:                       //  지금은 단계적으로 변경하는 효과가 전혀 없음!! 스케쥴러로 돌려야 함!!
          DEBUG_printf("\r\n Emergency\r\n");
          //          SetWeight(0,0,0,0);
          float Target_L,Target_R;
          Target_L = Motor1.Cmd.Icmd.q;
          Target_R = Motor2.Cmd.Icmd.q;
          UART3_Puts("Emergency Release !!\r\n");
          delay_msec(200);
          UART3_Puts("90% of SetWeight !!\r\n");
          Motor1.Cmd.Icmd.q = Target_L *0.9;     // 90%
          Motor2.Cmd.Icmd.q = Target_R *0.9;
          delay_msec(200);
          UART3_Puts("80% of SetWeight !!\r\n");
          Motor1.Cmd.Icmd.q = Target_L *0.8;     // 80%
          Motor2.Cmd.Icmd.q = Target_R *0.8;
          delay_msec(150);
          UART3_Puts("70% of SetWeight !!\r\n");
          Motor1.Cmd.Icmd.q = Target_L *0.7;     // 70%
          Motor2.Cmd.Icmd.q = Target_R *0.7;
          delay_msec(100);
          UART3_Puts("60% of SetWeight !!\r\n");
          Motor1.Cmd.Icmd.q = Target_L *0.6;     // 60%
          Motor2.Cmd.Icmd.q = Target_R *0.6;
          delay_msec(100);
          UART3_Puts("50% of SetWeight !!\r\n");
          Motor1.Cmd.Icmd.q = Target_L *0.5;     // 50%
          Motor2.Cmd.Icmd.q = Target_R *0.5;
          delay_msec(50);
          UART3_Puts("30% of SetWeight !!\r\n");
          Motor1.Cmd.Icmd.q = Target_L *0.3;     // 30%
          Motor2.Cmd.Icmd.q = Target_R *0.3;
          delay_msec(25);
          UART3_Puts("10% of SetWeight !!\r\n");
          Motor1.Cmd.Icmd.q = Target_L *0.1;     // 10%
          Motor2.Cmd.Icmd.q = Target_R *0.1;
          delay_msec(25);
          UART3_Puts("No Weight !!\r\n");
          UART3_Puts("** Please Reboot RoomFit **\r\n");
          Motor1.Cmd.Icmd.q = 0;     // 0%
          Motor2.Cmd.Icmd.q = 0;
          WP_Gym.WeightSet[L] = 0.0f;
          WP_Gym.WeightSet[R] = 0.0f;
          Debug_DriveMotor = 0;
          check_Emergency++;
          Return_0byte(EMERGENCY);
          break;
        case WEIGHTONOFF:
          switch(UART3_Getc_NoClear(1)){
            case ON:
              if(WP_Weight.Ctrl.OnOffStatus[L] == ON){
                Return_1byte(WEIGHTONOFF, WP_Weight.Ctrl.OnOffStatus[L]);
                UART3_Puts("Weight is already ON\r\n");
              } else {
                // 온으로 변경
                WP_Weight.Ctrl.OnOffStatus[L] = ON;
                WP_Weight.Ctrl.OnOffStatus[R] = ON;
                // 상태 리턴은 WESPION_App.c에서 수행 완료 후 보냄
              }
              break;
            case OFF:
              if(WP_Weight.Ctrl.OnOffStatus[L] == OFF){
                Return_1byte(WEIGHTONOFF, WP_Weight.Ctrl.OnOffStatus[L]);
                UART3_Puts("Weight is already OFF\r\n");
              }else {
                // 오프로 변경
                WP_Weight.Ctrl.OnOffStatus[L] = OFF;
                WP_Weight.Ctrl.OnOffStatus[R] = OFF;
                // 상태 리턴은 WESPION_App.c에서 수행 완료 후 보냄
              }
              break;
          }
          break;
            case WEIGHTPLUS:
//              delay_msec(5);
              LED_Blink_ALL(30,10,40,5);
              switch(UART3_Getc_NoClear(1)){
                case L:
                  switch(UART3_Getc_NoClear(2)){
                    case KG0P5:
                      WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] + 0.25f;
                      if(WP_Gym.WeightBLE[L] >= WP_Machine.MaxCurrent){
                        delay_msec(5);
                        LED_Blink_ALL(150,10,10,50);
                        WP_Gym.WeightBLE[L] = WP_Machine.MaxCurrent;
                      }
                      Task_Stop_10ms_1();
                      Task_Start_10ms_1(Task10ms_1);
                      break;
                    case KG5P0:
                      delay_msec(15);
                      LED_Blink_ALL(40,10,50,10);
                      WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] + 2.5f;
                      if(WP_Gym.WeightBLE[L] >= WP_Machine.MaxCurrent){
                        delay_msec(15);
                        LED_Blink_ALL(150,10,10,50);
                        WP_Gym.WeightBLE[L] = WP_Machine.MaxCurrent;
                      }
                      Task_Stop_10ms_1();
                      Task_Start_10ms_1(Task10ms_1);
                      break;
                  }
                  break;
                    case R:
                      switch(UART3_Getc_NoClear(2)){
                        case KG0P5:
                          WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] + 0.25f;
                          if(WP_Gym.WeightBLE[R] >= WP_Machine.MaxCurrent){
                            delay_msec(5);
                            LED_Blink_ALL(150,10,10,50);
                            WP_Gym.WeightBLE[R] = WP_Machine.MaxCurrent;
                          }
                          Task_Stop_10ms_1();
                          Task_Start_10ms_1(Task10ms_1);
                          break;
                        case KG5P0:
                          delay_msec(15);
                          LED_Blink_ALL(40,10,50,10);
                          WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] + 2.5f;
                          if(WP_Gym.WeightBLE[R] >= WP_Machine.MaxCurrent){
                            delay_msec(15);
                            LED_Blink_ALL(150,10,10,50);
                            WP_Gym.WeightBLE[R] = WP_Machine.MaxCurrent;
                          }
                          Task_Stop_10ms_1();
                          Task_Start_10ms_1(Task10ms_1);
                          break;
                      }
                      break;
                        case LR:
                          switch(UART3_Getc_NoClear(2)){
                            case KG0P5:
                              WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] + 0.25f;
                              WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] + 0.25f;
                              if(WP_Gym.WeightBLE[L] >= WP_Machine.MaxCurrent || WP_Gym.WeightBLE[R] >= WP_Machine.MaxCurrent){
                                delay_msec(15);
                                LED_Blink_ALL(150,10,10,50);
                                WP_Gym.WeightBLE[L] = WP_Machine.MaxCurrent;
                                WP_Gym.WeightBLE[R] = WP_Machine.MaxCurrent;
                              }
                              Task_Stop_10ms_1();
                              Task_Start_10ms_1(Task10ms_1);
                              break;
                            case KG5P0:
                              delay_msec(15);
                              LED_Blink_ALL(40,10,50,10);
                              WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] + 2.5f;
                              WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] + 2.5f;
                              if(WP_Gym.WeightBLE[L] >= WP_Machine.MaxCurrent || WP_Gym.WeightBLE[R] >= WP_Machine.MaxCurrent){
                                delay_msec(15);
                                LED_Blink_ALL(150,10,10,50);
                                WP_Gym.WeightBLE[L] = WP_Machine.MaxCurrent;
                                WP_Gym.WeightBLE[R] = WP_Machine.MaxCurrent;
                              }
                              Task_Stop_10ms_1();
                              Task_Start_10ms_1(Task10ms_1);
                              break;
                          }
                          break;
              }
              break;
                case WEIGHTMINUS:
//                  delay_msec(5);
                  LED_Blink_ALL(10,50,30,10);
                  switch(UART3_Getc_NoClear(1)){
                    case L:
                      switch(UART3_Getc_NoClear(2)){
                        case KG0P5:
                          WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 0.25f;
                          if(WP_Gym.WeightBLE[L] <= 0.0f){
                            delay_msec(5);
                            LED_Blink_ALL(150,10,10,50);
                            WP_Gym.WeightBLE[L] = 0.0f;
                          }
                          Task_Stop_10ms_1();
                          Task_Start_10ms_1(Task10ms_1);
                          break;
                        case KG5P0:
                          delay_msec(15);
                          LED_Blink_ALL(10,80,60,10);
                          WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 2.5f;
                          if(WP_Gym.WeightBLE[L] <= 0.0f){
                            delay_msec(15);
                            LED_Blink_ALL(150,10,10,50);
                            WP_Gym.WeightBLE[L] = 0.0f;
                          }
                          Task_Stop_10ms_1();
                          Task_Start_10ms_1(Task10ms_1);
                          break;
                      }
                      break;
                        case R:
                          switch(UART3_Getc_NoClear(2)){
                            case KG0P5:
                              WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 0.25f;
                              if(WP_Gym.WeightBLE[L] <= 0.0f){
                                delay_msec(15);
                                LED_Blink_ALL(150,10,10,50);
                                WP_Gym.WeightBLE[R] = 0.0f;
                              }
                              Task_Stop_10ms_1();
                              Task_Start_10ms_1(Task10ms_1);
                              break;
                            case KG5P0:
                              delay_msec(15);
                              LED_Blink_ALL(10,80,60,10);
                              WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 2.5f;
                              if(WP_Gym.WeightBLE[L] <= 0.0f){
                                delay_msec(15);
                                LED_Blink_ALL(150,10,10,50);
                                WP_Gym.WeightBLE[R] = 0.0f;
                              }
                              Task_Stop_10ms_1();
                              Task_Start_10ms_1(Task10ms_1);
                              break;
                          }
                          break;
                            case LR:
                              switch(UART3_Getc_NoClear(2)){
                                case KG0P5:
                                  WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 0.25f;
                                  WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] - 0.25f;
                                  if(WP_Gym.WeightBLE[L] <= 0.0f || WP_Gym.WeightBLE[R] <= 0.0f){
                                    delay_msec(15);
                                    LED_Blink_ALL(150,10,10,50);
                                    WP_Gym.WeightBLE[L] = 0.0f;
                                    WP_Gym.WeightBLE[R] = 0.0f;
                                  }
                                  Task_Stop_10ms_1();
                                  Task_Start_10ms_1(Task10ms_1);
                                  break;
                                case KG5P0:
                                  delay_msec(15);
                                  LED_Blink_ALL(10,80,60,10);
                                  WP_Gym.WeightBLE[L] = WP_Gym.WeightBLE[L] - 2.5f;
                                  WP_Gym.WeightBLE[R] = WP_Gym.WeightBLE[R] - 2.5f;
                                  if(WP_Gym.WeightBLE[L] <= 0.0f || WP_Gym.WeightBLE[R] <= 0.0f){
                                    delay_msec(15);
                                    LED_Blink_ALL(150,10,10,50);
                                    WP_Gym.WeightBLE[L] = 0.0f;
                                    WP_Gym.WeightBLE[R] = 0.0f;
                                  }
                                  Task_Stop_10ms_1();
                                  Task_Start_10ms_1(Task10ms_1);
                                  break;
                              }
                              break;
                  }
                  break;
                    case MODE_CHANGE:   // 3/19 업데이트 : MODE_SET으로 명칭 바꿔야 할 듯?
                      if(UART3_Getc_NoClear(1) > 2){    // 여기 2라고 된 걸 실제 모드 부분이랑 연동할 필요?
                        UART3_Puts("Err: Mode command number error\r\n");
                        WP_Gym.WeightModeError = 2;
                      } else{
                        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 first\r\n");
                          WP_Gym.WeightModeError = 1;
//                          break;
                        } else {
                          WP_Gym.WeightMode[L] = UART3_Getc_NoClear(1);
                          WP_Gym.WeightMode[R] = UART3_Getc_NoClear(1);   // 일단 좌측 값을 우측에도 적용
                          //                        WP_Gym.WeightMode[R] = UART3_Getc_NoClear(2);
                          WP_Gym.WeightModeError = 0;

                          //                        if(WP_Gym.WeightMode[L] >= 2) {
                          //                          WP_Gym.WeightMode[L] = 0;
                          //                        } else {
                          //                          WP_Gym.WeightMode[L]++;
                          //                        }
                          //                        if(WP_Gym.WeightMode[R] >= 2) {
                          //                          WP_Gym.WeightMode[R] = 0;
                          //                        } else {
                          //                          WP_Gym.WeightMode[R]++;
                          //                        }

                          switch (WP_Gym.WeightMode[L]) {
                            case 0:
                              UART3_Puts("Mode 0 - Constant\r\n");
                              LED_Weight_R = 8;
                              LED_Weight_G = 4;
                              LED_Weight_B = 80;
                              LED_Blink_ALL(LED_Weight_R, LED_Weight_G, LED_Weight_B,20);
                              break;
                            case 1:
                              LED_Weight_R = 20;
                              LED_Weight_G = 4;
                              LED_Weight_B = 80;
                              LED_Blink_ALL(LED_Weight_R, LED_Weight_G, LED_Weight_B,20);
                              UART3_Puts("Mode 1 - Eccentric: +");
                              UART3_Puts(Float2String(WP_Gym.F_EccSet*2,1));
                              UART3_Puts(" [A]\r\n");
                              break;
                            case 2:
                              LED_Weight_R = 10;
                              LED_Weight_G = 40;
                              LED_Weight_B = 40;
                              LED_Blink_ALL(LED_Weight_R, LED_Weight_G, LED_Weight_B,20);
                              UART3_Puts("Mode 2 - Band\r\n");
                              break;
                          }
                          UART3_Puts("Weight: ");
                          UART3_Puts(Float2String(WP_Gym.WeightSet[L],2));
                          UART3_Puts(" [A] : ");
                          if(WP_Weight.Ctrl.OnOffStatus[L] == ON){
                            UART3_Puts(" ON");
                          } else {
                            UART3_Puts(" OFF");
                          }
                          UART3_Puts(" [A]\r\n");
                        }
                        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]);
                      }

                      break;
                    case ECC_LEVEL:
                      WP_Gym.F_EccSet_Temp[L] = UART3_Getc_NoClear(1) * 0.5;  //  프로토콜 단위 환산
                      WP_Gym.F_EccSet_Temp[R] = UART3_Getc_NoClear(2) * 0.5;
                      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");
                        WP_Gym.F_Ecc_ErrorCode = 1;
                      } else{
//                        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에는 이 공통 변수가 쓰이고 있어서..
                        WP_Gym.F_Ecc_ErrorCode = 0;
                      }

//                      if (WP_Gym.F_EccSet > WP_Gym.F_EccLimit) {
//                        WP_Gym.F_EccSet = 0.5;
//                      } else {
//                        WP_Gym.F_EccSet = WP_Gym.F_EccSet + 0.5;
//                      }

                      UART3_Puts("Eccentric: +");
                      UART3_Puts(Float2String(WP_Gym.F_EccSet * 2,1));
                      UART3_Puts(" [A]\r\n");
//                      int EccSet20 = WP_Gym.F_EccSet * 20;
                      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 단위
                      break;
                    case SET_DATA:
                      DEBUG_printf("\r\n Set Data\r\n");
                      if(psize>5) {
                        uint16_t paddress=((uint16_t)UART3_Getc_NoClear(1)<<8)+(uint8_t)UART3_Getc_NoClear(2);
                        uint8_t data_size=UART3_Getc_NoClear(3);
                        DEBUG_printf(" paddress:0x%04X\tdata_size:%d\r\n", paddress, data_size);
                        if((paddress+data_size)>(sizeof(Data_Form_RAM)-4))  break;

                        for(uint8_t i=0; i<data_size; i++) {
                          *((uint8_t*)&RAM_Data+paddress+i)=UART3_Getc_NoClear(4+i);
                        }

                        MotorCtrl_Initialize();           ///Refresh Variables for Motor & !! Disable Gate Drivers !!
                        Initialize_Machine();
                      }
                      break;
                    case GET_DATA:
                      DEBUG_printf("\r\n Get Data\r\n");
                      if(psize>=5) {
                        uint16_t paddress=((uint16_t)UART3_Getc_NoClear(1)<<8)+(uint8_t)UART3_Getc_NoClear(2);
                        uint8_t data_size=UART3_Getc_NoClear(3);
                        uint8_t buffer[128+5];
                        DEBUG_printf(" paddress:0x%04X\tdata_size:%d\r\n", paddress, data_size);
                        if((paddress+data_size)>(sizeof(Data_Form_RAM)-4))  break;

                        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;
                        for(uint8_t i=0; i<data_size; i++) {
                          buffer[4+i] = *((uint8_t*)&RAM_Data+paddress+i);
                          checksum   += buffer[4+i];
                        }
                        buffer[4+data_size] = -checksum;
                        UART3_PutData(buffer, 5+data_size);
                      }
                      break;
                    case SAVE_DATA_TO_FLASH :
                      DEBUG_printf("\r\n Save Data to Flash\r\n");
                      Save_Data_to_Flash();
                      break;
                    case FAIL_SAFE_ERROR:
                      if(psize>=3) {
                        Error_Flag_Enable = (UART3_Getc_NoClear(1)!=0);
                        if(Error_Flag_Enable)     Return_4byte(FAIL_SAFE_ERROR, Error_Flag);
                      }
                      break;
                    case FACTORY_RESET:
                      DEBUG_printf("\r\n Factory Reset\r\n");
                      Factory_Reset();

                      MotorCtrl_Initialize();           ///Refresh Variables for Motor & !! Disable Gate Drivers !!
                      Initialize_Machine();
                      break;
                    case REBOOT:
                      DEBUG_printf("\r\n System Reboot\r\n");
                      System_Reboot();
                      break;

                      ///somebp 2024.02.02
                      //      case STANDBY:
                      //        DEBUG_printf("\r\n System Standby\r\n");
                      //        System_Standby();
                      //        break;
                    case SLEEP:
                      DEBUG_printf("\r\n System Sleep\r\n");
                      System_Sleep();
                      break;
                      ///somebp 2024.02.02

                    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");
                      {
                        uint8_t *p;
                        if(UART3_Getc_NoClear(1)<4) {
                          switch(UART3_Getc_NoClear(1)) {
                            case FLASH_ORIGIN_SECTOR:     //2
                              UART3_Puts("\r\nOriginal Sector\r\n");
                              p = (uint8_t*)&Origin_Data;
                              break;
                            case FLASH_BACKUP_SECTOR:     //3
                              UART3_Puts("\r\nBackup Sector\r\n");
                              p = (uint8_t*)&Backup_Data;
                              break;
                            case 0:
                              UART3_Puts("\r\nDefault RAM Data\r\n");
                              p = (uint8_t*)&Defualt_RAM_Data;
                              break;
                            case 1:
                              UART3_Puts("\r\nRAM Data\r\n");
                              p = (uint8_t*)&RAM_Data;
                              break;
                          }

                          //Display Dump
                          for(int i=0; i<sizeof(Data_Form_RAM); i++) {
                            if(i%16==0)  UART3_printf("\r\n[%04X] ", i);
                            UART3_printf("%02X ", *p++);
                            delay_msec(0.5);
                          }
                          UART3_Puts("\r\n");
                        }
                        else {
                          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));

                          Delay_sec(0.4);

                          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));

                          Delay_sec(0.4);

                          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("\r\n\r\n");
                        }
                      }
                      break;
      }
      UART3_Clear_Buffer(psize);
      old_available=UART3_Available();                ///somebp 2024.02.19
      psize = 0;
    }
  }
}
/**
 * @brief  Inaccurate Time Delay but NOT use Systick Timer (can be used in Interrupt Service Routine)
 * @param  Count 32bit Count
 */
void _delay(uint32_t Count)
{
  for(volatile uint32_t i=0; i<Count; i++);
}