|
@@ -23,8 +23,8 @@ Description: Primary system file for the (Trapezoidal) Sensored BLDC Control
|
|
|
void MemCopy();
|
|
|
void InitFlash();
|
|
|
#endif
|
|
|
-#define TIMER1_PER1 60
|
|
|
-#define TIMER1_PER2 500000
|
|
|
+#define TIMER1_S1 60
|
|
|
+#define TIMER1_S2 500000
|
|
|
#define TIMER1_PER 30000000
|
|
|
// Prototype statements for functions found within this file.
|
|
|
interrupt void MainISR(void);//主中断
|
|
@@ -37,8 +37,9 @@ 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);
|
|
@@ -144,6 +145,8 @@ Uint16 pwm_limit_table2[]= {950,1650,2320,3050,3750,4460,5200,5929,6623,7358,805
|
|
|
22602,23232,23935,24609,25240};
|
|
|
|
|
|
int32 pwmlimitrate = 32768;
|
|
|
+
|
|
|
+Uint32 IsrTicker=0;
|
|
|
void main(void)
|
|
|
{
|
|
|
|
|
@@ -177,10 +180,10 @@ void main(void)
|
|
|
StartCpuTimer0();
|
|
|
|
|
|
// Configure CPU-Timer 1,2 for background loops
|
|
|
- ConfigCpuTimer(&CpuTimer1, TIMER1_PER1, TIMER1_PER2);//2hz
|
|
|
- // ConfigCpuTimer(&CpuTimer2, 60, 50000);//20HZ
|
|
|
+ ConfigCpuTimer(&CpuTimer1, TIMER1_S1, TIMER1_S2);//2hz
|
|
|
+ ConfigCpuTimer(&CpuTimer2, 60, 1000000);//20HZ
|
|
|
StartCpuTimer1();
|
|
|
- // StartCpuTimer2();
|
|
|
+ StartCpuTimer2();
|
|
|
|
|
|
|
|
|
// Reassign ISRs.
|
|
@@ -211,7 +214,7 @@ void main(void)
|
|
|
GpioIntRegs.GPIOXINT1SEL.bit.GPIOSEL = 16; //定位到哪个引脚中断
|
|
|
GpioIntRegs.GPIOXINT2SEL.bit.GPIOSEL = 24; // XINT2 is GPIO24
|
|
|
EDIS;
|
|
|
- XIntruptRegs.XINT1CR.bit.POLARITY = 1; // 1为上升沿,0和2为下降沿,3为双边沿
|
|
|
+ XIntruptRegs.XINT1CR.bit.POLARITY = 0; // 1为上升沿,0和2为下降沿,3为双边沿
|
|
|
XIntruptRegs.XINT2CR.bit.POLARITY = 3; // 1为上升沿,0和2为下降沿,3为双边沿
|
|
|
//中断配置步骤-----1,开启模块中断使能,
|
|
|
XIntruptRegs.XINT1CR.bit.ENABLE = 1;
|
|
@@ -280,13 +283,10 @@ void main(void)
|
|
|
// IDLE loop. Just sit and loop forever:
|
|
|
for(;;) //infinite loop
|
|
|
{
|
|
|
- //BackTicker++;
|
|
|
- taskfree();
|
|
|
- // State machine entry & exit point
|
|
|
- //===========================================================
|
|
|
- //(*Alpha_State_Ptr)(); // jump to an Alpha state (A0,B0,...)
|
|
|
- //===========================================================
|
|
|
-
|
|
|
+ if(IsrTicker%2 ==0)
|
|
|
+ {
|
|
|
+ mainLoop();
|
|
|
+ }
|
|
|
}
|
|
|
} //END MAIN CODE
|
|
|
|
|
@@ -301,468 +301,17 @@ void main(void)
|
|
|
// =============================== MAIN ISR =====================================
|
|
|
// ==============================================================================
|
|
|
|
|
|
-Uint32 IsrTicker=0;
|
|
|
-interrupt void MainISR(void)
|
|
|
|
|
|
+interrupt void MainISR(void)
|
|
|
{
|
|
|
|
|
|
//--------------------------------------------------------------------------------------
|
|
|
|
|
|
// Verifying the ISR
|
|
|
IsrTicker++;
|
|
|
- CanMaster();
|
|
|
- SerialCommsTimer++;
|
|
|
- if(SerialCommsTimer>0x3FFF){
|
|
|
- SerialCommsTimer = 0x3FFF;
|
|
|
- }
|
|
|
-
|
|
|
- ServiceDog();
|
|
|
- speed_cal();
|
|
|
- pwmlimit_speed();
|
|
|
- taskfree();
|
|
|
- FaultTreat();
|
|
|
-#if (BUILDLEVEL==LEVEL1)
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the RMP3 module and call the RAMP Control 3 macro.
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- rmp3.DesiredInput = CmtnPeriodTarget;
|
|
|
- rmp3.Ramp3Delay = RampDelay;
|
|
|
- RC3_MACRO(&rmp3);
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the IMPULSE module and call the Impulse macro.
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- impl1.Period = rmp3.Out;
|
|
|
- IMPULSE_MACRO(&impl1);
|
|
|
- HALL3_READ_MACRO(&hall1);
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the MOD6 module and call the Mod 6 counter macro.
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- mod1.TrigInput = impl1.Out;
|
|
|
- MOD6CNT_MACRO(&mod1);
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the PWM_DRV module and call the PWM signal generation
|
|
|
-// update macro.
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- pwm1.CmtnPointerIn = test2;//(int16)mod1.Counter;
|
|
|
- if(EnableFlag == 0){
|
|
|
- pwm1.DutyFuncIn = 0;
|
|
|
- }else{
|
|
|
- pwm1.DutyFuncIn = test;
|
|
|
- }
|
|
|
-
|
|
|
- BLDCPWM_MACRO(1,2,3,&pwm1);
|
|
|
-
|
|
|
-
|
|
|
-#endif // (BUILDLEVEL==LEVEL1)
|
|
|
-// =============================== LEVEL 3 ======================================
|
|
|
-// Level 3 describes the closed-loop operation of sensored trapezoidal
|
|
|
-// drive of BLDC motor using Hall sensor.
|
|
|
-// ==============================================================================
|
|
|
-
|
|
|
-#if (BUILDLEVEL==LEVEL3)
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// ADC conversion and offset adjustment (observing back-emfs is optinal for this prj.)
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- BemfA = _IQ12toIQ(AdcResult.ADCRESULT1);
|
|
|
- BemfB = _IQ12toIQ(AdcResult.ADCRESULT2);
|
|
|
- BemfC = _IQ12toIQ(AdcResult.ADCRESULT3);
|
|
|
- DCbus_current = _IQ12toIQ(AdcResult.ADCRESULT4);//-_IQ(0.5); //1.65V offset added on HVDMC board.
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the RMP3 module and call the Ramp control 3 macro.
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-
|
|
|
-
|
|
|
- HALL3_READ_MACRO(&hall1);
|
|
|
-
|
|
|
- ClosedFlag=TRUE;
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the RMP2 module and call the Ramp control 2 macro.
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- rmp2.DesiredInput = (int32)DFuncDesired;
|
|
|
- RC2_MACRO(&rmp2);
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the PWM_DRV module and call the PWM signal generation
|
|
|
-// update macro.
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- if (ClosedFlag==TRUE) {
|
|
|
- if (hall1.CmtnTrigHall==0x7FFF) {
|
|
|
-
|
|
|
- PreviousState = pwm1.CmtnPointer;
|
|
|
-
|
|
|
-// Comment the following if-else-if statements in case of
|
|
|
-// inverted Hall logics for commutation states.
|
|
|
- /* if (hall1.HallGpioAccepted==5)
|
|
|
- { pwm1.CmtnPointer = 0;
|
|
|
-
|
|
|
- }
|
|
|
- else if (hall1.HallGpioAccepted==1)
|
|
|
- { pwm1.CmtnPointer = 1;
|
|
|
-
|
|
|
- }
|
|
|
- else if (hall1.HallGpioAccepted==3)
|
|
|
- { pwm1.CmtnPointer = 2;
|
|
|
-
|
|
|
- }
|
|
|
- else if (hall1.HallGpioAccepted==2)
|
|
|
- { pwm1.CmtnPointer = 3;
|
|
|
-
|
|
|
- }
|
|
|
- else if (hall1.HallGpioAccepted==6)
|
|
|
- { pwm1.CmtnPointer = 4;
|
|
|
-
|
|
|
- }
|
|
|
- else if (hall1.HallGpioAccepted==4)
|
|
|
- {
|
|
|
- pwm1.CmtnPointer = 5;
|
|
|
- }*/
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-// Comment the following if-else-if statements in case of
|
|
|
-// non-inverted Hall logics for commutation states.
|
|
|
- if (hall1.HallGpioAccepted==2)
|
|
|
- pwm1.CmtnPointer = 0;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==6)
|
|
|
- pwm1.CmtnPointer = 1;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==4)
|
|
|
- pwm1.CmtnPointer = 2;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==5)
|
|
|
- pwm1.CmtnPointer = 3;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==1)
|
|
|
- pwm1.CmtnPointer = 4;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==3)
|
|
|
- pwm1.CmtnPointer = 5;
|
|
|
-
|
|
|
-
|
|
|
- } //hall1.CmtnTrigHall == 0x7FFF
|
|
|
- } // ClosedFlag==TRUE
|
|
|
- else{
|
|
|
- pwm1.CmtnPointer = (int16)mod1.Counter;
|
|
|
- }
|
|
|
- pwm1.DutyFunc = DfuncTesting;
|
|
|
- BLDCPWM_MACRO(1,2,3,&pwm1);
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the SPEED_PR module and call the speed calculation macro.
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- if ((pwm1.CmtnPointer==5)&&(PreviousState==4)&&(hall1.CmtnTrigHall==0x7FFF))
|
|
|
- {
|
|
|
- speed1.TimeStamp = VirtualTimer;
|
|
|
- SPEED_PR_MACRO(&speed1);
|
|
|
- test = (test ==0 )? 1 : 0;
|
|
|
- }
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the PWMDAC module
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- pwmdac1.MfuncC1 = (int32)hall1.HallGpioAccepted<<20;
|
|
|
- pwmdac1.MfuncC2 = BemfA;
|
|
|
- PWMDAC_MACRO(6,pwmdac1) // PWMDAC 6A, 6B
|
|
|
-
|
|
|
- pwmdac1.MfuncC1 = DCbus_current;
|
|
|
- pwmdac1.MfuncC2 = (int32)mod1.Counter<<20;
|
|
|
- PWMDAC_MACRO(7,pwmdac1) // PWMDAC 7A, 7B
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the DATALOG module
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- DlogCh2 = (int16)mod1.Counter;
|
|
|
- DlogCh1 = (int16)hall1.HallGpioAccepted;
|
|
|
- DlogCh3 = (int16)_IQtoIQ15(BemfA);
|
|
|
- DlogCh4 = (int16)_IQtoIQ15(BemfB);
|
|
|
-
|
|
|
-
|
|
|
-#endif // (BUILDLEVEL==LEVEL3)
|
|
|
-
|
|
|
-// =============================== 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;
|
|
|
-
|
|
|
- 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,_IQ12(1),_IQ12(0.05));
|
|
|
- last_DC_current = DC_current_filer;
|
|
|
- carveData1();
|
|
|
- CanCurve();
|
|
|
-
|
|
|
- UserCANprocess();
|
|
|
- if(Fault_clear == 1) {
|
|
|
- static int16 fault_cnt = 0;
|
|
|
- GpioDataRegs.GPBCLEAR.bit.GPIO32 = 1;
|
|
|
- FaultFlag.bit.OverCurFlag = 0;
|
|
|
- fault_cnt++;
|
|
|
- if(fault_cnt>20000){
|
|
|
- Fault_clear = 0;
|
|
|
- fault_cnt = 0;
|
|
|
-
|
|
|
- }
|
|
|
- }else{
|
|
|
- GpioDataRegs.GPBSET.bit.GPIO32 = 1;
|
|
|
- }
|
|
|
- DC_Current_sum_filter += DC_current_filer*pwm1.DutyFunc>>15;
|
|
|
- // DC_Current_sum += DC_current_real*pwm1.DutyFunc>>15;
|
|
|
- DC_Current_cnt++;//由于stall最多16384/20K秒 所以最多0.819715秒计算一次平均值
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-#if (BUILDLEVEL==LEVEL4)
|
|
|
- BemfA = AdcResult.ADCRESULT1;
|
|
|
- BemfB = AdcResult.ADCRESULT2;
|
|
|
- BemfC = AdcResult.ADCRESULT3;
|
|
|
-
|
|
|
- HALL3_READ_MACRO(&hall1);
|
|
|
-
|
|
|
- if (hall1.Revolutions>=0)
|
|
|
- ClosedFlag=TRUE;
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// 电流闭环
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- pid1_spd.Ref = _IQ(0.3);
|
|
|
- pid1_spd.Fbk = _IQabs(speed1.Speed);
|
|
|
- // tempIdc=pid1_idc.Fbk;
|
|
|
- pid1_idc.Umax = pwmlimit<<9;
|
|
|
- pid1_idc.Umin = (-pwmlimit)<<9;
|
|
|
- pid1_idc.Ref = CurrentSet;//给定电流 反转给-
|
|
|
- if(speed1.SpeedRpm<0){
|
|
|
- pid1_idc.Fbk = -_IQ12toIQ(DC_current_filter_avr);// 电流改为Q15格式
|
|
|
- }else{
|
|
|
- pid1_idc.Fbk = _IQ12toIQ(DC_current_filter_avr);// 电流改为Q15格式
|
|
|
- }
|
|
|
-// if(pid1_idc.Fbk<0) pid1_idc.Fbk=tempIdc; // Eliminate negative values
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- step++;
|
|
|
- if(step == 1){
|
|
|
- PI_MACRO(&pid1_idc);
|
|
|
- }else if(step == 2){
|
|
|
- PI_MACRO(&pid1_spd);
|
|
|
- step = 0;
|
|
|
- }
|
|
|
-
|
|
|
- if(EnableFlag == 0){
|
|
|
- SetPI(&pid1_spd);
|
|
|
- resetPI(&pid1_idc);
|
|
|
- rmp2.DesiredInput = 0;
|
|
|
- RC2_MACRO(&rmp2);
|
|
|
- pwm1.DutyFuncIn = (int16)_IQtoIQ15(rmp2.Out); // controlled speed duty-cycle
|
|
|
-
|
|
|
- }else{
|
|
|
- pwm1.DutyFuncIn = (int16)_IQtoIQ15(_IQmpy(pid1_idc.Out,pid1_spd.Out)); // controlled speed duty-cycle
|
|
|
- rmp2.Out = pwm1.DutyFuncIn;
|
|
|
- // pwm1.DutyFuncIn = (int16)(_IQtoIQ15(pid1_idc.Out));
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- if (ClosedFlag==TRUE)
|
|
|
- {
|
|
|
- if (hall1.CmtnTrigHall==0x7FFF)
|
|
|
- {
|
|
|
- DC_current_avr = DC_Current_sum/DC_Current_cnt;
|
|
|
- DC_current_filter_avr = DC_Current_sum_filter/DC_Current_cnt;
|
|
|
- DC_Current_sum = 0;
|
|
|
- DC_Current_sum_filter =0;
|
|
|
- DC_Current_cnt = 0;
|
|
|
- //计算速度
|
|
|
- speed1.TimeStamp = VirtualTimer;
|
|
|
- SPEED_PR_MACRO(&speed1,hall1.HallGpioAccepted,PreviousState,hall1.Stall_status);
|
|
|
- PreviousState = hall1.HallGpioAccepted;
|
|
|
- if (hall1.HallGpioAccepted==2)
|
|
|
- pwm1.CmtnPointerIn = 0;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==6)
|
|
|
- pwm1.CmtnPointerIn = 1;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==4)
|
|
|
- pwm1.CmtnPointerIn = 2;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==5)
|
|
|
- pwm1.CmtnPointerIn = 3;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==1)
|
|
|
- pwm1.CmtnPointerIn = 4;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==3)
|
|
|
- { pwm1.CmtnPointerIn = 5;
|
|
|
-
|
|
|
- }//hall1.HallGpioAccepted==3
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- BLDCPWM_MACRO(1,2,3,&pwm1);
|
|
|
-
|
|
|
-#endif // (BUILDLEVEL==LEVEL4)
|
|
|
-
|
|
|
-
|
|
|
-// =============================== LEVEL 5 ======================================
|
|
|
-// Level 5 verifies the closed speed loop and speed PI controller.
|
|
|
-// ==============================================================================
|
|
|
-
|
|
|
-#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;
|
|
|
- //DCbus_current = _IQ12toIQ(AdcResult.ADCRESULT5); //采样值<<12
|
|
|
-
|
|
|
- rc1.TargetValue = SpeedRef;
|
|
|
- RC_MACRO(&rc1);
|
|
|
-
|
|
|
- HALL3_READ_MACRO(&hall1);
|
|
|
- pid1_spd.Ref = rc1.SetpointValue;
|
|
|
- pid1_spd.Fbk = speed1.Speed;
|
|
|
-
|
|
|
-
|
|
|
- if(EnableFlag == 0){
|
|
|
- resetPI(&pid1_spd);
|
|
|
- rmp2.DesiredInput = 0;
|
|
|
- RC2_MACRO(&rmp2);
|
|
|
- pwm1.DutyFuncIn = (int16)_IQtoIQ15(rmp2.Out); // controlled speed duty-cycle
|
|
|
-
|
|
|
- }else{
|
|
|
- pid1_spd.Umax = _IQ((float)pwmlimit/32767);
|
|
|
- pid1_spd.Umin = _IQ((float)(-pwmlimit)/32767);
|
|
|
- PI_MACRO(&pid1_spd);
|
|
|
- if((int16)_IQtoIQ15(pid1_spd.Out)>pwmlimit){
|
|
|
- pwm1.DutyFuncIn = pwmlimit; // controlled speed duty-cycle
|
|
|
- }else if((int16)_IQtoIQ15(pid1_spd.Out)<-pwmlimit){
|
|
|
- pwm1.DutyFuncIn = -pwmlimit; // controlled speed duty-cycle
|
|
|
- }else{
|
|
|
- pwm1.DutyFuncIn = (int16)_IQtoIQ15(pid1_spd.Out); // controlled speed duty-cycle
|
|
|
- }
|
|
|
- //pwm1.DutyFuncIn = test;
|
|
|
- rmp2.Out = pwm1.DutyFuncIn;
|
|
|
- }
|
|
|
- if(pwm1.DutyFunc>0){
|
|
|
- OpenPwm();
|
|
|
- }else{
|
|
|
- ClosePwm();
|
|
|
- }
|
|
|
-
|
|
|
- ClosedFlag = TRUE;
|
|
|
- if (ClosedFlag==TRUE)
|
|
|
- {
|
|
|
-
|
|
|
- if (hall1.CmtnTrigHall==0x7FFF)
|
|
|
- {
|
|
|
-
|
|
|
- // DC_current_avr = DC_Current_sum/DC_Current_cnt;
|
|
|
- DC_current_filter_avr = DC_Current_sum_filter/DC_Current_cnt;
|
|
|
- // DC_Current_sum = 0;
|
|
|
- DC_Current_sum_filter =0;
|
|
|
- DC_Current_cnt = 0;
|
|
|
- speed_direction(hall1.HallGpioAccepted,PreviousState);
|
|
|
-
|
|
|
- if (hall1.HallGpioAccepted==5)
|
|
|
- { pwm1.CmtnPointerIn = 5;
|
|
|
- speed1.TimeStamp = VirtualTimer;
|
|
|
-
|
|
|
- // SPEED_PR_MACRO(&speed1,hall1.HallGpioAccepted,PreviousState,hall1.Stall_status);
|
|
|
-
|
|
|
- }
|
|
|
- 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;
|
|
|
- /*
|
|
|
- if (hall1.HallGpioAccepted==2)
|
|
|
- pwm1.CmtnPointerIn = 0;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==6)
|
|
|
- pwm1.CmtnPointerIn = 1;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==4)
|
|
|
- pwm1.CmtnPointerIn = 2;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==5)
|
|
|
- pwm1.CmtnPointerIn = 3;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==1)
|
|
|
- pwm1.CmtnPointerIn = 4;
|
|
|
-
|
|
|
- else if (hall1.HallGpioAccepted==3)
|
|
|
- { pwm1.CmtnPointerIn = 5;
|
|
|
-
|
|
|
- }//hall1.HallGpioAccepted==3
|
|
|
-*/
|
|
|
- //}//direction == Normal
|
|
|
-
|
|
|
- } // delay
|
|
|
- } // ClosedFlag==TRUE
|
|
|
-
|
|
|
-
|
|
|
- BLDCPWM_MACRO(1,2,3,&pwm1);
|
|
|
-
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the PWMDAC module
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- pwmdac1.MfuncC1 = BemfA;
|
|
|
- pwmdac1.MfuncC2 = BemfB;
|
|
|
- PWMDAC_MACRO(6,pwmdac1) // PWMDAC 6A, 6B
|
|
|
-
|
|
|
- pwmdac1.MfuncC1 = DCbus_current;
|
|
|
- pwmdac1.MfuncC2 = mod1.Counter<<20;
|
|
|
- PWMDAC_MACRO(7,pwmdac1) // PWMDAC 7A, 7B
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Connect inputs of the DATALOG module
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-#endif // (BUILDLEVEL==LEVEL5)
|
|
|
-
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Call the DATALOG update function.
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-
|
|
|
-
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
-// Increase virtual timer and force 15 bit wrap around
|
|
|
-// ------------------------------------------------------------------------------
|
|
|
- VirtualTimer++;
|
|
|
- VirtualTimer &= 0x00007FFF;
|
|
|
+ IsrTicker = (IsrTicker>20000)? 0 : IsrTicker;
|
|
|
|
|
|
- IsrTime = MastIsrTimePercent();
|
|
|
+ //IsrTime = MastIsrTimePercent();
|
|
|
// Acknowledge interrupt to recieve more interrupts from PIE group 1
|
|
|
CpuTimer0.RegsAddr->TCR.bit.TIF=1;
|
|
|
PieCtrlRegs.PIEACK.bit.ACK1 = 1;
|
|
@@ -826,13 +375,14 @@ interrupt void xint2_isr(void)
|
|
|
{
|
|
|
static int32 last_XintTime = TIMER1_PER;
|
|
|
|
|
|
- if(CpuTimer1.RegsAddr->TCR.bit.TIF == 1){
|
|
|
+/* 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;
|
|
|
|
|
|
if(XintTime<30000){
|
|
|
//filter 防抖
|
|
@@ -855,7 +405,7 @@ interrupt void xint2_isr(void)
|
|
|
interrupt void xint1_isr(void)
|
|
|
{
|
|
|
|
|
|
- FaultFlag.bit.IgbtTempFaultFlag = 1;
|
|
|
+ FaultFlag.bit.Ipmfault = 1;
|
|
|
PieCtrlRegs.PIEACK.bit.ACK1 =1;
|
|
|
|
|
|
// PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
|
|
@@ -865,8 +415,11 @@ interrupt void EPWM1TZint_isr(void)
|
|
|
{
|
|
|
//過流
|
|
|
FaultFlag.bit.OverCurFlag = 1;
|
|
|
- EPwm1Regs.TZCLR.bit.OST = 1;
|
|
|
- EPwm1Regs.TZCLR.bit.INT = 1;
|
|
|
+ EALLOW;
|
|
|
+
|
|
|
+ // EPwm1Regs.TZCLR.bit.OST = 1;
|
|
|
+ // EPwm1Regs.TZCLR.bit.INT = 1;
|
|
|
+ EDIS;
|
|
|
PieCtrlRegs.PIEACK.bit.ACK2 = 1;//PIEACK_GROUP2;
|
|
|
}
|
|
|
void ServiceDog(void)
|
|
@@ -932,17 +485,11 @@ void speed_direction(Uint16 hall,Uint16 hall_last){
|
|
|
|
|
|
}
|
|
|
void speed_cal(void){
|
|
|
- /* int32 RotarfreOut = 0;
|
|
|
- int32 RotarfreqNew = 0;//电周期频率
|
|
|
|
|
|
- static int32 RotarfreSum = 0;
|
|
|
- static int32 RotarfreqArr[8] = {0,0,0,0,0,0,0,0};
|
|
|
- static int16 j = 0;*/
|
|
|
int32 RotarRPMNew = 0;
|
|
|
int32 RotarrpmOut = 0;
|
|
|
static int32 RotarrpmSum = 0;
|
|
|
- static int32 RotarrpmArr[16] = {0,0,0,0,0,0,0,0,0,0,
|
|
|
- 0,0,0,0,0,0};
|
|
|
+ static int32 RotarrpmArr[8] = {0,0,0,0,0,0,0,0};
|
|
|
|
|
|
static int16 k = 0;
|
|
|
if(CpuTimer1.RegsAddr->TCR.bit.TIF == 1){
|
|
@@ -950,35 +497,18 @@ void speed_cal(void){
|
|
|
XintTime = TIMER1_PER;
|
|
|
Xint2Cnt = 1;
|
|
|
CpuTimer1.RegsAddr->TCR.bit.TIF = 1;//写1清零
|
|
|
- // RotarfreqNew =0;
|
|
|
+
|
|
|
RotarRPMNew =0;
|
|
|
}else{
|
|
|
- //RotarfreqNew = (int32)(30000000)/XintTime;//60M clock /N
|
|
|
RotarRPMNew =(int32)360000000/XintTime;
|
|
|
}
|
|
|
if(Xint2Cnt == 1){
|
|
|
Xint2Cnt = 0;
|
|
|
- /* RotarfreqArr[j] = RotarfreqNew;
|
|
|
- RotarfreSum += RotarfreqArr[j];
|
|
|
- RotarfreOut = RotarfreSum >> 3;
|
|
|
- j++;
|
|
|
- if (j >= 8)
|
|
|
- {
|
|
|
- j = 0;
|
|
|
- }
|
|
|
- RotarfreSum -= RotarfreqArr[j];
|
|
|
-
|
|
|
-
|
|
|
- speed1.Speed = _IQdiv(RotarfreOut,BASE_FREQ);
|
|
|
- speed1.SpeedRpm = _IQmpy(speed1.BaseRpm,speed1.Speed);
|
|
|
- */
|
|
|
-
|
|
|
-
|
|
|
RotarrpmArr[k] = RotarRPMNew;
|
|
|
RotarrpmSum += RotarrpmArr[k];
|
|
|
- RotarrpmOut = RotarrpmSum>>4;
|
|
|
+ RotarrpmOut = RotarrpmSum>>3;
|
|
|
k++;
|
|
|
- if (k >= 16)
|
|
|
+ if (k >= 8)
|
|
|
{
|
|
|
k = 0;
|
|
|
}
|
|
@@ -1047,14 +577,472 @@ void taskfree(void){
|
|
|
}
|
|
|
int16 MastIsrTimePercent(void)
|
|
|
{
|
|
|
- int16 MasterIsrT = 0;
|
|
|
+ Uint32 T1 = 0;
|
|
|
|
|
|
+ int16 MasterIsrT =0;
|
|
|
+ static int32 T1_L = 60000000;
|
|
|
|
|
|
- MasterIsrT = 3000 - CpuTimer0.RegsAddr->TIM.all;
|
|
|
+ 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;
|
|
|
+ }
|
|
|
|
|
|
- MasterIsrT = (Uint32)MasterIsrT* 100 /3000;
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+
|
|
|
+ ServiceDog();
|
|
|
+ speed_cal();
|
|
|
+ pwmlimit_speed();
|
|
|
+ taskfree();
|
|
|
+ FaultTreat();
|
|
|
+
|
|
|
+ #if (BUILDLEVEL==LEVEL1)
|
|
|
+
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the RMP3 module and call the RAMP Control 3 macro.
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ rmp3.DesiredInput = CmtnPeriodTarget;
|
|
|
+ rmp3.Ramp3Delay = RampDelay;
|
|
|
+ RC3_MACRO(&rmp3);
|
|
|
+
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the IMPULSE module and call the Impulse macro.
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ impl1.Period = rmp3.Out;
|
|
|
+ IMPULSE_MACRO(&impl1);
|
|
|
+ HALL3_READ_MACRO(&hall1);
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the MOD6 module and call the Mod 6 counter macro.
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ mod1.TrigInput = impl1.Out;
|
|
|
+ MOD6CNT_MACRO(&mod1);
|
|
|
+
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the PWM_DRV module and call the PWM signal generation
|
|
|
+ // update macro.
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ pwm1.CmtnPointerIn = test2;//(int16)mod1.Counter;
|
|
|
+ if(EnableFlag == 0){
|
|
|
+ pwm1.DutyFuncIn = 0;
|
|
|
+ }else{
|
|
|
+ pwm1.DutyFuncIn = test;
|
|
|
+ }
|
|
|
+
|
|
|
+ BLDCPWM_MACRO(1,2,3,&pwm1);
|
|
|
+
|
|
|
+
|
|
|
+ #endif // (BUILDLEVEL==LEVEL1)
|
|
|
+ // =============================== LEVEL 3 ======================================
|
|
|
+ // Level 3 describes the closed-loop operation of sensored trapezoidal
|
|
|
+ // drive of BLDC motor using Hall sensor.
|
|
|
+ // ==============================================================================
|
|
|
+
|
|
|
+ #if (BUILDLEVEL==LEVEL3)
|
|
|
+
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // ADC conversion and offset adjustment (observing back-emfs is optinal for this prj.)
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ BemfA = _IQ12toIQ(AdcResult.ADCRESULT1);
|
|
|
+ BemfB = _IQ12toIQ(AdcResult.ADCRESULT2);
|
|
|
+ BemfC = _IQ12toIQ(AdcResult.ADCRESULT3);
|
|
|
+ DCbus_current = _IQ12toIQ(AdcResult.ADCRESULT4);//-_IQ(0.5); //1.65V offset added on HVDMC board.
|
|
|
+
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the RMP3 module and call the Ramp control 3 macro.
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+
|
|
|
+
|
|
|
+ HALL3_READ_MACRO(&hall1);
|
|
|
+
|
|
|
+ ClosedFlag=TRUE;
|
|
|
+
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the RMP2 module and call the Ramp control 2 macro.
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ rmp2.DesiredInput = (int32)DFuncDesired;
|
|
|
+ RC2_MACRO(&rmp2);
|
|
|
+
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the PWM_DRV module and call the PWM signal generation
|
|
|
+ // update macro.
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ if (ClosedFlag==TRUE) {
|
|
|
+ if (hall1.CmtnTrigHall==0x7FFF) {
|
|
|
+
|
|
|
+ PreviousState = pwm1.CmtnPointer;
|
|
|
+
|
|
|
+ // Comment the following if-else-if statements in case of
|
|
|
+ // inverted Hall logics for commutation states.
|
|
|
+ /* if (hall1.HallGpioAccepted==5)
|
|
|
+ { pwm1.CmtnPointer = 0;
|
|
|
+
|
|
|
+ }
|
|
|
+ else if (hall1.HallGpioAccepted==1)
|
|
|
+ { pwm1.CmtnPointer = 1;
|
|
|
+
|
|
|
+ }
|
|
|
+ else if (hall1.HallGpioAccepted==3)
|
|
|
+ { pwm1.CmtnPointer = 2;
|
|
|
+
|
|
|
+ }
|
|
|
+ else if (hall1.HallGpioAccepted==2)
|
|
|
+ { pwm1.CmtnPointer = 3;
|
|
|
+
|
|
|
+ }
|
|
|
+ else if (hall1.HallGpioAccepted==6)
|
|
|
+ { pwm1.CmtnPointer = 4;
|
|
|
+
|
|
|
+ }
|
|
|
+ else if (hall1.HallGpioAccepted==4)
|
|
|
+ {
|
|
|
+ pwm1.CmtnPointer = 5;
|
|
|
+ }*/
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // Comment the following if-else-if statements in case of
|
|
|
+ // non-inverted Hall logics for commutation states.
|
|
|
+ if (hall1.HallGpioAccepted==2)
|
|
|
+ pwm1.CmtnPointer = 0;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==6)
|
|
|
+ pwm1.CmtnPointer = 1;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==4)
|
|
|
+ pwm1.CmtnPointer = 2;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==5)
|
|
|
+ pwm1.CmtnPointer = 3;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==1)
|
|
|
+ pwm1.CmtnPointer = 4;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==3)
|
|
|
+ pwm1.CmtnPointer = 5;
|
|
|
+
|
|
|
+
|
|
|
+ } //hall1.CmtnTrigHall == 0x7FFF
|
|
|
+ } // ClosedFlag==TRUE
|
|
|
+ else{
|
|
|
+ pwm1.CmtnPointer = (int16)mod1.Counter;
|
|
|
+ }
|
|
|
+ pwm1.DutyFunc = DfuncTesting;
|
|
|
+ BLDCPWM_MACRO(1,2,3,&pwm1);
|
|
|
+
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the SPEED_PR module and call the speed calculation macro.
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ if ((pwm1.CmtnPointer==5)&&(PreviousState==4)&&(hall1.CmtnTrigHall==0x7FFF))
|
|
|
+ {
|
|
|
+ speed1.TimeStamp = VirtualTimer;
|
|
|
+ SPEED_PR_MACRO(&speed1);
|
|
|
+ test = (test ==0 )? 1 : 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the PWMDAC module
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ pwmdac1.MfuncC1 = (int32)hall1.HallGpioAccepted<<20;
|
|
|
+ pwmdac1.MfuncC2 = BemfA;
|
|
|
+ PWMDAC_MACRO(6,pwmdac1) // PWMDAC 6A, 6B
|
|
|
+
|
|
|
+ pwmdac1.MfuncC1 = DCbus_current;
|
|
|
+ pwmdac1.MfuncC2 = (int32)mod1.Counter<<20;
|
|
|
+ PWMDAC_MACRO(7,pwmdac1) // PWMDAC 7A, 7B
|
|
|
+
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the DATALOG module
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ DlogCh2 = (int16)mod1.Counter;
|
|
|
+ DlogCh1 = (int16)hall1.HallGpioAccepted;
|
|
|
+ DlogCh3 = (int16)_IQtoIQ15(BemfA);
|
|
|
+ DlogCh4 = (int16)_IQtoIQ15(BemfB);
|
|
|
+
|
|
|
+
|
|
|
+ #endif // (BUILDLEVEL==LEVEL3)
|
|
|
+
|
|
|
+ // =============================== 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;
|
|
|
+
|
|
|
+ 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,_IQ12(1),_IQ12(0.05));
|
|
|
+ last_DC_current = DC_current_filer;
|
|
|
+ carveData1();
|
|
|
+ CanCurve();
|
|
|
+
|
|
|
+ UserCANprocess();
|
|
|
+
|
|
|
+ DC_Current_sum_filter += DC_current_filer*pwm1.DutyFunc>>15;
|
|
|
+ // DC_Current_sum += DC_current_real*pwm1.DutyFunc>>15;
|
|
|
+ DC_Current_cnt++;//由于stall最多16384/20K秒 所以最多0.819715秒计算一次平均值
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ #if (BUILDLEVEL==LEVEL4)
|
|
|
+ BemfA = AdcResult.ADCRESULT1;
|
|
|
+ BemfB = AdcResult.ADCRESULT2;
|
|
|
+ BemfC = AdcResult.ADCRESULT3;
|
|
|
+
|
|
|
+ HALL3_READ_MACRO(&hall1);
|
|
|
+
|
|
|
+ if (hall1.Revolutions>=0)
|
|
|
+ ClosedFlag=TRUE;
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // 电流闭环
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ pid1_spd.Ref = _IQ(0.3);
|
|
|
+ pid1_spd.Fbk = _IQabs(speed1.Speed);
|
|
|
+ // tempIdc=pid1_idc.Fbk;
|
|
|
+ pid1_idc.Umax = pwmlimit<<9;
|
|
|
+ pid1_idc.Umin = (-pwmlimit)<<9;
|
|
|
+ pid1_idc.Ref = CurrentSet;//给定电流 反转给-
|
|
|
+ if(speed1.SpeedRpm<0){
|
|
|
+ pid1_idc.Fbk = -_IQ12toIQ(DC_current_filter_avr);// 电流改为Q15格式
|
|
|
+ }else{
|
|
|
+ pid1_idc.Fbk = _IQ12toIQ(DC_current_filter_avr);// 电流改为Q15格式
|
|
|
+ }
|
|
|
+ // if(pid1_idc.Fbk<0) pid1_idc.Fbk=tempIdc; // Eliminate negative values
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ step++;
|
|
|
+ if(step == 1){
|
|
|
+ PI_MACRO(&pid1_idc);
|
|
|
+ }else if(step == 2){
|
|
|
+ PI_MACRO(&pid1_spd);
|
|
|
+ step = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(EnableFlag == 0){
|
|
|
+ SetPI(&pid1_spd);
|
|
|
+ resetPI(&pid1_idc);
|
|
|
+ rmp2.DesiredInput = 0;
|
|
|
+ RC2_MACRO(&rmp2);
|
|
|
+ pwm1.DutyFuncIn = (int16)_IQtoIQ15(rmp2.Out); // controlled speed duty-cycle
|
|
|
+
|
|
|
+ }else{
|
|
|
+ pwm1.DutyFuncIn = (int16)_IQtoIQ15(_IQmpy(pid1_idc.Out,pid1_spd.Out)); // controlled speed duty-cycle
|
|
|
+ rmp2.Out = pwm1.DutyFuncIn;
|
|
|
+ // pwm1.DutyFuncIn = (int16)(_IQtoIQ15(pid1_idc.Out));
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (ClosedFlag==TRUE)
|
|
|
+ {
|
|
|
+ if (hall1.CmtnTrigHall==0x7FFF)
|
|
|
+ {
|
|
|
+ DC_current_avr = DC_Current_sum/DC_Current_cnt;
|
|
|
+ DC_current_filter_avr = DC_Current_sum_filter/DC_Current_cnt;
|
|
|
+ DC_Current_sum = 0;
|
|
|
+ DC_Current_sum_filter =0;
|
|
|
+ DC_Current_cnt = 0;
|
|
|
+ //计算速度
|
|
|
+ speed1.TimeStamp = VirtualTimer;
|
|
|
+ SPEED_PR_MACRO(&speed1,hall1.HallGpioAccepted,PreviousState,hall1.Stall_status);
|
|
|
+ PreviousState = hall1.HallGpioAccepted;
|
|
|
+ if (hall1.HallGpioAccepted==2)
|
|
|
+ pwm1.CmtnPointerIn = 0;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==6)
|
|
|
+ pwm1.CmtnPointerIn = 1;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==4)
|
|
|
+ pwm1.CmtnPointerIn = 2;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==5)
|
|
|
+ pwm1.CmtnPointerIn = 3;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==1)
|
|
|
+ pwm1.CmtnPointerIn = 4;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==3)
|
|
|
+ { pwm1.CmtnPointerIn = 5;
|
|
|
+
|
|
|
+ }//hall1.HallGpioAccepted==3
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ BLDCPWM_MACRO(1,2,3,&pwm1);
|
|
|
+
|
|
|
+ #endif // (BUILDLEVEL==LEVEL4)
|
|
|
+
|
|
|
+
|
|
|
+ // =============================== LEVEL 5 ======================================
|
|
|
+ // Level 5 verifies the closed speed loop and speed PI controller.
|
|
|
+ // ==============================================================================
|
|
|
+
|
|
|
+ #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;
|
|
|
+ //DCbus_current = _IQ12toIQ(AdcResult.ADCRESULT5); //采样值<<12
|
|
|
+
|
|
|
+ rc1.TargetValue = SpeedRef;
|
|
|
+ RC_MACRO(&rc1);
|
|
|
+
|
|
|
+ HALL3_READ_MACRO(&hall1);
|
|
|
+ pid1_spd.Ref = rc1.SetpointValue;
|
|
|
+ pid1_spd.Fbk = speed1.Speed;
|
|
|
+ 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{
|
|
|
+ pid1_spd.Umax = _IQ((float)pwmlimit/32767);
|
|
|
+ pid1_spd.Umin = _IQ((float)(-pwmlimit)/32767);
|
|
|
+
|
|
|
+ PI_MACRO(&pid1_spd);
|
|
|
+ int16 pwmtmp =(int16)_IQtoIQ15(pid1_spd.Out);
|
|
|
+ if((int16)_IQtoIQ15(pid1_spd.Out)>pwmlimit){
|
|
|
+ pwmtmp = pwmlimit; // controlled speed duty-cycle
|
|
|
+ }else if((int16)_IQtoIQ15(pid1_spd.Out)<-pwmlimit){
|
|
|
+ pwmtmp = -pwmlimit; // controlled speed duty-cycle
|
|
|
+ }else{
|
|
|
+ pwmtmp = (int16)_IQtoIQ15(pid1_spd.Out); // controlled speed duty-cycle
|
|
|
+ }
|
|
|
+
|
|
|
+ pwm1.DutyFuncIn = ((int32)pwmtmp*LimitMotCtrTemp(600000))>>15;//1分钟降一次
|
|
|
+ //pwm1.DutyFuncIn = test;
|
|
|
+ rmp2.Out = pwm1.DutyFuncIn;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ ClosedFlag = TRUE;
|
|
|
+ if (ClosedFlag==TRUE)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (hall1.CmtnTrigHall==0x7FFF)
|
|
|
+ {
|
|
|
+
|
|
|
+ // DC_current_avr = DC_Current_sum/DC_Current_cnt;
|
|
|
+ DC_current_filter_avr = DC_Current_sum_filter/DC_Current_cnt;
|
|
|
+ // DC_Current_sum = 0;
|
|
|
+ DC_Current_sum_filter =0;
|
|
|
+ DC_Current_cnt = 0;
|
|
|
+ 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;
|
|
|
+ /*
|
|
|
+ if (hall1.HallGpioAccepted==2)
|
|
|
+ pwm1.CmtnPointerIn = 0;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==6)
|
|
|
+ pwm1.CmtnPointerIn = 1;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==4)
|
|
|
+ pwm1.CmtnPointerIn = 2;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==5)
|
|
|
+ pwm1.CmtnPointerIn = 3;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==1)
|
|
|
+ pwm1.CmtnPointerIn = 4;
|
|
|
+
|
|
|
+ else if (hall1.HallGpioAccepted==3)
|
|
|
+ { pwm1.CmtnPointerIn = 5;
|
|
|
+
|
|
|
+ }//hall1.HallGpioAccepted==3
|
|
|
+ */
|
|
|
+ //}//direction == Normal
|
|
|
+
|
|
|
+ } // delay
|
|
|
+ } // ClosedFlag==TRUE
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(Enable_ALLOW == 0 && pwm1.DutyFunc==0){
|
|
|
+ bldcpwm_close();
|
|
|
+
|
|
|
+ }else{
|
|
|
+ BLDCPWM_MACRO(1,2,3,&pwm1);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ // BLDCPWM_MACRO(1,2,3,&pwm1);
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+ // Connect inputs of the DATALOG module
|
|
|
+ // ------------------------------------------------------------------------------
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ #endif // (BUILDLEVEL==LEVEL5)
|
|
|
+ //程序占用率
|
|
|
+ //IsrTime = MastIsrTimePercent();
|
|
|
+ T2 = CpuTimer2.RegsAddr->TIM.all;
|
|
|
+ if(T1 < T2){
|
|
|
+ IsrTime = (Uint32)(T1 +CpuTimer2.RegsAddr->PRD.all -T2)* 100 /6000;
|
|
|
+ }else{
|
|
|
+ IsrTime = (Uint32)(T1-T2)* 100 /6000;
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+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 */
|
|
|
+
|
|
|
+}
|