/* * [임베디드제어Lab] PID/SMC 제어기 토글 버전
 * 기능: 
 *   - 'o' 입력으로 PID와 SMC 제어 모드 전환
 *   - s/S: 제어 시작
 *   - r/R: 시스템 리셋
 *   - 숫자: 목표 속도 설정
 *   - 랜덤 외란(±5) 적용
 */

// ==========================================
// 1. 시뮬레이션 설정
// ==========================================
#define USE_ANTI_WINDUP     1     // 안티와인드업 사용
#define INITIAL_MODE        1     // 1:PID, 0:SMC (초기 모드)

// ==========================================
// 2. 매크로 선언
// ==========================================
#define KP  0.8
#define KI  1.5 
#define KD  0.05

#define SMC_Keq 2.0f
#define SMC_KeqDot 0.4f
#define SMC_Lamda 10.0f
#define SMC_SwitchingGain 1.0f

// ==========================================
// 3. 구조체 정의
// ==========================================
typedef struct {
    float Kp, Ki, Kd;
    float T_s;
    float outMax, outMin;
    float pterm;
    float iterm;
    float dterm;
    float prevError;    
} PID_Controller;

typedef struct {
    float Error;
    float SlidingSurface;
    float ErrorInteg;
    float Equivalent;
    float Switching;
    float Keq, Keqdot;
    float Lamda, SwitchingGain;
    float UpLimit, DownLimit;
    float SamplingTime;
} Sliding_Controller;

// ==========================================
// 전역 변수 선언
// ==========================================
PID_Controller myPID;
Sliding_Controller mySMC;
float currentSpeed = 0.0;
float controlOutput = 0.0;
float targetSpeed = 0.0; 
bool isRunning = false;

// 제어 모드 열거형
enum ControlMode { PID_MODE, SMC_MODE };
ControlMode currentMode;

// ==========================================
// 4. PID 함수
// ==========================================
void PID_Init(PID_Controller *pid, float kp, float ki, float kd, float ts)
{
    pid->Kp = kp; pid->Ki = ki; pid->Kd = kd;
    pid->T_s = ts;
    pid->pterm = 0.0f;
    pid->iterm = 0.0f;
    pid->dterm = 0.0f;
    pid->prevError  = 0.0f;
    pid->outMax = 200.0f; 
    pid->outMin = -200.0f; 
}

void PID_Reset(PID_Controller *pid)
{
    pid->pterm = 0.0f;
    pid->iterm = 0.0f;
    pid->dterm = 0.0f;
    pid->prevError  = 0.0f;
}

float PID_Compute(PID_Controller *pid, float target, float measured) {
    float error, output = 0.0f;
    
    error = target - measured;
    
    pid->pterm = pid->Kp * error;
    pid->iterm += (pid->Ki * error * pid->T_s);

    #if USE_ANTI_WINDUP
        if (pid->iterm > pid->outMax) pid->iterm = pid->outMax;
        else if (pid->iterm < pid->outMin) pid->iterm = pid->outMin;
    #endif

    pid->dterm = pid->Kd * ((error - pid->prevError) / pid->T_s);
    
    output = pid->pterm + pid->iterm + pid->dterm;

    if (output > pid->outMax) output = pid->outMax;
    else if (output < pid->outMin) output = pid->outMin;

    pid->prevError = error;

    return output;
}

// ==========================================
// 5. 슬라이딩 모드 함수
// ==========================================

float Saturation(float val, float limit) {
    if (val > limit) return 1.0;
    else if (val < -limit) return -1.0;
    else return val / limit;
}

void SMC_Init(Sliding_Controller *smc, float Keq, float KeqDot, float Lamda, float SwitchingGain, float Limit, float Tsamp)
{
    smc->Keq = Keq;
    smc->Keqdot = KeqDot;
    smc->Lamda = Lamda;
    smc->SwitchingGain = SwitchingGain;
    smc->UpLimit = Limit;
    smc->DownLimit = -Limit;
    smc->SamplingTime = Tsamp;
}

void SMC_Reset(Sliding_Controller *smc)
{
    smc->Error = 0.0;
    smc->ErrorInteg = 0.0;
    smc->SlidingSurface = 0.0;
    smc->Equivalent = 0.0;
    smc->Switching = 0.0;
}

float SMC_Compute(Sliding_Controller *smc, float Reference, float Feedback)
{
    float error;
    float Output;

    error = Reference - Feedback;
    smc->Error = error;
    smc->ErrorInteg += (error * smc->SamplingTime);

    smc->SlidingSurface = error + (smc->Lamda * smc->ErrorInteg);
    smc->Equivalent = (smc->Keq * smc->Lamda * error) + (smc->Keqdot * Feedback);
    smc->Switching = 10 * Saturation(smc->SlidingSurface, SMC_SwitchingGain);

    Output = smc->Equivalent + smc->Switching;

    return Output;
}

// ==========================================
// 6. 모터 함수 (랜덤 외란 포함)
// ==========================================
float Simulated_DC_Motor_Plant(float input_pwm, float current_speed)
{
    float MOTOR_GAIN = 2.5;    
    float INERTIA_TAU = 0.5;   
    float SAMPLING_TIME = 0.01;
    float steady_state_speed, speed_change = 0.0;
    
    // 랜덤 외란 생성 (-5 ~ +5 범위)
    float disturbance = random(-500, 500) / 100.0f;  // -5.0 ~ 5.0
    
    steady_state_speed = input_pwm * MOTOR_GAIN;
    speed_change = (steady_state_speed - current_speed) * (SAMPLING_TIME / INERTIA_TAU);
    
    return current_speed + speed_change + disturbance;
}

// ==========================================
// 6. 메인 루프
// ==========================================
void setup()
{
    Serial.begin(115200);
    Serial.setTimeout(50);
    randomSeed(millis());
    
    // 제어기 초기화
    PID_Init(&myPID, KP, KI, KD, 0.01);
    SMC_Init(&mySMC, SMC_Keq, SMC_KeqDot, SMC_Lamda, SMC_SwitchingGain, 200.0, 0.01);
    
    // 초기 모드 설정
    currentMode = (INITIAL_MODE == 1) ? PID_MODE : SMC_MODE;
    
    Serial.print("Initial mode: ");
    Serial.println(currentMode == PID_MODE ? "PID" : "SMC");
    Serial.println("Commands: s=start, r=reset, o=switch mode, number=speed");
}

void loop()
{
    // 입력 처리
    if (Serial.available() > 0)
    {
        char peekChar = Serial.peek();
        if (isDigit(peekChar) || peekChar == '-')
        {
            targetSpeed = Serial.parseFloat();
            while(Serial.available()) Serial.read(); 
        }
        else
        {
            char cmd = Serial.read();
            if (cmd == 's' || cmd == 'S') {
                isRunning = true;
                if(targetSpeed == 0) targetSpeed = 100.0;
            } 
            else if (cmd == 'r' || cmd == 'R') {
                isRunning = false;
                targetSpeed = 0.0;
                currentSpeed = 0.0;
                controlOutput = 0.0;
                PID_Reset(&myPID);
                SMC_Reset(&mySMC);
            }
            else if(cmd == 'o' || cmd =='O') {
              currentMode = (currentMode == PID_MODE) ? SMC_MODE : PID_MODE;
            }
        }
    }
    else
    {
      // NOP : Not operation
    }

    if (isRunning)
    {
        // 모드별 제어기 실행
        if (currentMode == PID_MODE)
        {
            controlOutput = PID_Compute(&myPID, targetSpeed, currentSpeed);
            currentSpeed = Simulated_DC_Motor_Plant(controlOutput, currentSpeed);
        }
        else // SMC_MODE
        {
            controlOutput = SMC_Compute(&mySMC, targetSpeed, currentSpeed);
            currentSpeed = Simulated_DC_Motor_Plant(controlOutput, currentSpeed);
        }
    }
    else
    {
        controlOutput = 0.0;
        currentSpeed = 0.0;
    }

    // 그래프 출력
    Serial.print("Target:");
    Serial.print(targetSpeed);
    
    Serial.print(", Actual:");
    Serial.print(currentSpeed);
    
    Serial.print(", Error:");
    Serial.println(currentMode == PID_MODE ? myPID.prevError : mySMC.Error);
    
    delay(10);
}