#include <MyProject.h>

CurrentOffset      xdata mcCurOffset;  ///< 电流采样偏置电压采集缓存

/**
    @brief      对FOC的相关寄存器进行配置,先清理寄存器，后配置，最后使能
    @exception  初始化FOC，需要先关闭DRVIVER，对FOC寄存器配置完成后，使能FOC再打开DRVIVER,否则可能导致第一拍电流采样错误
    @date       2022-07-14
*/
void FOC_Init(void)
{
    DRV_CMR         = 0x0ABF;  // UH/VH/WH UL/VL/WL 互补并使能
    /* 使能FOC */
    ClrBit(DRV_CR, DRVEN);                                // 关闭DRVIVER 计时器,防止第一拍采样出错
    ClrBit(DRV_CR, FOCEN);
    SetBit(DRV_CR, FOCEN);
    ClrBit(FOC_CR0, MERRS1);
    SetBit(FOC_CR0, MERRS0);
    FOC_EOMEKLPF    = _Q8(1.0);                            // FOC内部 速度滤波系数
    /* 配置FOC寄存器 */
    FOC_CR1         = 0;                                   // 清零 FOC_CR1
    FOC_CR2         = 0;                                   // 清零 FOC_CR2
    FOC_IDREF       = 0;                                   // 清零 Id
    FOC_IQREF       = 0;                                   // 清零 Iq
    FOC__THETA      = 0;                                   // 清零 角度
    FOC_RTHEACC     = 0;                                   // 清零 爬坡函数的初始加速度
    FOC__RTHESTEP   = 0;                                   // 清零 爬坡速度
    FOC_RTHECNT     = 0;                                   // 清零 爬坡次数
    FOC_THECOMP     = _Q15(0.0 / 180.0);                   // SMO 估算补偿角
    FOC_THECOR      = 0x04;                                // 误差角度补偿
    /* 电流环参数配置 */
    FOC_DMAX        = DOUTMAX;
    FOC_DMIN        = DOUTMIN;
    FOC_QMAX        = QOUTMAX;
    FOC_QMIN        = QOUTMIN;
    /* 位置估算参数配置 */
    FOC_EK1         = OBS_K1T;
    FOC_EK2         = OBS_K2T;
    FOC_EK3         = OBS_K3T;
    FOC_EK4         = OBS_K4T;
    /* -----AO/PLL/SMO ----- */
    #if (EstimateAlgorithm == AO)
    {
        ClrBit(FOC_CR2, ESEL);
        FOC_KSLIDE      = OBS_KSLIDE;
        FOC_EKLPFMIN    = OBS_EA_KS;
        SetBit(FOC_CR0, ESCMS);
        SetBit(FOC_CR3, MFP_EN);
    }
    #elif (EstimateAlgorithm == SMO)
    {
        ClrBit(FOC_CR2, ESEL);
        FOC_KSLIDE      = OBS_KSLIDE;
        FOC_EKLPFMIN    = OBS_EA_KS;
    }
    #elif (EstimateAlgorithm == PLL)
    {
        SetBit(FOC_CR2, ESEL);
        FOC_KSLIDE      = OBSE_PLLKP_GAIN;
        FOC_EKLPFMIN    = OBSE_PLLKI_GAIN;
    }
    #endif
    FOC_FBASE       = OBS_FBASE;                            // 由速度计算角度增量的系数
    FOC_OMEKLPF     = SPEED_KLPF;                           // 估算器内速度低通滤波系数
    FOC_TGLI        = PWM_TGLI_LOAD;                        // 死区配置
    SetBit(FOC_CR1, SVPWMEN);                               // SVPWM模式
    #if (IR_MODE == CCW)
    SetBit(DRV_CR, DDIR);
    #endif
    #if (OverModulation)
    SetBit(FOC_CR1, OVMDL);
    #endif
    /* 单电阻采样； 需要最小采样窗,FOC_TRGDLY为0，七段式SVPWM方式 */
    #if (Shunt_Resistor_Mode == Single_Resistor)
    {
        SetReg(FOC_CR1, CSM0 | CSM1, 0x00);
        FOC_TSMIN  = PWM_TS_LOAD;                           // 最小采样窗口
        FOC_TRGDLY = 0x0c;                                  // 采样时刻在中点，一般考虑开关噪声影响，会设置延迟；
        // 如：0x0c表示延迟12个clock，提前用反码形式，如0x84表示提前12个clock。
        ClrBit(FOC_CR2, F5SEG);                             // 7段式，单电阻仅支持7段式
    }
    /* 双电阻采样，可设置死区补偿值，在下降沿结束前开始采样Ia Ib */
    #elif (Shunt_Resistor_Mode == Double_Resistor)          // double resistor sample
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0);
        FOC_TSMIN       = PWM_DT_LOAD;                      // 死区补偿值
        FOC_TRGDLY      = 0x07;                             // ADC采样的时刻，采样时刻在计数器零点附近与单电阻不同
        // 如：0x83为下降沿结束前3个clock采样Ia 0x01为上升沿开始后第一个clock开始采样。根据实际情况调整。
        FOC_TBLO        = PWM_DLOWL_TIME;                   // 下桥臂最小脉冲，保证采样
        /*五段式或七段式选择*/
        #if (SVPMW_Mode == SVPWM_7_Segment)
        {
            ClrBit(FOC_CR2, F5SEG);                         // 7段式
        }
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        {
            SetBit(FOC_CR2, F5SEG);                         // 5段式
        }
        #endif
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        {
            ClrBit(FOC_CR2, DSS);                           // 7段式
        }
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        {
            SetBit(FOC_CR2, DSS);                           // 5段式
        }
        #endif //end DouRes_Sample_Mode
    }
    /*三电阻采样*/
    #elif (Shunt_Resistor_Mode == Three_Resistor)           // signel resistor sample
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0 | CSM1);          // 三电阻
        FOC_TSMIN  = PWM_DT_LOAD;                           // 死区补偿值
        FOC_TRGDLY = 0x06;                                  // ADC采样的时刻，采样时刻在计数器零点附近与单电阻不同。
        // 如：0x83为下降沿结束前3个clock采样Ia，0x01为上升沿开始后第一个clock开始采样。根据实际情况调整。
        FOC_TBLO = PWM_OVERMODULE_TIME;                     // 过调制电流采样处理的TB脉宽
        /* 五段式或七段式选择 */
        #if (SVPMW_Mode == SVPWM_7_Segment)
        {
            ClrBit(FOC_CR2, F5SEG);                         // 7段式
        }
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        {
            SetBit(FOC_CR2, F5SEG);                         // 5段式
        }
        #endif // end SVPMW_Mode
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        {
            ClrBit(FOC_CR2, DSS);                           // 7段式
        }
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        {
            SetBit(FOC_CR2, DSS);                           // 5段式
        }
        #endif // end DouRes_Sample_Mode
    }
    #endif
    #if (Shunt_Resistor_Mode == Single_Resistor)    // 单电阻校正
    {
        /*set ibus current sample offset*/
        SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
        FOC_CSO = mcCurOffset.Iw_busOffset;         // 写入Ibus的偏置
    }
    #elif (Shunt_Resistor_Mode == Double_Resistor)  // 双电阻校正
    {
        /*set ia, ib current sample offset*/
        SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
        FOC_CSO  = mcCurOffset.IuOffset;            // 写入IA的偏置
        SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
        FOC_CSO  = mcCurOffset.IvOffset;            // 写入IB的偏置
    }
    #elif (Shunt_Resistor_Mode == Three_Resistor)   // 三电阻校正
    {
        /*set ibus current sample offset*/
        SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
        FOC_CSO = mcCurOffset.IuOffset;             // 写入IA的偏置
        SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
        FOC_CSO = mcCurOffset.IvOffset;             // 写入IB的偏置
        SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
        FOC_CSO = mcCurOffset.Iw_busOffset;         // 写入IC的偏置
    }
    #endif
    /*  -------------------------------------------------------------------------------------------------
        DRV_CTL：PWM来源选择
        OCS = 0, DRV_COMR
        OCS = 1, FOC/SVPWM/SPWM
        -------------------------------------------------------------------------------------------------*/
    /*计数器比较值来源FOC*/
    SetBit(DRV_CR, DRVEN);  // 计数器使能      0-->Disable 1-->Enable
    SetBit(DRV_CR, OCS);
}


/**
    @brief     预充电分三步，第一步是对U相进行预充电，第二步是对V两相进行预充电;第三步是对W三相进行预充电。
    @brief     低压应用一般不需要预充电，在customer.h禁止预充电功能寄即可
    @exception 在低功率高压应用中，过流值较小，充电电流过大可能导致触发硬件过流, 需要注意检查
    @date      2022-07-14
*/
void Motor_Charge(void)
{
    if (McStaSet.SetFlag.ChargeSetFlag == 0)
    {
        McStaSet.SetFlag.ChargeSetFlag = 1;
        SetBit(DRV_CR, DRVEN);  // 计数器使能      0-->Disable 1-->Enable
        DRV_DR = CHARGE_DUTY * DRV_ARR;              //下桥臂10% duty
        DRV_CMR &= 0xffc0;
        /*  -------------------------------------------------------------------------------------------------
            DRV_CTL：PWM来源选择
            OCS = 0, DRV_COMR
            OCS = 1, FOC/SVPWM/SPWM
            -------------------------------------------------------------------------------------------------*/
        ClrBit(DRV_CR, OCS);
        mcFocCtrl.ChargeStep = 0;
    }
    
    if ((mcFocCtrl.State_Count < CHARGE_TIME) && (mcFocCtrl.ChargeStep == 0))
    {
        mcFocCtrl.ChargeStep = 1;
        DRV_CMR |= 0x01;                          // U相下桥臂通
        MOE = 1;
    }
    
    if (( mcFocCtrl.State_Count <= (CHARGE_TIME << 1) / 3) && (mcFocCtrl.ChargeStep == 1))
    {
        mcFocCtrl.ChargeStep = 2;
        DRV_CMR |= 0x04;                         // V相下桥臂通
    }
    
    if ((mcFocCtrl.State_Count <= CHARGE_TIME / 3) && (mcFocCtrl.ChargeStep == 2))
    {
        mcFocCtrl.ChargeStep = 3;
        DRV_CMR |= 0x10;                          // W相下桥臂通
    }
}


/**
    @brief     开启次功能启动时候会将电机强拉到 设定角度，之后再启动
    @date      2022-07-14
*/
void Motor_Align(void)
{
    if (McStaSet.SetFlag.AlignSetFlag == 0)
    {
        McStaSet.SetFlag.AlignSetFlag = 1;
        /* -----FOC初始化----- */
        FOC_Init();
        /* 配置预定位的电流、KP、KI */
        FOC_IDREF   = ID_Align_CURRENT;
        FOC_IQREF   = IQ_Align_CURRENT;
        FOC_DKP     = DQKP_Alignment;
        FOC_DKI     = DQKI_Alignment;
        FOC_QKP     = DQKP_Alignment;
        FOC_QKI     = DQKI_Alignment;
        FOC_EKP     = OBSW_KP_GAIN_START;
        FOC_EKI     = OBSW_KI_GAIN_START;
        SetBit(FOC_CR2, UDD);
        // 配置预定位角度
        FOC__THETA  = Align_Angle1;
        #if (EstimateAlgorithm == SMO )
        FOC__ETHETA   = FOC__THETA - 4096;
        #else
        FOC__ETHETA   = FOC__THETA;
        #endif
        // DRV_CMR |= 0x3F;                         // U、V、W相输出
        MOE = 1;
    }
}



/**
    @brief     静止启动配置函数
    @date      2022-07-14
*/
void Motor_Static_Open(void)
{
    FOC_Init();
    MOE = 1;
    FOC__THETA = mcFocCtrl.AngleStart;
    FOC_IDREF = ID_Start_CURRENT;                         // D轴启动电流
    FOC_DKP = DKPStart;
    FOC_DKI = DKIStart;
    FOC_QKP = QKPStart;
    FOC_QKI = QKIStart;
    FOC_EKP = OBSW_KP_GAIN_START;
    FOC_EKI = OBSW_KI_GAIN_START;
    /*启动方式选择*/
    #if (Open_Start_Mode == Omega_Start)              // Omega 启动
    {
        FOC_EFREQACC        = MOTOR_OMEGA_RAMP_ACC;
        FOC_EFREQMIN        = MOTOR_OMEGA_RAMP_MIN;
        FOC_EFREQHOLD       = MOTOR_OMEGA_RAMP_END;
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | ANGM);
    }
    #elif (Open_Start_Mode == Open_Start)
    {
        FOC_RTHEACC         = MOTOR_OPEN_RAMP_ACC;      // 爬坡函数的初始加速度
        FOC__RTHESTEP       = MOTOR_OPEN_RAMP_MIN;      // 0.62 degree acce speed
        FOC_RTHECNT         = MOTOR_OPEN_RAMP_CNT;       // acce time
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, RFAE);
    }
    #elif (Open_Start_Mode == Open_Omega_Start)
    {
        FOC_RTHEACC         = MOTOR_OPEN_RAMP_ACC;      // 爬坡函数的初始加速度
        FOC__RTHESTEP       = MOTOR_OPEN_RAMP_MIN;      // 0.62 degree acce speed
        FOC_RTHECNT         = MOTOR_OPEN_RAMP_CNT;       // acce time
        FOC_EFREQACC        = Motor_OMEGA_RAMP_ACC;
        FOC_EFREQMIN        = MOTOR_OMEGA_RAMP_MIN;
        FOC_EFREQHOLD       = MOTOR_OMEGA_RAMP_END;
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | RFAE | ANGM);
    }
    #endif //end Open_Start_Mode
    /*不同启动方式下，切换到MCRUN状态*/
    #if (Open_Start_Mode == Open_Start)         //OPEN状态启动时拖动多次
    {
        mcFocCtrl.State_Count   = 0;
        FOC_EKP                 = OBSW_KP_GAIN_RUN4;                            // 估算器里的PI的KP
        FOC_EKI                 = OBSW_KI_GAIN_RUN4;                            // 估算器里的PI的KI
    }
    #elif (Open_Start_Mode == Omega_Start)
    {
        /*********PLL或SMO**********/
        #if (EstimateAlgorithm == SMO  || EstimateAlgorithm == AO)
        {
            mcFocCtrl.State_Count = ATO_START_HOLDTIME + (ATO_RAMP_PERIOD << 2); //ATO 爬坡控制时间
        }
        #elif (EstimateAlgorithm == PLL)
        {
            mcFocCtrl.State_Count   = 0;
            FOC_EKP                 = OBSW_KP_GAIN_RUN4;                       // 估算器里的PI的KP
            FOC_EKI                 = OBSW_KI_GAIN_RUN4;                       // 估算器里的PI的KI
        }
        #endif
    }
    #endif
    FOC_IQREF = IQ_Start_CURRENT;                          // Q轴启动电流
}

/**
    @brief     三下桥刹车
    @date      2022-07-14
*/
void MC_Break(void)
{
    MOE         = 1;
    ClrBit(DRV_CR, FOCEN);  // 关闭FOC
    /*  软件设置PWM占空比输出以上桥为参考，配置为互补输出时下桥反向 */
    ClrBit(DRV_CR, OCS);    // OCS = 0, DRV_COMR;  OCS = 1, FOC/SVPWM/SPWM
    SetBit(DRV_CR, DRVEN);  // 计数器使能      0-->Disable 1-->Enable
    DRV_CMR = 0x0015;       //  关闭上桥输出，开启下桥输出
    DRV_DR  = (DRV_ARR) + 4 ; // ARR+4为全开
}



/**
    @brief     上电时，先对硬件电路的电流进行采集，写入对应的校准寄存器中。
              调试时，需观察mcCurOffset结构体中对应变量是否在范围内。采集结束后，OffsetFlag置1。
    @exception 默认循环1000次等待偏置电压稳定，需要注意若需要每次电机启动前都对偏置电压进行采样校准，
              那么需要保证偏置电压是稳定的，且采样计数变量已被清零（重新进行1000次循环)，否则可能导致采样出错
    @date      2022-07-14
*/
void GetCurrentOffset(void)
{
    SetBit(ADC_CR, ADCBSY);             // 使能ADC
    
    while (ReadBit(ADC_CR, ADCBSY));
    
    #if (Shunt_Resistor_Mode == Single_Resistor)                   //单电阻模式
    {
        mcCurOffset.Iw_busOffsetSum += ((ADC4_DR & 0x7ff8));
        mcCurOffset.Iw_busOffset     = mcCurOffset.Iw_busOffsetSum >> 4;
        mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
        mcCurOffset.IbusOffsetSum += ((ADC7_DR & 0x7ff8));
        mcCurOffset.IbusOffset     = mcCurOffset.IbusOffsetSum >> 4;
        mcCurOffset.IbusOffsetSum -= mcCurOffset.IbusOffset;
    }
    #elif (Shunt_Resistor_Mode == Double_Resistor)                 //双电阻模式
    {
        mcCurOffset.IuOffsetSum     += ((ADC0_DR & 0x7ff8));
        mcCurOffset.IuOffset         = mcCurOffset.IuOffsetSum >> 4;
        mcCurOffset.IuOffsetSum     -= mcCurOffset.IuOffset;
        mcCurOffset.IvOffsetSum     += ((ADC1_DR & 0x7ff8));
        mcCurOffset.IvOffset         = mcCurOffset.IvOffsetSum >> 4;
        mcCurOffset.IvOffsetSum     -= mcCurOffset.IvOffset;
    }
    #elif (Shunt_Resistor_Mode == Three_Resistor)                 //三电阻模式
    {
        mcCurOffset.IuOffsetSum     += ((ADC0_DR & 0x7ff8));
        mcCurOffset.IuOffset         = mcCurOffset.IuOffsetSum >> 4;
        mcCurOffset.IuOffsetSum     -= mcCurOffset.IuOffset;
        mcCurOffset.IvOffsetSum     += ((ADC1_DR & 0x7ff8));
        mcCurOffset.IvOffset         = mcCurOffset.IvOffsetSum >> 4;
        mcCurOffset.IvOffsetSum     -= mcCurOffset.IvOffset;
        mcCurOffset.Iw_busOffsetSum += ((ADC4_DR & 0x7ff8));
        mcCurOffset.Iw_busOffset     = mcCurOffset.Iw_busOffsetSum >> 4;
        mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
    }
    #endif
    mcCurOffset.OffsetCount++;
    
    if (mcCurOffset.OffsetCount > 1000)
    {
        mcCurOffset.OffsetFlag = 1;
    }
}



/**
    @brief     关闭输出，关闭FOC，电机切换到mcReady状态被调用一次
    @date      2022-07-14
*/
void Motor_Ready(void)
{
}