/********************************************************************************

 **** Copyright (C), 2018, Fortior Technology Co., Ltd.                      ****

 ********************************************************************************
    File Name     : FocControl.c
    Author        : Bruce HW&RD
    Date          : 2018-12-27
    Description   : .C file function description
    Version       : 1.0
    Function List :

    Record        :
    1.Date        : 2018-12-27
     Author      : Bruce HW&RD
     Modification: Created file

********************************************************************************/

#include "FU68xx_5.h"
#include <Myproject.h>



/* Private variables --------------------------------------------------------*/
MotStaType   mcState;
MotStaM      McStaSet;
PFCStaType   PFCState;
uint8  chushihuaflag = 0;
extern uint16 PFCoffsetflag, PFCoffset;


/*---------------------------------------------------------------------------*/
/*  Name     :   void MC_Control(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 电机状态机函数，包括初始化、预充电、顺风逆风判断、预定位、启动、运行、故障等
    /*---------------------------------------------------------------------------*/
void MC_Control(void)
{
    switch (mcState)
    {
        case mcReady:
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else
            {
                Motor_Ready();
                
                if ((mcCurOffset.OffsetFlag == 1) && (mcSpeedRamp.FlagONOFF == 1) && (mcSpeedRamp.TargetValue != 0))
                {
                    mcState = mcInit;
                    ClrBit(DRV_CR, DRVEN);
                    _nop_(); _nop_(); _nop_(); _nop_();
                    SetBit(DRV_CR, DRVEN);
                }
            }
            
            break;
            
        case mcInit:           // 初始化状态，进入mcCharge状态
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else
            {
                if ((mcFocCtrl.mcDcbusFlt > _Q15(Under_Protect_Voltage / HW_BOARD_VOLT_MAX)))
                {
                    Motor_Init();
                    //                                  mcState                 = mcCharge;     // 跳入mcCharge状态
                    //                                  mcFocCtrl.State_Count   = Charge_Time;  // 预充电状态时间设置
                    mcState                   = mcAlign;
                    mcFocCtrl.State_Count     = Align_Time;
                }
            }
            
            break;
            
        case mcCharge: // 预充电状态，MCU输出固定频率占空比，预充电结束后，跳入mcTailWind
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else if (mcSpeedRamp.FlagONOFF == 0 || mcSpeedRamp.TargetValue == 0)
            {
                mcState = mcStop;
            }
            else
            {
                Motor_Charge();
                
                if (mcFocCtrl.State_Count == 0)
                {
                    MOE = 0;                   // 关闭输出
                    mcState                   = mcAlign;
                    mcFocCtrl.State_Count     = Align_Time;
                }
            }
            
            break;
            
        case mcAlign:  // 预定位时间结束后，直接启动; AlignTestMode=1用于初始位置检测调试用
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else if (mcSpeedRamp.FlagONOFF == 0 || mcSpeedRamp.TargetValue == 0)
            {
                mcState               = mcStop;
            }
            else
            {
                Motor_Align();
                #if (AlignTestMode == 1)
                {
                    while (1);
                }
                #endif
            }
            
            break;
            
        case mcStart:   // 配置电机启动参数，进入mcRun状态。
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else if (mcSpeedRamp.FlagONOFF == 0 || mcSpeedRamp.TargetValue == 0)
            {
                mcState               = mcStop;
            }
            else
            {
                Motor_Open();
            }
            
            break;
            
        case mcRun:     // 运行状态，若运行状态的给定变为0，进入mcStop状态。
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else if (mcSpeedRamp.FlagONOFF == 0 || mcSpeedRamp.TargetValue == 0)
            {
                mcState = mcStop;
                mcFocCtrl.State_Count = 30000;
            }
            else
            {}
            
            break;
            
        case mcStop:
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else
            {
                //                            PFCoffsetflag=0;
                //                              if ((mcFocCtrl.SpeedFlt < _Q15(1800/ MOTOR_SPEED_BASE))||(mcFocCtrl.State_Count == 0))
                {
                    #if (StopBrakeFlag == 0)
                    {
                        mcState = mcReady;
                        FOC_CR1 = 0x00;
                        ClrBit(DRV_CR, FOCEN);   //关闭FOC
                        MOE = 0;
                    }
                    #else
                    
                    if (mcFocCtrl.SpeedFlt < Motor_Stop_Speed)
                    {
                        MOE = 0;
                        FOC_CR1 = 0x00;
                        ClrBit(DRV_CR, FOCEN);
                        DRV_DR   = DRV_ARR + 1;
                        DRV_CMR  = 0x00;
                        DRV_CMR |= 0x015;     // 三相下桥臂通，刹车
                        ClrBit(DRV_CR, OCS);  // OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
                        MOE = 1;
                        mcState  = mcBrake;
                        mcFocCtrl.State_Count = StopWaitTime;
                    }
                    
                    #endif
                }
            }
            
            break;
            
        case mcBrake:
            if (mcFaultSource != FaultNoSource)
            {
                mcState = mcFault;
            }
            else if (mcSpeedRamp.FlagONOFF == true)
            {
                mcState                 = mcReady;
                mcFocCtrl.State_Count   = 0;      //单位：1ms  强制关机时间
            }
            else if (mcFocCtrl.State_Count == 0)
            {
                mcState = mcReady;
                MOE = 0;
                ClrBit(DRV_CR, FOCEN);
            }
            
            break;
            
        case mcFault:
            if (mcFaultSource == FaultNoSource)
            {
                mcState = mcReady;
            }
            else
            {}
            
            break;
            
        default:
            mcState = mcReady;
            break;
    }
}

#if (PFCEnable==1)
void PFCRUN_Control(void)
{
    switch (PFCState)
    {
        case PFCReady:
            if (PFCFaultSource != PFCFaultNoSource)
            {
                PFCState = PFCFault;
            }
            else
            {
                memset(&PFCSet, 0, sizeof(PFCValue));
                PFCFaultSource = 0;
                chushihuaflag = 0;
                PFCState = PFCInt;
            }
            
            break;
            
        case PFCInt:
            if (PFCFaultSource != PFCFaultNoSource)
            {
                PFCState = PFCFault;
            }
            else
            {
                if (mcSpeedRamp.FlagONOFF == 1)
                {
                    if (chushihuaflag == 0)
                    {
                        PFC_init();
                        PFC_start();
                        chushihuaflag = 1;
                        UDC_REF       = mcFocCtrl.mcDcbusFlt + _Q15(5.0 / HW_BOARD_VOLT_MAX);
                    }
                    
                    if ((PFCSet.OffsetFlag == 1) && (mcFocCtrl.CtrlMode == 1) && (PFCCONTROL.FlagONOFF == 1) && (chushihuaflag == 1))
                    {
                        ClrBit(ADC_MASK, CH1EN | CH12EN);
                        ClrBit(PFC_CR0, PFCEN);    // PFC使能
                        ClrBit(PFC_CR0, PFCOE);    //输出使能
                        SetBit(PFC_CR0, PFCEN);    // PFC使能
                        SetBit(PFC_CR0, PFCOE);    //输出使能
                        UDC_UKMAX   = PFC_UOUTMAX; // 外环输出最大值
                        PFCState = PFCRun;
                    }
                    
                    //                           else
                    //                           {
                    //                                  PFCState  = PFCStop;
                    //                           }
                }
                
                if ((mcSpeedRamp.FlagONOFF == 0))
                {
                    PFCState  = PFCStop;
                }
            }
            
            break;
            
        case PFCRun:
            if (PFCFaultSource != PFCFaultNoSource)
            {
                PFCState = PFCFault;
            }
            else
            {
                if (PFCCONTROL.FlagONOFF == 0)
                {
                    PFCState = PFCStop;
                }
            }
            
            break;
            
        case PFCStop:
            if (PFCFaultSource != PFCFaultNoSource)
            {
                PFCState = PFCFault;
            }
            else
            {
                if (IAC_UKMAX > 11)
                {
                    PFCSet.IACOUTMax = IAC_UKMAX;
                    PFCSet.IACOUTMax -= 10;
                    IAC_UKMAX       = PFCSet.IACOUTMax;
                }
                
                if (IAC_UKMAX <= 11 || mcState != mcRun)
                {
                    ClrBit(PFC_CR0, PFCOE); //关闭PFC输出
                    ClrBit(PFC_CR0, PFCEN); //关闭PFC使能
                    PFCState = PFCReady;
                }
            }
            
            break;
            
        case PFCFault:
            if (PFCFaultSource == PFCFaultNoSource)
            {
                PFCState = PFCReady;
            }
            else
            {
            }
            
            break;
            
        default:
            PFCState = PFCReady;
            break;
    }
}
#endif