/* ============================================================================== System Name: HVBLDC_Sensored File Name: HVBLDC_Sensored.C Description: Primary system file for the (Trapezoidal) Sensored BLDC Control Using Hall Effect Sensors ===================================================================================*/ // Include header files used in the main function #include "PeripheralHeaderIncludes.h" #define MATH_TYPE IQ_MATH #include "IQmathLib.h" #include "HVBLDC_Sensored.h" #include "HVBLDC_Sensored-Settings.h" #include #include "UserCan.h" #include "lowpass.h" #include "var.h" #ifdef FLASH #pragma CODE_SECTION(MainISR,"ramfuncs"); void MemCopy(); void InitFlash(); #endif #define TIMER1_S1 60 #define TIMER1_S2 500000 #define TIMER1_PER 30000000 #define MAINLOOPDIV 5 // 主中断40K 40K/MAINLOOPDIV 主循环 // Prototype statements for functions found within this file. interrupt void MainISR(void);//主中断 interrupt void xint1_isr(void);//FO中断 interrupt void xint2_isr(void);//HALL A中断 interrupt void EPWM1TZint_isr(void);//TZ中断 过流中断 void DeviceInit(); void HVDMC_Protection(void); void speed_direction(Uint16 hall,Uint16 hall_last);//使用中断计算转速 void speed_cal(void);//使用中断计算转速 void pwmlimit_speed(void);// void ClosePwm(void); void bldcpwm_close(void); void OpenPwm(void); void mainLoop(void); // State Machine function prototypes //------------------------------------ int16 MastIsrTimePercent(void); void taskfree(void); void ServiceDog(void); void EnableDog(void); // Used for running BackGround in flash, and ISR in RAM //速度计算相关 int16 t4 = 0; int16 Xint2Cnt = 0; int16 hallCmtnTrig = 0; extern Uint16 *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart; int16 SerialCommsTimer; // Global variables used in this system HALL3 hall1 = HALL3_DEFAULTS; SPEED_MEAS_CAP speed1 = SPEED_MEAS_CAP_DEFAULTS; PWMGEN pwm1 = PWMGEN_DEFAULTS; _iq20 kscaler = _IQ20(90*3.3/4096/2.3); float32 T = 0.001/ISR_FREQUENCY; // Samping period (sec), see parameter.h Uint16 BackTicker = 0; Uint16 PreviousState; Uint16 ClosedFlag = 0; Uint32 VirtualTimer = 0; Uint16 ILoopFlag = FALSE; Uint16 SpeedLoopFlag = FALSE; int16 DFuncDesired = 0x0300; // Desired duty cycle (Q15) int16 DfuncTesting = 500;//0x0300;//占空比 int16 Delay30 = 0x3FFF; int16 DelayFlag = 0; Uint16 AlignFlag = 0x000F; Uint16 LoopCount = 0; Uint16 TripFlagDMC=0; //PWM trip status #if (BUILDLEVEL<= LEVEL2) Uint32 CmtnPeriodTarget = 0x00000500; Uint32 CmtnPeriodSetpt = 0x00002000;//要大于CmtnPeriodTarget才能使用-- Uint32 RampDelay = 10; #else Uint32 CmtnPeriodTarget = 0x00000450; Uint32 CmtnPeriodSetpt = 0x00001500; Uint32 RampDelay = 10; #endif _iq DCbus_current=0; _iq Speedgd=0; _iq tempIdc=0; int32 Rt; Uint16 ch1=0; Uint16 ch2=0; Uint16 ch3=0; // Used for ADC Configuration int ChSel[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; int TrigSel[16] = {5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5}; int ACQPS[16] = {8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8}; // Instance PI regulator to regulate the DC-bus current and speed // Instance a PWM driver instance // Instance a PWM DAC driver instance PWMDAC pwmdac1 = PWMDAC_DEFAULTS; // Instance a Hall effect driver // Instance a ramp controller to smoothly ramp the frequency // Instance a RAMP2 Module RMP2 rmp2 = RMP2_DEFAULTS; // Instance a RAMP3 Module RMP3 rmp3 = RMP3_DEFAULTS; // Instance a MOD6 Module MOD6CNT mod1 = MOD6CNT_DEFAULTS; // Instance a IMPULSE Module IMPULSE impl1 = IMPULSE_DEFAULTS; // Instance a SPEED_PR Module // Create an instance of DATALOG Module 950:3A 但是只有外界扭矩低于36N才能启动起来 /*//50N Uint16 pwm_limit_table1[5] = {950,1300,1650,1930,2320};//0 50 100 150 200 //// 0, 100, 200, 300, 400 500 600 700 800 900 1000 Uint16 pwm_limit_table2[]= {950,1650,2320,2985,3668,4348,5024,5705,6383,7061,7760, 8463,9141,9820,10510,11187,11859,12684,13216,13895,14558,//1100-2000 15280,15955,16635,17290,17970,18645,19320,19960,20635,21299, 21970,22640,23280,24000,24700,24700}; Uint16 pwm_limit_table_neg[]= {950,1616,2200,2930,3614,4286,4961,5639,6313,6987,7670, 8348,9022,9700,10361,11080,11714,12397,13080,13743,14438,//1100-2000 15117,15730,16425,17092,17757,18434,19100,19781,20425,21081, 21743,22428,23097,23781,24402,24402};*/ /*//55N 后来测试为50N Uint16 pwm_limit_table1[5] = {950,1330,1650,2000,2300};//0 50 100 150 200 Uint16 pwm_limit_table1_NEG[5] = {950,1300,1650,1935,2285}; Uint16 pwm_limit_table2[]= {950,1650,2300,3030,3750,4455,5130,5830,6515,7210,7900, 8580,9268,9990,10667,11363,12045,12725,13420,14120,14800,//1100-2000 15500,16196,16875,17555,18120,18800,19450,20175,20850,21665, 22320,22950,23610,24260,24945,24945}; Uint16 pwm_limit_table_neg[]= {950,1300,2290,2980,3700,4370,5070,5770,6420,7120,7800, 8500,9180,9860,10540,11235,11910,12570,13260,13945,14600,//1100-2000 15280,15940,16615,17290,17925,18600,19280,19950,20610,21260, 21930,22550,23230,23890,24560,24560};*/ //first test ---------- /*Uint16 pwm_limit_table2[]= {950,1650,2320,2985,3668,4348,5024,5705,6383,7058,7739, 8449,9122,9800,10480,11163,11826,12499,13181,13860,14515, 15192,15872,16530,17203,17861,18537,19183,19848,20525,21161, 21838,22488,23216,23850,24507,24507};*/ //// 0, 100, 200, 300, 400 500 600 700 800 900 1000 /*Uint16 pwm_limit_table_neg[]= {950,1616,2272,2961,3641,4305,4983,5659,6326,6994,7670, 8348,9022,9700,10361,11502,11714,12379,13066,13727,14426,//1100-2000 15085,15610,16325,17005,17660,18338,19000,19693,20390,21048, 21706,22428,23104,22823,24434,24434};*/ //first test ---------- //---55Nm_2----------------------------------------------------------- Uint16 pwm_limit_table1[5] = {1000,1340,1680,2020,2360};//0 200 Uint16 pwm_limit_table1_NEG[5] = {1000,1350,1740,2070,2400};//0 200 Uint16 pwm_limit_table2[]= {1000,1680,2360,3080,3780,4480,5200,5920,6630,7345,8045, 8765,9480,10160,10860,11560,12280,13000,13680,14400,15040,//1100-2000 15740,16440,17110,17830,18510,19210,19910,20610,21260,21950, 22600,23300,23990,24630,25320,25320}; Uint16 pwm_limit_table_neg[]= {1000,1740,2400,3100,3800,4480,5190,5900,6600,7300,8000, 8700,9400,10100,10790,11470,12170,12870,13570,14260,14970,//1100-2000 15620,16300,17000,17660,18340,19038,19715,20395,21070,21750, 22400,23070,23760,24420,25100,25100}; //---59Nm_2----------------------------------------------------------- /*Uint16 pwm_limit_table1[5] = {1000,2440};//0 200 //Uint16 pwm_limit_table1_NEG[5] = {1000,2500};//0 200 Uint16 pwm_limit_table2[]= {1000,1720,2440,3190,3890,4560,5290,6000,6730,7430,8150, 8835,9550,10260,10990,11690,12410,13100,13830,14530,15210,//1100-2000 15930,16610,17310,18000,18660,19360,20000,20740,21430,22100, 22810,23500,24150,24800,25320,25320}; Uint16 pwm_limit_table_neg[]= {1000,1850,2500,3210,3890,4590,5290,5990,6690,7400,8110, 8800,9480,10180,10880,11560,12260,12950,13640,14350,15040,//1100-2000 15730,16390,17090,17770,18470,19120,19820,20490,21180,21800, 22470,23160,23800,24420,25100,25100};*/ //------------------------------------------------------------------ int32 pwmlimitrate = 24507; //NTC _iq20 kb = _IQ20(18.26); _iq12 ktable[] = {_IQ12(-0.901),_IQ12(-2.5013),_IQ12(-6.3342),_IQ12(-16.002),_IQ12(-38.551),_IQ12(-81.812)}; _iq12 btable[] = {_IQ12(28.381),_IQ12(50.657),_IQ12(73.203),_IQ12(98.576),_IQ12(124.83),_IQ12(150.67)}; Uint32 IsrTicker=0; Uint32 startuptimer=0; void main(void) { DeviceInit(); // Device Life support & GPIO InitCan(); // Only used if running from FLASH // Note that the variable FLASH is defined by the compiler #ifdef FLASH // Copy time critical code and Flash setup code to RAM // The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart // symbols are created by the linker. Refer to the linker files. MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart); // Call Flash Initialization to setup flash waitstates // This function must reside in RAM InitFlash(); // Call the flash wrapper init function #endif //(FLASH) // Initialize all the Device Peripherals: // This function is found in DSP280x_CpuTimers.c InitCpuTimers(); // Configure CPU-Timer 0 to interrupt every ISR Period: // 60MHz CPU Freq, ISR Period (in uSeconds) // This function is found in DSP280x_CpuTimers.c ConfigCpuTimer(&CpuTimer0, 60, 1000/ISR_FREQUENCY);//40K StartCpuTimer0(); // Configure CPU-Timer 1,2 for background loops ConfigCpuTimer(&CpuTimer1, TIMER1_S1, TIMER1_S2);//2hz ConfigCpuTimer(&CpuTimer2, 60, 1000000);//20HZ StartCpuTimer1(); StartCpuTimer2(); // Reassign ISRs. // Reassign the PIE vector for TINT0 to point to a different // ISR then the shell routine found in DSP280x_DefaultIsr.c. // This is done if the user does not want to use the shell ISR routine // but instead wants to use their own ISR. EALLOW; // This is needed to write to EALLOW protected registers PieVectTable.TINT0 = &MainISR;//40KHZ PieVectTable.XINT1 = &xint1_isr; PieVectTable.XINT2 = &xint2_isr; PieVectTable.EPWM1_TZINT = &EPWM1TZint_isr; EDIS; // This is needed to disable write to EALLOW protected registers // Enable PIE group 1 interrupt 7 for TINT0 PieCtrlRegs.PIEIER1.bit.INTx7 = 1; PieCtrlRegs.PIEIER1.bit.INTx4 = 1; // Enable PIE Gropu 1 INT4 //XINT1 PieCtrlRegs.PIEIER1.bit.INTx5 = 1; // Enable PIE Gropu 1 INT5 //XINT2 // Enable CPU INT1 for TINT0: IER |= M_INT1; PieCtrlRegs.PIEIER2.bit.INTx1 = 1; IER |= M_INT2; // Enable Global realtime interrupt DBGM EALLOW; GpioIntRegs.GPIOXINT1SEL.bit.GPIOSEL = 16; //定位到哪个引脚中断 GpioIntRegs.GPIOXINT2SEL.bit.GPIOSEL = 24; // XINT2 is GPIO24 EDIS; XIntruptRegs.XINT1CR.bit.POLARITY = 0; // 1为上升沿，0和2为下降沿，3为双边沿 XIntruptRegs.XINT2CR.bit.POLARITY = 3; // 1为上升沿，0和2为下降沿，3为双边沿 //中断配置步骤-----1,开启模块中断使能， XIntruptRegs.XINT1CR.bit.ENABLE = 1; XIntruptRegs.XINT2CR.bit.ENABLE = 1; // Enable XINT2 // Enable global Interrupts and higher priority real-time debug events: EINT; // Enable Global interrupt INTM ERTM; // Enable Global realtime interrupt DBGM // Initialize PWM module pwm1.PeriodMax = (SYSTEM_FREQUENCY/PWM_FREQUENCY)*1000/2; // Asymmetric PWM 20kHZ 3000 pwm1.DutyFunc = 0; // 占空比ALIGN_DUTY/32767 // DutyFunc = Q15 BLDCPWM_INIT_MACRO(1,2,3,&pwm1); // Initialize PWMDAC module pwmdac1.PeriodMax = 500; // @60Mhz, 1500->20kHz, 1000-> 30kHz, 500->60kHz pwmdac1.HalfPerMax = pwmdac1.PeriodMax/2; // Needed to adjust the duty cycle range in the macro PWMDAC_INIT_MACRO(6,pwmdac1) ; // PWM 6A,6B PWMDAC_INIT_MACRO(7,pwmdac1) ; // PWM 7A,7B // Initialize Hall module hall1.DebounceAmount = 1; hall1.Revolutions = 1; HALL3_INIT_MACRO(&hall1);//读hall值 初始化HallGpioBuffer,HallGpioAccepted // Initialize the SPEED_PR module speed1.InputSelect = 0; speed1.BaseRpm = (Uint32)120*BASE_FREQ/POLES;//(Uint32)120*BASE_FREQ/POLES/6;//// 使用一个换相周期//额定转速 = 60*(基频/(极数/2)) = 60*f/极对数 speed1.SpeedScaler = (Uint32)((Uint32)ISR_FREQUENCY*10000/BASE_FREQ);//保留一位小数//40000/f // For the kits < Rev 1.1 ------------------------------------------------- ChSel[0]=15; // Dummy meas. avoid 1st sample issue Rev0 Picollo ChSel[1]=2; // ChSelect: ADC B7-> Phase A Voltage ChSel[2]=3; // ChSelect: ADC B6-> Phase B Voltage ChSel[3]=4; // ChSelect: ADC B4-> Phase C Voltage ChSel[4]=7; // ChSelect: ADC A2-> DC Bus vol ChSel[5]=6; // DC current 放大20倍 Imeasure = I*5mΩ*20 ChSel[6]=5; // NTC ChSel[7]=1; // VOT //------------------------------------------------------------------------- ADC_MACRO_INIT(ChSel,TrigSel,ACQPS); // Initialize RMP2 module rmp2.Out = (int32)0; rmp2.Ramp2Delay = 0x00000001; rmp2.Ramp2Max = 32767/2; rmp2.Ramp2Min = -32768/2; // Initialize RMP3 module rmp3.DesiredInput = CmtnPeriodTarget; rmp3.Ramp3Delay = RampDelay; rmp3.Out = CmtnPeriodSetpt; rmp3.Ramp3Min = 0x00000010; InitVar(); //Call HVDMC Protection function HVDMC_Protection(); EnableDog(); // IDLE loop. Just sit and loop forever: for(;;) //infinite loop { if(IsrTicker%MAINLOOPDIV ==0) {//8Khz mainLoop(); } } } //END MAIN CODE //================================================================================= // STATE-MACHINE SEQUENCING AND SYNCRONIZATION FOR SLOW BACKGROUND TASKS //================================================================================= // ============================================================================== // =============================== MAIN ISR ===================================== // ============================================================================== interrupt void MainISR(void) { //-------------------------------------------------------------------------------------- ServiceDog(); // Verifying the ISR startuptimer++; startuptimer = (startuptimer>60000)? 60000 : startuptimer;//1.5S IsrTicker++; IsrTicker = (IsrTicker>20000)? 0 : IsrTicker; VirtualTimer++; VirtualTimer &= 0x00007FFF; HALL3_READ_MACRO(&hall1); ClosedFlag = TRUE; if (ClosedFlag==TRUE) { if (hall1.CmtnTrigHall==0x7FFF) { hallCmtnTrig = 1; speed_direction(hall1.HallGpioAccepted,PreviousState); if (hall1.HallGpioAccepted==5) { pwm1.CmtnPointerIn = 5; } else if (hall1.HallGpioAccepted==1) { pwm1.CmtnPointerIn = 0; } else if (hall1.HallGpioAccepted==3) { pwm1.CmtnPointerIn = 1; } else if (hall1.HallGpioAccepted==2) { pwm1.CmtnPointerIn = 2; } else if (hall1.HallGpioAccepted==6) { pwm1.CmtnPointerIn = 3; } else if (hall1.HallGpioAccepted==4) { pwm1.CmtnPointerIn = 4; } PreviousState = hall1.HallGpioAccepted; } // delay } // ClosedFlag==TRUE if(Enable_ALLOW == 0 && pwm1.DutyFunc==0){ bldcpwm_close(); }else{ BLDCPWM_MACRO(1,2,3,&pwm1); } // Acknowledge interrupt to recieve more interrupts from PIE group 1 CpuTimer0.RegsAddr->TCR.bit.TIF=1; PieCtrlRegs.PIEACK.bit.ACK1 = 1; //PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; }// ISR Ends Here /**********************************************************/ /***************Protection Configuration*******************/ /**********************************************************/ void HVDMC_Protection(void) { // Configure Trip Mechanism for the Motor control software // -Cycle by cycle trip on CPU halt // -One shot IPM trip zone trip // These trips need to be repeated for EPWM1 ,2 & 3 //=========================================================================== //Motor Control Trip Config, EPwm1,2,3 //=========================================================================== EALLOW; // CPU Halt Trip 使能TZ6周期触发，周期触发可以自动清零 但是一次触发不能自动清零 // EPwm1Regs.TZSEL.bit.CBC6=0x1;//Enable TZ6 as a CBC trip source for this ePWM module // EPwm2Regs.TZSEL.bit.CBC6=0x1; // EPwm3Regs.TZSEL.bit.CBC6=0x1; //使能故障的单次触发 EPwm1Regs.TZSEL.bit.OSHT1 = 1; //enable TZ1 for OSHT EPwm2Regs.TZSEL.bit.OSHT1 = 1; //enable TZ1 for OSHT EPwm3Regs.TZSEL.bit.OSHT1 = 1; //enable TZ1 for OSHT EPwm1Regs.TZEINT.bit.OST = 1; // What do we want the OST/CBC events to do? // TZA events can force EPWMxA // TZB events can force EPWMxB //故障发生时的输出状态为低 EPwm1Regs.TZCTL.bit.TZA = TZ_FORCE_LO; // EPWMxA will go low EPwm1Regs.TZCTL.bit.TZB = TZ_FORCE_LO; // EPWMxB will go low EPwm2Regs.TZCTL.bit.TZA = TZ_FORCE_LO; // EPWMxA will go low EPwm2Regs.TZCTL.bit.TZB = TZ_FORCE_LO; // EPWMxB will go low EPwm3Regs.TZCTL.bit.TZA = TZ_FORCE_LO; // EPWMxA will go low EPwm3Regs.TZCTL.bit.TZB = TZ_FORCE_LO; // EPWMxB will go low EDIS; // Clear any spurious OV trip 清除标志位 EPwm1Regs.TZCLR.bit.OST = 1; EPwm2Regs.TZCLR.bit.OST = 1; EPwm3Regs.TZCLR.bit.OST = 1; //************************** End of Prot. Conf. ***************************// } //速度计算中断 interrupt void xint2_isr(void) { static int32 last_XintTime = TIMER1_PER; static Uint32 last_hall = 0; Uint32 temp = 0; if(CpuTimer1.RegsAddr->TCR.bit.TIF == 1){ // 速度为0 XintTime = (int32)TIMER1_PER; CpuTimer1.RegsAddr->TCR.bit.TIF = 1;//写1清中断标志位 }else{ XintTime = (int32)TIMER1_PER - CpuTimer1.RegsAddr->TIM.all; } //XintTime = (int32)TIMER1_PER - CpuTimer1.RegsAddr->TIM.all; temp = (GpioDataRegs.GPADAT.all>>24)&0x00000007; /* read all three GPIOs at once*/ if(_IQabs(XintTime)<36000 || temp == last_hall ){// 不应该超过10000rpm 也不应该和上一次的hall相同 否则说明有振动 //filter 防抖 XintTime = XintTime + last_XintTime; PieCtrlRegs.PIEACK.bit.ACK1 =1; last_XintTime = XintTime; last_hall = temp; if(Direction == -1){ XintTime = -XintTime; } return ; } last_XintTime = XintTime; last_hall = temp; if(Direction == -1){ XintTime = -XintTime; } Xint2Cnt = 1; CpuTimer1.RegsAddr->TCR.bit.TRB = 1; //重新装载 CpuTimer1.RegsAddr->TCR.bit.TSS = 0;//定时器restart PieCtrlRegs.PIEACK.bit.ACK1 =1; } interrupt void xint1_isr(void) { FaultFlag.bit.Ipmfault = 1; PieCtrlRegs.PIEACK.bit.ACK1 =1; // PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; } interrupt void EPWM1TZint_isr(void) { //過流 FaultFlag.bit.OverCurFlag = 1; EALLOW; // EPwm1Regs.TZCLR.bit.OST = 1; // EPwm1Regs.TZCLR.bit.INT = 1; EDIS; PieCtrlRegs.PIEACK.bit.ACK2 = 1;//PIEACK_GROUP2; } void ServiceDog(void) { EALLOW; SysCtrlRegs.WDKEY = 0x0055; SysCtrlRegs.WDKEY = 0x00AA; EDIS; } //--------------------------------------------------------------------------- // Example: DisableDog: //--------------------------------------------------------------------------- // This function disables the watchdog timer. //--------------------------------------------------------------------------- // 使能看门狗 //--------------------------------------------------------------------------- void EnableDog(void) { EALLOW; SysCtrlRegs.WDCR= 0x0028; EDIS; } void OpenPwm(void) { EALLOW; EPwm1Regs.TZCLR.bit.OST = 1; //EnablePWM1 EPwm2Regs.TZCLR.bit.OST = 1; //EnablePWM2 EPwm3Regs.TZCLR.bit.OST = 1; //EnablePWM3 EPwm1Regs.TZCLR.bit.INT = 1; EDIS; } void ClosePwm(void) { EALLOW; EPwm1Regs.TZFRC.bit.OST = 0x1; //disabPWMl EPwm2Regs.TZFRC.bit.OST = 0x1; //disabPWM2 EPwm3Regs.TZFRC.bit.OST = 0x1; //disabPWM3 EDIS; } void speed_direction(Uint16 hall,Uint16 hall_last){ //防抖 static int16 i = 0; static int16 j=0; if(hall_last == 2 && hall == 6 || hall_last == 6 && hall == 4 ||hall_last == 4 && hall == 5 ||hall_last == 5 && hall == 1||hall_last == 1 && hall == 3||hall_last == 3 && hall == 2) { i++; j--; if(j<0){ j=0; } if(i>5){ i = 6; Direction = 1; } } else if(hall_last == 4 && hall == 6 || hall_last == 6 && hall == 2 ||hall_last == 2 && hall == 3 ||hall_last == 3 && hall == 1||hall_last == 1 && hall == 5|| hall_last == 5 && hall == 4) { i--; if(i<0){ i=0; } j++; if(j>5){ j= 6; Direction = -1; } } } void speed_cal_filter(void){ int32 RotarRPMNew = 0; int32 RotarrpmOut = 0; static int32 last_speed = 0; static int32 last_RotarRPMNew = 0; Uint16 Halltemp = (Uint16)((GpioDataRegs.GPADAT.all>>24)&0x00000007); /* read all three GPIOs at once*/ static Uint16 Hall_1 = 0; //static Uint16 Hall_2 = 0; static int32 Hall_2_cnt = 0; int16 directiontemp = 0; if(CpuTimer1.RegsAddr->TCR.bit.TIF == 1){ // 速度为0 XintTime = TIMER1_PER; Xint2Cnt = 1; RotarRPMNew =0; last_speed = 0; CpuTimer1.RegsAddr->TCR.bit.TIF = 1;//写1清零 }else{ if(Halltemp == hall1.HallGpioAccepted){//hall读取不变 Hall_2_cnt++; if(Hall_2_cnt>MAINLOOPFREQ/2){//0.5s Hall_2_cnt = MAINLOOPFREQ/2+1; } }else{ Hall_2_cnt = 0; } if(_IQabs(XintTime) == TIMER1_PER || Hall_2_cnt>=4000){ RotarRPMNew = 0; last_speed = 0; }else{ RotarRPMNew =(int32)360000000/XintTime;//计算新速度 } } if(Xint2Cnt == 1){ //防止突然反向 Xint2Cnt = 0; if(Hall_1 == 2 && Halltemp == 6 || Hall_1 == 6 && Halltemp == 4 ||Hall_1 == 4 && Halltemp == 5 ||Hall_1 == 5 && Halltemp == 1||Hall_1 == 1 && Halltemp == 3||Hall_1 == 3 && Halltemp == 2) { directiontemp = 1; } else if(Hall_1 == 4 && Halltemp == 6 || Hall_1 == 6 && Halltemp == 2 ||Hall_1 == 2 && Halltemp == 3 ||Hall_1 == 3 && Halltemp == 1||Hall_1 == 1 && Halltemp == 5|| Hall_1 == 5 && Halltemp == 4) { directiontemp = -1; } if((Direction ==1 && directiontemp == -1 )||(Direction == -1 && directiontemp == 1 )){ //说明在振动 RotarRPMNew = 0; last_speed = 0; } Hall_1 = Halltemp; } test2 = RotarRPMNew; if(_IQabs(RotarRPMNew - last_RotarRPMNew)>1500 &&(_IQabs(RotarRPMNew)<300 ||_IQabs(last_RotarRPMNew)<300) ){ //在0转速附近有大的阶跃 test = RotarRPMNew; RotarRPMNew = 0; last_speed = 0; last_RotarRPMNew = 0; }else if(_IQabs(RotarRPMNew - last_RotarRPMNew)>1500&&(_IQabs(RotarRPMNew)<1500 ||_IQabs(last_RotarRPMNew)<1500)){ //低转速附近有大的阶跃 if(_IQabs(RotarRPMNew)>_IQabs(last_RotarRPMNew)){ test3 = RotarRPMNew; test5 = last_RotarRPMNew; RotarRPMNew = last_RotarRPMNew; } } /* if(RotarRPMNew - last_RotarRPMNew>500){ RotarRPMNew = last_RotarRPMNew+4; }else if(RotarRPMNew - last_RotarRPMNew<-500){ RotarRPMNew = last_RotarRPMNew-4; }*/ //last_RotarRPMNew = RotarRPMNew; RotarrpmOut = lowpassfilter(last_speed,RotarRPMNew,_IQ12(0.584),_IQ12(0.125)); last_speed = RotarrpmOut; speed1.Speed = _IQdiv(RotarrpmOut,speed1.BaseRpm); speed1.SpeedRpm = RotarrpmOut; last_RotarRPMNew = speed1.SpeedRpm; // } } void speed_cal(void){ int32 RotarRPMNew = 0; int32 RotarrpmOut = 0; static int32 RotarrpmSum = 0; static int32 RotarrpmArr[8] = {0,0,0,0,0,0,0,0}; static int16 k = 0; if(CpuTimer1.RegsAddr->TCR.bit.TIF == 1){ // 速度为0 XintTime = TIMER1_PER; Xint2Cnt = 1; CpuTimer1.RegsAddr->TCR.bit.TIF = 1;//写1清零 int count = 0; for(count = 0;count<8;count++){ RotarrpmArr[count] = 0; } RotarrpmSum = 0; RotarRPMNew =0; }else{ RotarRPMNew =(int32)360000000/XintTime; } if(Xint2Cnt == 1){ Xint2Cnt = 0; RotarrpmArr[k] = RotarRPMNew; RotarrpmSum += RotarrpmArr[k]; RotarrpmOut = RotarrpmSum>>3; k++; if (k >= 8) { k = 0; } RotarrpmSum -= RotarrpmArr[k]; speed1.Speed = _IQdiv(RotarrpmOut,speed1.BaseRpm); speed1.SpeedRpm = RotarrpmOut; } } void pwmlimit_speed(void){ if(Enable_ALLOW == FALSE){ if(_IQabs(speed1.SpeedRpm)< 200){ startuptimer = 0; pwmlimit = 1300; } } if(_IQabs(speed1.SpeedRpm)< 200 && startuptimer>=60000){ Uint16 index = _IQabs(speed1.SpeedRpm)/50; if(speed1.SpeedRpm>0){ pwmlimit = (Uint32)(_IQabs(speed1.SpeedRpm)-index*50)*(pwm_limit_table1[index+1] - pwm_limit_table1[index])/50+pwm_limit_table1[index]; }else{ pwmlimit = (Uint32)(_IQabs(speed1.SpeedRpm)-index*50)*(pwm_limit_table1_NEG[index+1] - pwm_limit_table1_NEG[index])/50+pwm_limit_table1_NEG[index]; } }else if(_IQabs(speed1.SpeedRpm)< 3500 && startuptimer>=60000){ Uint16 index = _IQabs(speed1.SpeedRpm)/100; if(speed1.SpeedRpm>0){ pwmlimit = (Uint32)(_IQabs(speed1.SpeedRpm)-index*100)*(pwm_limit_table2[index+1] - pwm_limit_table2[index])/100+pwm_limit_table2[index]; }else{ pwmlimit = (Uint32)(_IQabs(speed1.SpeedRpm)-index*100)*(pwm_limit_table_neg[index+1] - pwm_limit_table_neg[index])/100+pwm_limit_table_neg[index]; } }else if(startuptimer>=60000){ if(speed1.SpeedRpm>0){ pwmlimit = pwm_limit_table2[36];//最大值 }else{ pwmlimit = pwm_limit_table_neg[36];//最大值 } }else{ } } void taskfree(void){ if(SerialCommsTimer>(MAINLOOPFREQ/4)){ GpioDataRegs.GPATOGGLE.bit.GPIO22 = 1; SerialCommsTimer = 0; DCbus_voltage = AdcResult.ADCRESULT4/10;// U = Uadc * 0.099964 Tvot = ((Uint32)AdcResult.ADCRESULT7 *kscaler - kb)>>20; int32 R = 10000;//分压电阻 int32 Rtadc = AdcResult.ADCRESULT6; Rt= Rtadc*R/(4096-Rtadc); if(Rt>32116){ Tmotor = 0; } else if(Rt>15652){ Tmotor = (ktable[0]*Rt/1000 + btable[0])>>12; }else if(Rt>6523){ Tmotor = (ktable[1]*Rt/1000 + btable[1])>>12; }else if(Rt>2968){ Tmotor = (ktable[2]*Rt/1000 + btable[2])>>12; }else if(Rt>1228){ Tmotor = (ktable[3]*Rt/1000 + btable[3])>>12; }else if(Rt > 657){ Tmotor = (ktable[4]*Rt/1000 + btable[4])>>12; }else if(Rt > 324){ Tmotor = (ktable[5]*Rt/1000 + btable[5])>>12; }else{ Tmotor = 125; } } } int16 MastIsrTimePercent(void) { Uint32 T1 = 0; int16 MasterIsrT =0; static int32 T1_L = 60000000; T1 = CpuTimer2.RegsAddr->TIM.all; if(T1_L < T1){ MasterIsrT = (Uint32)(T1_L +CpuTimer2.RegsAddr->PRD.all -T1)* 100 /9000; }else{ MasterIsrT = (Uint32)(T1_L-T1)* 100 /9000; } T1_L = T1; // MasterIsrT = (Uint32)MasterIsrT* 100 /3000; return MasterIsrT; } void mainLoop(void){ Uint32 T1 = 0; Uint32 T2 = 0; T1 = CpuTimer2.RegsAddr->TIM.all; CanMaster(); SerialCommsTimer++; if(SerialCommsTimer>0x3FFF){ SerialCommsTimer = 0x3FFF; } //speed_cal(); speed_cal_filter(); pwmlimit_speed(); taskfree(); FaultTreat(); // =============================== LEVEL 4 ====================================== // Level 4 verifies the closed current loop and current PI controller. // ============================================================================== //-------------平均电流和can------------------------------- static _iq last_DC_current=0; static Uint16 DC_Current_cnt = 0; //static int32 DC_Current_sum = 0; static int32 DC_Current_sum_filter = 0; int32 dt = _IQ12(0.1); int32 cutoff = _IQ12(1); DC_current_real = (Uint32)AdcResult.ADCRESULT5*33;//Q12 I真实 = adc * 3.3 / 4096 /0.1 DC_current_filer = lowpassfilter(last_DC_current,DC_current_real,cutoff,dt); last_DC_current = DC_current_filer; DC_Current_sum_filter += DC_current_filer*pwm1.DutyFunc>>15; DC_Current_cnt++;//由于stall最多16384/20K秒 所以最多0.819715秒计算一次平均值 if(hallCmtnTrig==1){ hallCmtnTrig = 0; DC_current_filter_avr = DC_Current_sum_filter/DC_Current_cnt; DC_Current_sum_filter =0; DC_Current_cnt = 0; } #ifdef DEBUG_CAN carveData1(); CanCurve(); #endif UserCANprocess(); #if (BUILDLEVEL==LEVEL5) // ------------------------------------------------------------------------------ // ADC conversion and offset adjustment (observing back-emfs is optinal for this prj.) // ------------------------------------------------------------------------------ BemfA = AdcResult.ADCRESULT1; BemfB = AdcResult.ADCRESULT2; BemfC = AdcResult.ADCRESULT3; #ifdef SPEEDCLOSED pid1_spd.Ref = SpeedRef;//rc1.SetpointValue; pid1_spd.Fbk = speed1.Speed; #endif if(EnableFlag == 0 || FaultFlag.all !=0){ Enable_ALLOW = FALSE; }else{ Enable_ALLOW = TRUE; } if(Enable_ALLOW == FALSE){ resetPI(&pid1_spd); rmp2.DesiredInput = 0; RC2_MACRO(&rmp2); pwm1.DutyFuncIn = (int16)_IQtoIQ15(rmp2.Out); // controlled speed duty-cycle }else{ int16 pwmtmp = 0; pid1_spd.Umax = _IQ15toIQ(pwmlimit);//_IQ((float)pwmlimit/32767); pid1_spd.Umin = -pid1_spd.Umax;//_IQ((float)(-pwmlimit)/32767); rmp2.Ramp2Max = pwmlimit; rmp2.Ramp2Min = -pwmlimit; #ifdef SPEEDCLOSED PI_MACRO(&pid1_spd); pwmtmp =(int16)_IQtoIQ15(pid1_spd.Out); rmp2.DesiredInput = pwmtmp; #else rmp2.DesiredInput = PwmSet; #endif RC2_MACRO(&rmp2); pwmtmp = rmp2.Out; pwm1.DutyFuncIn = ((int32)pwmtmp*LimitMotCtrTemp(600000))>>15;//1分钟降一次 if(pwm1.DutyFuncIn>pwmlimit){ pwm1.DutyFuncIn = pwmlimit; // controlled speed duty-cycle }else if(pwm1.DutyFuncIn<-pwmlimit){ pwm1.DutyFuncIn = -pwmlimit; // controlled speed duty-cycle } //rmp2.Out = pwm1.DutyFuncIn; } #endif // (BUILDLEVEL==LEVEL5) //程序占用率 T2 = CpuTimer2.RegsAddr->TIM.all; if(T1 < T2){ IsrTime = (Uint32)(T1 +CpuTimer2.RegsAddr->PRD.all -T2)* 100 /7500; }else{ IsrTime = (Uint32)(T1-T2)* 100 /7500; } } void bldcpwm_close(void){ EPwm1Regs.AQCSFRC.bit.CSFA = 1; /* Forcing a continuous Low on output A of EPWM3 */ EPwm1Regs.AQCSFRC.bit.CSFB = 1; /* Forcing a continuous Low on output B of EPWM3 */ EPwm2Regs.AQCSFRC.bit.CSFA = 1; /* Forcing a continuous Low on output A of EPWM3 */ EPwm2Regs.AQCSFRC.bit.CSFB = 1; /* Forcing a continuous Low on output B of EPWM3 */ EPwm3Regs.AQCSFRC.bit.CSFA = 1; /* Forcing a continuous Low on output A of EPWM3 */ EPwm3Regs.AQCSFRC.bit.CSFB = 1; /* Forcing a continuous Low on output B of EPWM3 */ }