RT-Thread-【N32G457】基于RT-Thread和N32G457的电机驱动RT-Thread问答社区

【N32G457】基于RT-Thread和N32G457的电机驱动

发布于 2022-03-24 22:44:08 浏览：793 订阅该版

![微信图片_20220328220533.jpg](https://oss-club.rt-thread.org/uploads/20220328/779d8ef7b11a59484109562d07170aa1.jpg.webp)
![QQ截图20220324212135.png](https://oss-club.rt-thread.org/uploads/20220324/baa547e6023d7e7a06a4585b3a609372.png.webp)
![QQ截图20220324212238.png](https://oss-club.rt-thread.org/uploads/20220324/eb18577b85378d3e891c07732cccc7fd.png.webp)
![QQ截图20220324213300.png](https://oss-club.rt-thread.org/uploads/20220324/cd352b23fdcb3d97caa07f03963d0056.png.webp)
![QQ截图20220324213359.png](https://oss-club.rt-thread.org/uploads/20220324/4baa9838fd4e2eb6b4a81d221abff7eb.png.webp)
![QQ截图20220324213430.png](https://oss-club.rt-thread.org/uploads/20220324/20419c225c57af948ca0a489d3c96a51.png.webp)
[电机驱动.mp4](https://oss-club.rt-thread.org/uploads/20220324/c3ccee9b0831a2d81e1567f8927b1a6e.mp4)
```
#include "..\include\main.h"
/*------------------------------------------------------------------------*/  
void MAIN_SECTION StartProgram(void) 
{
    unsigned int Temp;
    
    Initialize();
    tSys.StartupDelay = STARTUP_DELAY;
    ENIE(); 
    
    do
    {
        DISIE();
        Temp = tSys.StartupDelay;
        ENIE();
    }while(Temp != 0);
    
    GetCurrentOffset();
    
    while(1)
    {
        AdcModule();//ADC转换值处理模块
        MotorModule();//电机控制模块
        ErrorModule();//错误处理模块
        LedDisplay();//LED状态显示
    }
}
/*-----------------------------------------------------------------------------------------------*/
void __attribute__((section(".main_section"),interrupt,shadow,no_auto_psv)) _AD1Interrupt(void)
{
    IFS0bits.AD1IF = 0;//清除ADC转换完成的中断标志位
    /*--------------------用户代码区-------------------------------------*/
    LED0_PIN = LED_ON;
    CoreControl();
    AdcFilter();
    SpiTransmit();
    CountDowmTimer();
    LED0_PIN = LED_OFF;
    /*-------------------用户代码区结束---------------------------------*/
}
/*-----------------------------------------------------------------------------------------------*/
static void MAIN_SECTION CountDowmTimer(void)
{
    tSys.TickPwm ++;
    
    if(tSys.TickPwm >= PWM_NUM_1MS)//1ms到
    {
        tSys.TickPwm = 0;
        
        if(tSys.StartupDelay != 0)
            tSys.StartupDelay --;
        
        if(tSys.SpeedCount != 0)
            tSys.SpeedCount --;
        
        if(TickAng != 0)
            TickAng--;
    }
}
```> #include "..\include\spwm.h"
#include "..\include\Define.h"
#include "..\include\User.h"
/*----------------------------------------------------------------------------*/
//函数声明
void CORE_SECTION GetUabc(void);
void CORE_SECTION SPWMOut(void);
/*----------------------------------------------------------------------------*/
//结构体变量定义
tSpwm_Type CORE_RAM tSpwm;
/*----------------------------------------------------------------------------*/
//结构体变量外部调用声明
extern tCore_Type CORE_RAM tCore;
extern tU_Type CORE_RAM tUm;> 
#include "..\include\p33EP128MC506.h"
#include "..\include\Define.h"
#include "..\include\User.h"
/*---------------------------------------------------------------------------*/
//函数声明
static void MAIN_SECTION CountDowmTimer(void);
/*---------------------------------------------------------------------------*/
//外部函数调用声明
extern void INIT_SECTION Initialize(void);
extern void SPI_SECTION SpiTransmit(void);
extern void UART_SECTION SentReceData(void);
extern void UART_SECTION UartModule(void);
extern void CORE_SECTION CoreControl(void);
extern void ADC_SECTION AdcFilter(void);
extern void ADC_SECTION GetCurrentOffset(void);
extern void MOTOR_SECTION MotorModule(void);
extern void ADC_SECTION AdcModule(void);
extern void ERROR_SECTION ErrorModule(void);
extern void LED_SECTION LedDisplay(void);
/*---------------------------------------------------------------------------*/
//变量定义
unsigned int MAIN_RAM TickAng;
//结构体变量定义
tSys_Type MAIN_RAM  tSys;
/*---------------------------------------------------------------------------*/
//外部变量引用声明
extern unsigned int LED_RAM LedCount;
extern unsigned int UART_RAM UartCount;
/*---------------------------------------------------------------------------*/
#define PWM_NUM_1MS  (120000 / PWM_PERIOD_VALUE) 
#include "..\include\svpwm.h"
#include "..\include\Define.h"
#include "..\include\User.h"
/*----------------------------------------------------------------------------*/
//函数声明
void CORE_SECTION SvpwmInitalize(void);
void CORE_SECTION SvpwmOut(void);
/*----------------------------------------------------------------------------*/
static void CORE_SECTION CalPwmDutyTime(signed int sV1t,signed int sV2t);
/*----------------------------------------------------------------------------*/
//合成电压矢量大小最大值宏定义
#define PERCENT_MAX     62258      //65535 * 0.95
/*----------------------------------------------------------------------------*/
//结构体变量定义
tSvpwm_Type  CORE_RAM tSvpwm;
/*----------------------------------------------------------------------------*/
//结构体变量外部调用声明
extern tU_Type CORE_RAM tUm;
#include "..\include\Pid.h"
/*----------------------------------------------------------------------------*/
//计算比例项输出
static signed int MOTOR_SECTION CalProportion(tPID_Type *tVar)
{
    UNION_S32 sTemp32;
    signed int sTemp;
    tPID_Type tPVar;
    
    tPVar = *tVar;
    
    sTemp32.S32 = __builtin_mulus(tPVar.Kp,tPVar.sErr);
    sTemp = sTemp32.Words.High;
    //计算积分项限幅值
    if(sTemp >= 0)
        sTemp32.Words.High = 32767 - sTemp;
    else
        sTemp32.Words.High = 32767 + sTemp;
    sTemp32.Words.Low = 0;//积分限幅值低16位赋值为0
    tPVar.sLimit = sTemp32.S32;
    *tVar = tPVar;
    
    return (sTemp);
}
/*----------------------------------------------------------------------------*/
//计算积分项输出
static signed int MOTOR_SECTION CalIntegration(tPID_Type *tVar)
{
    UNION_S32 sTemp32;
    tPID_Type tIVar;
    
    tIVar = *tVar;
    
    sTemp32.S32 = __builtin_mulus(tIVar.Ki,tIVar.sErr);
    tIVar.qSumErr32.S32 += sTemp32.S32;
    //积分累加项限幅
    if(tIVar.qSumErr32.S32 > tIVar.sLimit)
        tIVar.qSumErr32.S32 = tIVar.sLimit;
    if(tIVar.qSumErr32.S32 < -tIVar.sLimit)
        tIVar.qSumErr32.S32 = -tIVar.sLimit;
    
    sTemp32.S32 = tIVar.qSumErr32.S32;//得到积分项
    
    *tVar = tIVar;
    
    return (sTemp32.Words.High);//取积分项高16位，整数部分
}
/*----------------------------------------------------------------------------*/
//计算微分项输出
static signed int MOTOR_SECTION CalDifferential(tPID_Type *tVar)
{
    UNION_S32 sTemp32;
    tPID_Type tDVar;
    signed int sDeltaErr;
    
    tDVar = *tVar;
    sDeltaErr = tDVar.sErr - tDVar.sErrLast;
    tDVar.sErrLast = tDVar.sErr;//更新上一次误差
    
    sTemp32.S32 = __builtin_mulus(tDVar.Kd,sDeltaErr);
    
    *tVar = tDVar;
    
    return (sTemp32.Words.High);//取微分项高16位，整数部分
}
/*----------------------------------------------------------------------------*/
//PID控制输出
void MOTOR_SECTION PidControl(tPID_Type *tPidStruct)
{
    signed int sTemp;
    signed int sTempD;
    tPID_Type tPidVar;
    UNION_S32 sTemp32;
    
    tPidVar = *tPidStruct;
    sTemp = CalProportion(&tPidVar) + CalIntegration(&tPidVar);//比例积分项输出相加
    sTempD = CalDifferential(&tPidVar);//微分项
    sTemp32.S32 = (signed long)sTemp + (signed long)sTempD;
    if(sTemp32.S32 > 32767)
        tPidVar.sOut = 32767;
    else if(sTemp32.S32 < -32767)
        tPidVar.sOut = -32767;
    else
        tPidVar.sOut = sTemp + sTempD;
    
    *tPidStruct = tPidVar;
}

#include "..\include\Pwm.h"
/*------------------------------------------------------------------------------*/
void PWM_SECTION PwmDutySet(unsigned int PwmDtuy1,unsigned int PwmDtuy2,unsigned int PwmDtuy3)
{
    PDC1 = PwmDtuy1;
    PDC2 = PwmDtuy2;
    PDC3 = PwmDtuy3;
}
#include "..\include\Voltage.h"
/*----------------------------------------------------------------------------*/
void CORE_SECTION UsOut(void)
{
    CalSinCos(tUm.qAngle);//正弦表查表
    GetUsSector();//扇区判断
    GetUxyz();//X Y Z计算
    SvpwmOut();//SVPWM时间计算 和占空比控制
    GetUalphabeta();//获取Ualpha Ubeta
}
/*----------------------------------------------------------------------------*/
//获取合成电压矢量所在扇区
static void CORE_SECTION GetUsSector(void)
{
    if((tUm.qAngle>=0) && (tUm.qAngle < EANGLE60))
        tUm.Sector = 1;
    else if((tUm.qAngle >= EANGLE60) && (tUm.qAngle < EANGLE120))
        tUm.Sector = 2;
    else if(tUm.qAngle >= EANGLE120)
        tUm.Sector = 3;
    else if(tUm.qAngle < -EANGLE120)
        tUm.Sector = 4;
    else if((tUm.qAngle >= -EANGLE120) && (tUm.qAngle < -EANGLE60))
        tUm.Sector = 5;
    else   
        tUm.Sector = 6; 
}
/*----------------------------------------------------------------------------*/
//计算X、Y、Z
//     x = sin（θ） 
//     y = 1/2Sin（θ） + sqrt(3)/2 Cos（θ）
//     z = 1/2Sin（θ） - sqrt(3)/2 Cos（θ）
static void CORE_SECTION GetUxyz(void)
{
    UNION_S32 sTemp32;
    
    tUm.qUx = tCore.sSinValue;
    sTemp32.S32 = __builtin_mulus(SQRT3DIV2,tCore.sCosValue);       
    tUm.qUy = (tCore.sSinValue >> 1) + sTemp32.Words.High;
    tUm.qUz = (tCore.sSinValue >> 1) - sTemp32.Words.High;
}
/*----------------------------------------------------------------------------*/
//计算Ualpha、Ubeta
//Ualpha = |Us|Cosθus
//Ubeta = |Us|Sinθus
/*----------------------------------------------------------------------------*/
static void CORE_SECTION GetUalphabeta(void)
{
    tUm.qUs = (signed int)(tUm.Percent >> 1);
    tUm.qUalphaOld = tUm.qUalpha;
    tUm.qUbetaOld = tUm.qUbeta;
    tUm.qUalpha = MulFrac(tUm.qUs,tCore.sCosValue);
    tUm.qUbeta = MulFrac(tUm.qUs,tCore.sSinValue);
}
/*----------------------------------------------------------------------------*/
//计算电压合成矢量的大小、计算电压合成矢量与电流合成矢量夹角（超前角）
//合成电压矢量大小求取公式：
//|Us| = sqrt(Ud^2 + Uq^2) 
//Uq^2 = Us^2 - Ud^2
//Uq = sqrt（Us^2 - Ud^2）
//UqMax = sqrt（UsMax^2 - Ud^2）
//超前角计算公式：
//θdq = arctan（-Ud / Uq）
/*----------------------------------------------------------------------------*/
void CORE_SECTION GetUs(void)
{
    unsigned int Temp;
    signed int qVq_Max;
    
    tUm.qUsMax = (signed int)(tUm.PercentMax >> 1);
    Temp = (unsigned int)(MulFrac(tUm.qUsMax,tUm.qUsMax) - MulFrac(tUm.qUd,tUm.qUd));//UsMax^2 - Ud^2
    Temp <<= 1;
    Temp = CalSqrt(Temp);
    qVq_Max = (signed int)(Temp >> 1);
    //对Uq进行限幅处理
    if(tUm.qUq > qVq_Max)
        tUm.qUq = qVq_Max;
    if(tUm.qUq < -qVq_Max)
        tUm.qUq = -qVq_Max;
    Temp = (unsigned int)(MulFrac(tUm.qUq,tUm.qUq) + MulFrac(tUm.qUd,tUm.qUd));
    Temp <<= 1;
    tUm.PercentOut = CalSqrt(Temp);
    tUm.qDQAngle = CalAtan(-tUm.qUd,tUm.qUq);
}
#include "..\include\Adc.h"
/*----------------------------------------------------------------------------*/
//AN0 - Ia AN1 - Ib AN2 - Ic AN3 - Vbus电压 AN4 - 旋钮电压 AN6 - Ibus电流
void ADC_SECTION GetAdcValue(void)
{
    if(tAdc.Index == 3)
    {
        tAdc.An3.ValueInstant = ADC1BUF0;
        tAdc.An0.ValueInstant = ADC1BUF1;
        tAdc.An1.ValueInstant = ADC1BUF2;
        tAdc.An2.ValueInstant = ADC1BUF3;
        
        AD1CHS0 = 0x0404;
        tAdc.Index = 4;
    }
    else if(tAdc.Index == 4)
    {
        tAdc.An4.ValueInstant = ADC1BUF0;
        tAdc.An0.ValueInstant = ADC1BUF1;
        tAdc.An1.ValueInstant = ADC1BUF2;
        tAdc.An2.ValueInstant = ADC1BUF3;
        
        AD1CHS0 = 0x0606;
        tAdc.Index = 6;
    }
    else if(tAdc.Index == 6)
    {
        tAdc.An6.ValueInstant = ADC1BUF0;
        tAdc.An0.ValueInstant = ADC1BUF1;
        tAdc.An1.ValueInstant = ADC1BUF2;
        tAdc.An2.ValueInstant = ADC1BUF3;
        
        AD1CHS0 = 0x0303;
        tAdc.Index = 3;
    }
    else
    {
        tAdc.An3.ValueInstant = ADC1BUF0;
        tAdc.An0.ValueInstant = ADC1BUF1;
        tAdc.An1.ValueInstant = ADC1BUF2;
        tAdc.An2.ValueInstant = ADC1BUF3;
        
        AD1CHS0 = 0x0303;
        tAdc.Index = 3;
    }
}
/*----------------------------------------------------------------------------*/
//ADC采样转换结果滤波处理
//AN0 - Ia AN1 - Ib AN2 - Ic AN3 - Vbus电压 AN4 - 旋钮电压 AN6 - Ibus电流
void ADC_SECTION AdcFilter(void)
{
    if(tSys.StartupDelay != 0)
    {
        //Ia64次平均滤波处理
        tAdc.An0.ValueSum += tAdc.An0.ValueInstant;
        tAdc.An0.ValueNum ++;
        if(tAdc.An0.ValueNum >= ANI_SAMPLE_NUM)
        {
            tAdc.An0.ValueAverage = tAdc.An0.ValueSum >> 6;
            tAdc.An0.ValueSum = 0;
            tAdc.An0.ValueNum = 0;
        }
        
        //Ib64次平均滤波处理
        tAdc.An1.ValueSum += tAdc.An1.ValueInstant;
        tAdc.An1.ValueNum ++;
        if(tAdc.An1.ValueNum >= ANI_SAMPLE_NUM)
        {
            tAdc.An1.ValueAverage = tAdc.An1.ValueSum >> 6;
            tAdc.An1.ValueSum = 0;
            tAdc.An1.ValueNum = 0;
        }
        
        //Ic64次平均滤波处理
        tAdc.An2.ValueSum += tAdc.An2.ValueInstant;
        tAdc.An2.ValueNum ++;
        if(tAdc.An2.ValueNum >= ANI_SAMPLE_NUM)
        {
            tAdc.An2.ValueAverage = tAdc.An2.ValueSum >> 6;
            tAdc.An2.ValueSum = 0;
            tAdc.An2.ValueNum = 0;
        }       
    }
    //Vbus电压 64次平均滤波处理
    tAdc.An3.ValueSum += tAdc.An3.ValueInstant;
    tAdc.An3.ValueNum ++;
    if(tAdc.An3.ValueNum >= ANU_SAMPLE_NUM)
    {
        tAdc.An3.ValueAverage = tAdc.An3.ValueSum >> 6;
        tAdc.An3.ValueSum = 0;
        tAdc.An3.ValueNum = 0;
        tAdc.An3.ValueFilter = LowPassFilter(tAdc.An3.ValueAverage,tAdc.An3.ValueFilter,VBUS_LOWPASS_CONSTANT);
    } 
    //旋钮电压 8次平均滤波处理
    tAdc.An4.ValueSum += tAdc.An4.ValueInstant;
    tAdc.An4.ValueNum ++;
    if(tAdc.An4.ValueNum >= ANP_SAMPLE_NUM)
    {
        tAdc.An4.ValueAverage = tAdc.An4.ValueSum >> 3;
        tAdc.An4.ValueSum = 0;
        tAdc.An4.ValueNum = 0;
    } 
}
/*----------------------------------------------------------------------------*/
//获取初始电流偏置值
void ADC_SECTION GetCurrentOffset(void)
{
    tAdc.IoffsetA = tAdc.An0.ValueAverage; 
    tAdc.IoffsetB = tAdc.An1.ValueAverage; 
    tAdc.IoffsetC = tAdc.An2.ValueAverage; 
}
/*----------------------------------------------------------------------------*/
//一阶低通滤波器
//y(n) = (1-a)y(n-1) + ax(n)
//a = Ts*2*pi*fc
static unsigned int ADC_SECTION LowPassFilter(unsigned int xvar,unsigned int yvar,unsigned int factor)
{
    UNION_U32 unTemp32;
    unTemp32.U32 = (unsigned long)(__builtin_muluu(xvar,factor) + __builtin_muluu(yvar,0-factor));
    return(unTemp32.Words.High);
}
/*----------------------------------------------------------------------------*/
//获取设定速度 10位ADC，当旋钮电压为3.3V时，AD转换结果是1023，将AD转换结果左移2位
//得到最大的设定转速，最大的设定转速为 4092
/*----------------------------------------------------------------------------*/
static void ADC_SECTION GetSetSpeed(void)
{
    unsigned int Temp16;
    
    Temp16 = tAdc.An4.ValueAverage << 2;
    
    if(Temp16 < 200)
        Temp16 = 0;
    
    if(Temp16 > MOTRO_MAX_SPEED)
        Temp16 = MOTRO_MAX_SPEED;
    
    tAdc.SetSpeed = Temp16;
}
/*----------------------------------------------------------------------------*/
//Vbus电压获取，计算公式：
//R23 / (R23 + R13) * Vbus = 输入电压AD转换结果 / 1024 * 3.3V
//Vbus = (R23 + R13) / R23 * 3.3 * 输入电压AD转换结果 / 1024 
//R23 = 2K R13 = 30K
//Vbus = 16 * 3.3 * 输入电压AD转换结果 / 1024
//VBUS_FACTOR = 16 * 3.3 = 52.8 ≈ 53
//Vbus = VBUS_FACTOR * 输入电压AD转换结果 / 1024
/*----------------------------------------------------------------------------*/
static unsigned int ADC_SECTION GetVbusVoltage(unsigned int Vbus_Adc)
{
    UNION_U32 unTemp32;
    
    unTemp32.U32 = __builtin_muluu(VBUS_FACTOR,Vbus_Adc);
    unTemp32.U32 >>= 10;
    return(unTemp32.Words.Low);
}
//ADC转换值处理模块
void ADC_SECTION AdcModule(void)
{
    tAdc.BusValue = GetVbusVoltage(tAdc.An3.ValueFilter);
    GetSetSpeed();
}