📄 motor_control_2812.c
字号:
#include "IQmathLib.h" /* Include header for IQmath library */
#include "DSP28_Device.h"
#include "motor_control_2812.h"
#include "parameter.h"
//#include "SCI2812_scia.h"
// Prototype statements for functions found within this file.
interrupt void EvaTimer1(void);
interrupt void cpu_timer0_isr(void);
interrupt void EvaPdpinta(void);
interrupt void Xint1(void);
interrupt void EvaCaputure1(void);
interrupt void EvaCap(void);
interrupt void EvaCap2(void);
interrupt void EvbQep1(void);//上溢中断
interrupt void EvbQep2(void);//下溢中断
void error(void);
extern void scia_fifo_init(void);// Initialize the SCI FIFO in the file Dsp28_Sci.c
extern void scib_fifo_init(void);// Initialize the SCI FIFO in the file Dsp28_Sci.c
extern void scia_loopback_init(void);// Initalize SCI for digital loop back in the file Dsp28_Sci.c
extern void scib_loopback_init(void);// Initalize SCI for digital loop back in the file Dsp28_Sci.c
extern void scia_tx_isr(void);
extern void alarm1(void);
extern void alarm2(void);
extern void alarm3(void);
extern void alarm4(void);
extern void alarm5(void);
int LED_count=0;
int LED_num=0;
int LED_count2=0;
int LED_num2=0;
int overcurrent=0;
int ipmerror = 0;
int pwmdisable=0;
int brake=0;
long GlobalQ = GLOBAL_Q;
_iq Imax1=_IQ(0);
_iq Imax2=_IQ(0);
_iq Imax3=_IQ(0);
_iq speed_ref = _IQ(0); /* Speed reference (pu) */
_iq speed_ref2=_IQ(0);
_iq speed_ref_flt=_IQ(0); /* Speed reference after filter (pu) */
float T = SAMPLING_PERIOD; /* Samping period (sec), see parameter.h */
long repeat=0;
/* For vector control,added bye lyh */
_iq speedback;/*the speed from the sensor*/
PARK park1 = PARK_DEFAULTS;
IPARK ipark1 = IPARK_DEFAULTS;
IPARK ipark2 = IPARK_DEFAULTS;
ICLARKE iclarke1 = ICLARKE_DEFAULTS;
RMPCNTL rc1 = RMPCNTL_DEFAULTS;
RAMPGEN rg1 = RAMPGEN_DEFAULTS;
SVGENDQ svm1 = SVGENDQ_DEFAULTS;
PWMGEN pwm1 = PWMGEN_DEFAULTS;
LPF lpf_w_ref = LPF_DEFAULTS;
DERIVE derive1 = DERIVE_DEFAULTS;
CAPTURE cap1 = CAPTURE_DEFAULTS;
QEP qep1 = QEP_DEFAULTS;
IM_svm svm2 = IM_svm_DEFAULTS;
void main(void)
{
// Initialize System Control registers, PLL, WatchDog, Clocks to default state:
// This function is found in the DSP28_SysCtrl.c file.
InitSysCtrl();
// HISPCP prescale register settings, normally it will be set to default values
EALLOW; // This is needed to write to EALLOW protected registers
SysCtrlRegs.HISPCP.all = 0x0000; /* SYSCLKOUT/1 */
EDIS; // This is needed to disable write to EALLOW protected registers
// Disable and clear all CPU interrupts:
DINT;
IER = 0x0000;
IFR = 0x0000;
// Initialize Pie Control Registers To Default State:
// This function is found in the DSP28_PieCtrl.c file.
InitPieCtrl();
// Initialize the PIE Vector Table To a Known State:
// This function is found in DSP28_PieVect.c.
// This function populates the PIE vector table with pointers
// to the shell ISR functions found in DSP28_DefaultIsr.c.
InitPieVectTable();
//Init_ram();
EALLOW; // This is needed to write to EALLOW protected registers
PieVectTable.T1UFINT = &EvaTimer1;
PieVectTable.TINT0 = &cpu_timer0_isr;
PieVectTable.PDPINTA = &EvaPdpinta;
PieVectTable.XINT1 = &Xint1;
PieVectTable.ADCINT = &EvaTimer1;
PieVectTable.CAPINT1 = &EvaCaputure1;
PieVectTable.T2OFINT = &EvaCap;
PieVectTable.T2PINT = &EvaCap2;//周期中断
PieVectTable.T4OFINT = &EvbQep1;
PieVectTable.T4UFINT = &EvbQep2;
EDIS; // This is needed to disable write to EALLOW protected registers
Init_interrupt();
//Init_ram();
InitGpio();
InitAdc();
// Configure ADC
// AdcRegs.ADCTRL3.bit.SMODE_SEL =1;
AdcRegs.ADCMAXCONV.all = 0x0005; // Setup 6 conv's on SEQ1
AdcRegs.ADCCHSELSEQ1.bit.CONV00 = 0x2; // Setup ADCINA0 as 1st SEQ1 conv.
AdcRegs.ADCCHSELSEQ1.bit.CONV01 = 0x9; // Setup ADCINA1 as 2nd SEQ1 conv.
AdcRegs.ADCCHSELSEQ1.bit.CONV02 = 0xf; // Setup ADCINA2 as 2nd SEQ1 conv.
AdcRegs.ADCCHSELSEQ1.bit.CONV03 = 0xe; // Setup ADCINB0 as 1st SEQ2 conv.
AdcRegs.ADCCHSELSEQ2.bit.CONV04 = 0x8; // Setup ADCINB1 as 2nd SEQ2 conv.
AdcRegs.ADCCHSELSEQ2.bit.CONV05 = 0x0; // Setup ADCINB2 as 2nd SEQ2 conv.
AdcRegs.ADCTRL2.bit.EVA_SOC_SEQ1 = 1; // Enable EVASOC to start SEQ1
AdcRegs.ADCTRL2.bit.INT_ENA_SEQ1 = 1; // Enable SEQ1 interrupt (every EOS)
// AdcRegs.ADCTRL2.bit.SOC_SEQ2 = 1;
/*Capture Unit setup for the QEP*/
/**/
tadc.cur_param = _IQdiv(_IQ20toIQ(_IQ20div(_IQ20(1000),_IQ20(62))),_IQ(BASE_CURRENT)); //1000/62/I_BASE
tadc.cur_param = _IQdiv(_IQmpy(tadc.cur_param,_IQ(2.0)),_IQ(3.0)); //1000/62/I_BASE*2/3
tadc.v_param = _IQmpyIQX(_IQ12div(_IQ12(54250),_IQ12(BASE_VOLTAGE*2.5)),12,_IQdiv(_IQ(1),_IQ(62)),24);//(54000+250)/2.5/62/VDC_BASE
//tadc.v_param = _IQmpyIQX(_IQ12div(_IQ12(84510),_IQ12(BASE_VOLTAGE*2.5)),12,_IQdiv(_IQ(1),_IQ(62)),24);//1000uF的主回路
tadc.Cur_u_offset = 0;
tadc.Cur_v_offset = 0;
tadc.A = _IQ(0.0007112855);
tadc.B = _IQ(0.037932);
/* Initialize modules */
//pwm1.n_period = 0x927C; /* ISR frequency = 2 kHz with 150MHz clock (0x927C = 37500) */
// pwm1.n_period = 0x493E; /* ISR frequency = 4 kHz with 150MHz clock (0x493E = 18750) */
pwm1.n_period = 0x249F; /* ISR frequency = 8 kHz with 150MHz clock (0x249F = 9375) */
// pwm1.n_period = 0xffff; /* ISR frequency = 2 kHz with 150MHz clock (0x927C = 37500) */
//pwm1.DB_COM = -150; //2.8us
pwm1.DB_COM = -150; //2.8us
pwm1.init(&pwm1);
/* Initialize the Capture and qep */
cap1.init(&cap1);
qep1.init(&qep1);
/* Initialize the RAMPGEN module */
rg1.step_angle_max = _IQ(50*T);
/*Initialize the speed sensor module*/
qep1.wc =derive1.wc_wr;
qep1.Tc =derive1.tc;
cap1.Tc =derive1.tc;
cap1.wc =derive1.wc_wr;
/* Initialize the TIME_FE module */
GpioDataRegs.GPASET.bit.GPIOA7=1; //act for brake on the SMTH mainpower
GpioDataRegs.GPACLEAR.bit.GPIOA6 = 1;//short,端口值低电平时继电器短路
// GpioDataRegs.GPFCLEAR.bit.GPIOF14=1;//XF
GpioDataRegs.GPFSET.bit.GPIOF14=1;
// Enable CAP1 interrupt bits for GP timer 2
EvaRegs.EVAIMRC.bit.CAP1INT = 1;
//EvaRegs.EVAIMRC.bit.CAP1INT = 1;
EvaRegs.EVAIFRC.bit.CAP1INT = 1;
EvaRegs.EVAIMRB.bit.T2OFINT = 1;
EvaRegs.EVAIFRB.bit.T2OFINT = 1;//T2的上溢中断
EvaRegs.EVAIMRB.bit.T2PINT = 1;
EvaRegs.EVAIFRB.bit.T2PINT = 1;//T2的周期匹配中断
EvaRegs.EVAIFRB.bit.T2OFINT = 1;
EvbRegs.EVBIMRB.bit.T4OFINT = 1;//中断屏蔽寄存器
EvbRegs.EVBIFRB.bit.T4OFINT = 1;//中断标志位
PieCtrlRegs.PIEIER3.bit.INTx4 = 1;
// Enable PIE group 3 interrupt 4 for T2PINT
PieCtrlRegs.PIEIER3.bit.INTx1 = 1;
// Enable PIE group 3 interrupt 5 for CAPINT1
PieCtrlRegs.PIEIER3.bit.INTx5 = 1;
PieCtrlRegs.PIEIER5.bit.INTx4 = 1;
//PieCtrlRegs.PIEIER5.bit.INTx3 = 1;
// Enable CPU INT3 for CAPINT:
IER |= M_INT3;
IER |= M_INT5;
// Enable global Interrupts and higher priority real-time debug events:
EINT; // Enable Global interrupt INTM
ERTM; // Enable Global realtime interrupt DBGM
// IDLE loop. Just sit and loop forever:
for(;;)
{
if (CpuTimer0.InterruptCount >=500)
{
// GpioDataRegs.GPFTOGGLE.bit.GPIOF3=1;
//GpioDataRegs.GPFTOGGLE.bit.GPIOF14=1;
CpuTimer0.InterruptCount = 0;
}
}
}
#pragma CODE_SECTION(EvaTimer1, "EvaTimer1Funcs")
interrupt void EvaTimer1(void)
{
if (GpioDataRegs.GPADAT.bit.GPIOA11 == 1) //the state of s1
{
// EvaRegs.COMCONA.all = 0xA400;
EvaRegs.COMCONA.all = 0xA400;
EvaRegs.COMCONA.all = 0xA400;
pwmdisable=1;
//GpioDataRegs.GPBSET.bit.GPIOB13 =1;
}
tadc.V_dc = AdcRegs.ADCRESULT0 >>4;
tadc.Cur_dc = AdcRegs.ADCRESULT1 >>4;
tadc.Adc_2 = AdcRegs.ADCRESULT2 >>4;
tadc.Adc_1 = AdcRegs.ADCRESULT3 >>4;
tadc.Cur_v = (AdcRegs.ADCRESULT4 >>4);//-tadc.Adc_0;
tadc.Cur_u = (AdcRegs.ADCRESULT5 >>4);//-tadc.Adc_0;
tadc.Cur_u0 = 2237;
tadc.Cur_v0 = 2240;
tadc.V_dc0 = 0;
tadc.REF_A = _IQmpyIQX(tadc.A,24,_IQ19(tadc.Cur_u0),19)+tadc.B;
tadc.IA = _IQmpyIQX(tadc.A,24,_IQ19(tadc.Cur_u),19)+tadc.B-tadc.REF_A;
tadc.IA = _IQmpy(tadc.IA,tadc.cur_param); //(*1000/62/I_BASE*2/3)
tadc.REF_B = _IQmpyIQX(tadc.A,24,_IQ19(tadc.Cur_v0),19)+tadc.B;
tadc.IB = _IQmpyIQX(tadc.A,24,_IQ19(tadc.Cur_v),19)+tadc.B-tadc.REF_B;
tadc.IB = _IQmpy(tadc.IB,tadc.cur_param);
tadc.IC = -(tadc.IA+tadc.IB);
//tadc.REF = _IQmpyIQX(tadc.A,24,_IQ19(tadc.V_dc0),19)+tadc.B;
tadc.REF_DC = _IQmpyIQX(tadc.A,24,_IQ19(tadc.V_dc0),19)+tadc.B;
tadc.VDC = _IQmpyIQX(tadc.A,24,_IQ19(tadc.V_dc),19)+tadc.B-tadc.REF_DC;
tadc.VDC = _IQmpy(tadc.VDC,tadc.v_param);
//tadc.VDC = _IQ(1.4825858);
/*过压保护,阈值为600V*/
/*参考转速滤波*/
lpf_w_ref.x = speed_ref;
lpf_w_ref.calc(&lpf_w_ref);
speed_ref_flt = lpf_w_ref.y;
rc1.target_value = speed_ref_flt;
rc1.calc(&rc1);
rg1.rmp_freq = rc1.setpt_value;
rg1.calc(&rg1);
ipark1.de = _IQmpy(rc1.setpt_value,_IQ(1.0));
ipark1.qe = _IQ(0.0);
ipark1.ang = rg1.rmp_out;
ipark1.calc(&ipark1);
svm1.Ualfa = _IQmpy(tadc.VDCinv,ipark1.ds);
svm1.Ubeta = _IQmpy(tadc.VDCinv,ipark1.qs);
//svm1.Ualfa = ipark1.ds;
//svm1.Ubeta = ipark1.qs;
svm1.calc(&svm1);
svm2.Ualfa = ipark1.ds;
svm2.Ubeta = ipark1.qs;
svm2.Vdc = tadc.VDC;
svm2.calc(&svm2);
pwm1.Mfunc_c1 = (Uint16)_IQtoIQ14(svm1.Ta);
pwm1.Mfunc_c2 = (Uint16)_IQtoIQ14(svm1.Tb);
pwm1.Mfunc_c3 = (Uint16)_IQtoIQ14(svm1.Tc);
pwm1.update(&pwm1);
//speed_ref = (long)Add_Val*(long)1024;
if(speed_ref>_IQ(1))
speed_ref = _IQ(1.0);
// Reinitialize for next ADC sequence
AdcRegs.ADCTRL2.bit.RST_SEQ1 = 1; // Reset SEQ1
AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1; // Clear INT SEQ1 bit
PieCtrlRegs.PIEACK.all |= PIEACK_GROUP1;
}
interrupt void EvaCaputure1(void)
{
cap1.calc(&cap1);
//speed_accu=cap1.speed;
EvaRegs.EVAIMRC.bit.CAP1INT = 1;
PieCtrlRegs.PIEACK.all |= PIEACK_GROUP3;
EvaRegs.EVAIFRC.bit.CAP1INT = 1;
}
interrupt void EvaCap(void)
{
cap1.ISR(&cap1);//定时器上溢中断
EvaRegs.EVAIMRB.bit.T2OFINT = 1;
PieCtrlRegs.PIEACK.all |= PIEACK_GROUP3;
EvaRegs.EVAIFRB.bit.T2OFINT = 1;
}
interrupt void EvaCap2(void)
{
cap1.ISR(&cap1);//定时器周期中断
EvaRegs.EVAIMRB.bit.T2PINT = 1;
PieCtrlRegs.PIEACK.all |= PIEACK_GROUP3;
EvaRegs.EVAIFRB.bit.T2PINT = 1;
}
interrupt void EvbQep1(void)
{
qep1.isr1(&qep1);//定时器上溢中断
EvbRegs.EVBIMRB.bit.T4OFINT = 1;
PieCtrlRegs.PIEACK.all |= PIEACK_GROUP3;
EvbRegs.EVBIFRB.bit.T4OFINT = 1;
}
interrupt void EvbQep2(void)
{
qep1.isr2(&qep1);
EvbRegs.EVBIMRB.bit.T4UFINT = 1;
PieCtrlRegs.PIEACK.all |= PIEACK_GROUP3;
EvbRegs.EVBIFRB.bit.T4UFINT = 1;
}
interrupt void cpu_timer0_isr(void)
{// peirod=2ms
CpuTimer0.InterruptCount++;
// Acknowledge this interrupt to recieve more interrupts from group 1
//PieCtrlRegs.PIEACK.all = PIEACK_GROUP1 + PIEACK_GROUP3;
//PieCtrlRegs.PIEACK.bit.ACK1 =1;
//PieCtrlRegs.PIEACK.bit.ACK3 =1;
PieCtrlRegs.PIEACK.all |= PIEACK_GROUP1;
}
interrupt void EvaPdpinta(void)
{
// EvaRegs.COMCONA.all = 0xA600;//0XA400 .9=0
ipmerror = 1;
// Enable more interrupts from this timer
EvaRegs.EVAIMRA.bit.PDPINTA = 1;
// Note: To be safe, use a mask value to write to the entire
// EVAIFRA register. Writing to one bit will cause a read-modify-write
// operation that may have the result of writing 1's to clear
// bits other then those intended.
EvaRegs.EVAIFRA.all = BIT0;
// Acknowledge interrupt to recieve more interrupts from PIE group 1
//PieCtrlRegs.PIEACK.all = PIEACK_GROUP1 + PIEACK_GROUP3;
//PieCtrlRegs.PIEACK.bit.ACK1 =1;
//PieCtrlRegs.PIEACK.bit.ACK3 =1;
PieCtrlRegs.PIEACK.all |= PIEACK_GROUP1;
}
interrupt void Xint1(void)
{
EvaRegs.COMCONA.all = 0xA400;//0XA400 .9=0
overcurrent = 1;
Imax1=tadc.IA;
Imax2=tadc.IB;
Imax3=tadc.IC;
Imax3=tadc.IC;
// Acknowledge interrupt to recieve more interrupts from PIE group 1
//PieCtrlRegs.PIEACK.all = PIEACK_GROUP1 + PIEACK_GROUP3;
//PieCtrlRegs.PIEACK.bit.ACK1 =1;
//PieCtrlRegs.PIEACK.bit.ACK3 =1;
PieCtrlRegs.PIEACK.all |= PIEACK_GROUP1;
}
//===========================================================================
// No more.
//===========================================================================
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -