#include <MyProject.h>

uint8 data g_1mTick = 0;


/**
    @brief        参考电压，偏置电压配置
    @date         2022-07-14
*/
void VREFConfigInit(void)
{
    // 配置参考电压为5V
    SetReg(VREF_VHALF_CR, VRVSEL1 | VRVSEL0, VRVSEL0);
    // 配置半电压为2.5V
    SetBit(VREF_VHALF_CR, VHALFSEL1 | VHALFSEL0);
    #if (VREF_OUT_EN)
    SetBit(P3_AN, PIN5);
    SetBit(P3_OE, PIN5);
    #endif
    #if (VHALF_EN)
    SetBit(P3_AN, P32);
    SetBit(VREF_VHALF_CR, VHALFEN);
    #endif
    SetBit(VREF_VHALF_CR, VREFEN);
}
/**
    @brief     硬件初始化，初始化需要使用的硬件设备配置，FOC必须配置的是运放电压、运放初始化、ADC初始化、Driver初始化
              其他的可根据实际需求加。
    @date      2022-07-14
*/
void HardwareInit(void)
{
    VREFConfigInit();
    CMP3_Init();
    GPIO_Init();
    ADC_Init();
    Driver_Init();
    AMP_Init();
    Timer3_Init();
	
    CMP3_Interrupt_Init();
	
    ClrBit(IP2, PSYSTICK1 | PSYSTICK0);
    SetBit(DRV_SR, SYSTIE);
    EA = 1;
}



/**
    @brief        部分变量初始化，上电运行一次
    @date         2022-07-14
*/
void SoftwareInit(void)
{
    memset(&mcFocCtrl, 0, sizeof(FOCCTRL));
    /* -----保护----- */
    mcFaultSource = 0;
    /*****电机状态机时序变量***********/
    McStaSet.SetMode = 0;
    mcCurOffset.IuOffsetSum = 16383;
    mcCurOffset.IvOffsetSum = 16383;
    mcCurOffset.Iw_busOffsetSum = 16383;
    mcState = mcReady;
    mcFaultSource   = FaultNoSource;
}



void main(void)
{
    uint16 PowerUpCnt = 0;
    for (PowerUpCnt = 0; PowerUpCnt < 55000; PowerUpCnt++);
    
    SoftwareInit();
    HardwareInit();
    
    while (1)
    {
        if (!mcCurOffset.OffsetFlag)
        {
            GetCurrentOffset();
        }
        else
        {
            MC_Control();
            
            if (g_1mTick)
            {
                if ((mcState != mcInit) && (mcState != mcReady))
                {
                    mcFocCtrl.SpeedFlt  = LPF_Zero_Update(FOC__EOME, mcFocCtrl.SpeedFlt, 30);
                }
                else
                {
                    mcFocCtrl.SpeedFlt = 0;
                }
                
                SetBit(ADC_CR, ADCBSY);
                Power_Currt = (ADC7_DR);
                Power_Currt   = Abs_F16(Power_Currt - mcCurOffset.Iw_busOffset);
                mcFocCtrl.mcADCCurrentbus  = LPF_Zero_Update(Power_Currt << 2, mcFocCtrl.mcADCCurrentbus, 20);
                
                if (mcState != mcRun)
                {
                    mcFocCtrl.mcDcbusFlt = LPF_Zero_Update(ADC2_DR, mcFocCtrl.mcDcbusFlt, 50);
                }
                else
                {
                    mcFocCtrl.mcDcbusFlt = FOC__UDCFLT;
                }
                
                TargetRef_Process();
                Speed_response();
                ATORamp();
                g_1mTick = 0;
            }
        }
    }
}