/**
    @copyright None
    @file      Control.c
    @author    Comment Vivre
    @date      2025-11-01
    @brief     None
*/
#include <MyProject.h>


/**
    @function     Ref_Ramp
    @brief        控制量步进控制
    @date         2025-11-08
*/
void Ref_Ramp(void)
{
    if ( motorControl.ActualRef < ( motorControl.TargetRef - motorControl.RampInc))
    { motorControl.ActualRef += motorControl.RampInc; }
    else if (motorControl.ActualRef > ( motorControl.TargetRef + motorControl.RampDec))
    { motorControl.ActualRef -= motorControl.RampDec; }
    else
    { motorControl.ActualRef = motorControl.TargetRef; }
}


/**
    @function     Loop_Control
    @brief        环路控制
    @date         2025-11-08
*/
void Loop_Control(void)
{
    if ((mcState == mcRun) || (mcState == mcStop))
    {
        switch (motorControl.LoopState)
        {
            case OPEN_MODE:
            {
                if (motorControl.ActualSpeed > MOTOR_LOOP_RPM)
                {
                    motorControl.LoopTime ++;
                    
                    if (motorControl.LoopTime > 15)
                    {
                        motorControl.ISRef =  FOC_IQREF;
                        #if (LOOP_MODE == SPEED_CONTROL_MODE)
                        motorControl.ActualRef = motorControl.ActualSpeed + _Q15(200.0 / MOTOR_SPEED_BASE);
                        #endif
                        motorControl.LoopTime = 10;
                        motorControl.LoopState = 1;
                        PI0_UKH = motorControl.ISRef;
                    }
                }
            }
            break;
            
            case CLOSE_MODE:
            {
                motorControl.LoopTime++;
                
                if (motorControl.LoopTime > LOOP_CYCLE)
                {
                    #if (OUT_LOOP_CONTROL)
                    {
                        Ref_Ramp();
                        #if (LOOP_MODE == SPEED_CONTROL_MODE)
                        {
                        
                        
                            // 速度外环
                            motorControl.ISRef = HW_Zero_Calc(motorControl.ActualRef - motorControl.ActualSpeed);
                            FOC_IQREF = motorControl.ISRef;
                            
                            #if (FiledWeakenCompEnable)
                            {
                                FiledWeakenControl(FOC__UD, FOC__UQ, udc.WeakenUsRef, motorControl.ISRef);
                                
                                if (mcFieldWeaken.mcIdref < ID_Limit)
                                {mcFieldWeaken.mcIdref = ID_Limit;}
                                
                                FOC_IQREF = mcFieldWeaken.mcIdref;
                                FOC_IDREF = mcFieldWeaken.mcIqref;
                            }
                            #else
                            {
                                FOC_IQREF = motorControl.ISRef;
                                FOC_IDREF = 0;
                            }
                            #endif
                        }
                        #endif
                        
                    }
                    #else
                    {
                        // 纯电流环
                        motorControl.ActualQOutValue = FOC_IQREF;
                    
                        if (motorControl.ActualQOutValue < (motorControl.QOutRef - QOUTINC))
                        { motorControl.ActualQOutValue = motorControl.QOutRef + QOUTINC; }
                        else if (motorControl.ActualQOutValue > (motorControl.QOutRef + QOUTINC))
                        { motorControl.ActualQOutValue = motorControl.QOutRef - QOUTINC; }
                        else
                        { motorControl.ActualQOutValue = motorControl.QOutRef; }
                    
                        FOC_IQREF = motorControl.ActualQOutValue;
                    
                    }
                    #endif
                    motorControl.LoopTime = 0;
                }
            }
            break;
        }
    }
}

/**
    @function     Get_Target_Ref
    @brief        速度给定
    @date         2025-11-01
*/
void Get_Target_Ref(void)
{
    if (powerControl.PowerSate == POWER_RUN)
    {
        #if (CONTROL_MODE == NONEMODE)
        isCtrlPowOn = true;
        motorControl.TargetRef = TARGET_SPEED_SET;
        motorControl.QOutRef = I_Value(2.0);
        #elif (CONTROL_MODE == UARTMODE)
        #endif
    }
}