無(wú)位置傳感器的無(wú)刷直流電動(dòng)機(jī)調(diào)速控制程序

-.z?無(wú)位置傳感器的無(wú)刷直流電動(dòng)機(jī)調(diào)速控制程序?.include"240*.h".global_c_int0;--------------------------------------以下定義常數(shù)和變量--------------------------------------------------KP.set245;電流調(diào)節(jié)比例系數(shù),0.12的Q11格式KPS.set100;速度調(diào)節(jié)比例系數(shù).bssCAPT,1;換相控制字.bssP,1;更新占空比的比擬值,由電流調(diào)節(jié)輸出.bssIDC_REF,1;電流參考值,由速度調(diào)節(jié)輸出.bssIDC_ERRORK,1;電流偏差.bssFLAGCUR,1;更新比擬值和換相標(biāo)志,1-允許更新,0-不允許.bssSPEED_REF,1;給定速度參考.bssV_ERRORK,1;速度偏差.bssSPEED_COUNT,1;速度調(diào)節(jié)環(huán)計(jì)數(shù)器.bssV1,1;相電壓1.bssV2,1;相電壓2.bssV3,1;相電壓3.bssNEUTRAL,1;中性點(diǎn)電壓.bssFLAG,1;感應(yīng)電動(dòng)勢(shì)變符號(hào)標(biāo)志,1-變了;0-沒(méi)變.bssFLAGUP,1;轉(zhuǎn)過(guò)一機(jī)械轉(zhuǎn)標(biāo)志,1-沒(méi)轉(zhuǎn)過(guò),0-轉(zhuǎn)過(guò).bssBCOUNT,1;延遲時(shí)間更新值,磁定位時(shí)臨時(shí)變量.bssB2COUNT,1;延遲時(shí)間.bssSTALL,1;磁定位完畢標(biāo)志,1-完畢,0-沒(méi)完畢.bssASYM,1;延時(shí)計(jì)算感應(yīng)電動(dòng)勢(shì)計(jì)數(shù)器.bssSPEEDFLAG,1;第一轉(zhuǎn)時(shí)制止速度調(diào)節(jié)標(biāo)志,1-制止,0-允許.bssTIME,1;每轉(zhuǎn)時(shí)間計(jì)數(shù)器.bssSTACK,6;軟堆棧區(qū);--------------------------------------定義主向量-------------------------------------------------------------.sect"vectors"RSVECTB_c_int0;復(fù)位INT1BPHANTOM;INT1INT2BPHANTOM;INT2INT3BPHANTOM;INT3INT4BPHANTOM;INT4INT5BPHANTOM;INT5INT6BADCINT;ADC中斷…;略;--------------------------------------以下是程序-------------------------------------------------------------.te*t;--------------------------------------初始化程序--------------------------------------------------------------_c_int0SETCFCLRCOVMSETCS*M;符號(hào)擴(kuò)展SETCINTM;關(guān)中斷LARAR1,*STACKLDP*0E0HSPLK*68H,WDCR;不用看門(mén)狗SPLK*0284H,SCSR1;10MCLKIN,20MCLKOUTLDP*0E8HSPLK*500,T1PR;PWM設(shè)置,周期50微秒SPLK*0000H,T1TSPLK*0FFFH,ACTRA;PWM1～6全部高電平輸出SPLK*01F4H,DBTCONA;死區(qū)1.6微秒SPLK*500,CMPR1;占空比為0SPLK*500,CMPR2SPLK*500,CMPR3SPLK*8200H,CONA;允許比擬,T1下溢重載SPLK*0840H,T1CON;連續(xù)增減計(jì)數(shù),內(nèi)部時(shí)鐘,不分頻SPLK*0100H,GPTCONA;T1周期中斷標(biāo)志啟動(dòng)AD轉(zhuǎn)換LDP*0E1HSPLK*0FC0H,MCRA;引腳設(shè)置為PWMLDP*0LACC*020HSACLIMR;允許INT6中斷LACCIFR;清標(biāo)志SACLIFRLDP*0E8HLACCEVAIFRA;清標(biāo)志SACLEVAIFRALACCEVAIFRBSACLEVAIFRBLACCEVAIFRCSACLEVAIFRCSPLK*0,EVAIMRA;屏蔽中斷SPLK*0,EVAIMRBSPLK*0,EVAIMRCLDP*0E1H;AD設(shè)置SPLK*0500H,ADCTRL2;允許EVA啟動(dòng)AD轉(zhuǎn)換SPLK*0900H,ADCTRL1;10分頻SPLK*0003H,MA*CONV;4個(gè)轉(zhuǎn)換通道SPLK*3210H,CHSELSEQ1;ADCIN00-03通道CLRCINTM;開(kāi)總中斷;--------------------------------------變量初始化---------------------------------------------------------LDP*0;變量初始化SPLK*020H,IDC_REF;磁定位電流SPLK*0,IDC_ERRORKSPLK*0300H,SPEED_REF;轉(zhuǎn)速初值SPLK*00112,P;最小占空比SPLK*0000H,FLAGCURSPLK*0000H,SPEED_COUNTSPLK*0000H,CAPTSPLK*0000H,V1SPLK*0000H,V2SPLK*0000H,V3SPLK*0000H,NEUTRALSPLK*0000H,FLAGSPLK*0001H,FLAGUP;沒(méi)轉(zhuǎn)過(guò)一轉(zhuǎn)SPLK*0001H,SPEEDFLAG;制止速度調(diào)節(jié)SPLK*0000H,BCOUNTSPLK*0000H,B2COUNTSPLK*0000H,STALLSPLK*0000H,ASYMSPLK*0000H,V_ERRORKSPLK*0000H,TIME;--------------------------------------準(zhǔn)備磁定位-------------------------------------------------------------LDP*0LACCP;裝載比擬初值LDP*0E8H;反相驅(qū)動(dòng)SPLK*03FDH,ACTRA;PWM1低有效,PWM6低電平,其它高電平SACLCMPR1;A相入,C相出,B相不通電SPLK*0000H,CMPR2SPLK*0000H,CMPR3;--------------------------------------等待磁定位完畢------------------------------------------------------MAGSTALLLDP*0LACCSTALL;檢測(cè)磁定位是否完畢BDMAGSTALL,EQ;沒(méi)完畢繼續(xù)等待;--------------------------------------磁定位完畢換相----------------------------------------------------LACCPLDP*0E8HSPLK*03DFH,ACTRA;PWM3低有效,PWM6低電平,其它高電平SACLCMPR2;B相入,C相出,A不通電SPLK*0000H,CMPR3SPLK*0000H,CMPR1LDP*0SPLK*4,CAPT;CAPT初值;--------------------------------------主循環(huán)-----------------------------------------------------------------LOOPLDP*0LACCFLAGCUR;檢測(cè)是否更新比擬值BDLOOP,EQ;不更新跳轉(zhuǎn)SPLK*0,FLAGCUR;更新,清標(biāo)志CALLSEQUENCE;調(diào)用更新子程序BLOOP;等待下次更新;--------------------------------------假中斷處理-------------------------------------------------------------PHANTOMCLRCINTMRET;--------------------------------------更新比擬值或換相子程序-----------------------------------------SEQUENCELDP*0LACCTIME;每轉(zhuǎn)時(shí)間計(jì)數(shù)器ADD*1SACLTIMELACCCAPT;換相控制字ADD*CAPT_DETER;加起始地址BACCCAPT_DETERBRISING1;相當(dāng)于H1上升沿BFALLING3;相當(dāng)于H3下降沿BRISING2;相當(dāng)于H2上升沿BFALLING1;相當(dāng)于H1下降沿BRISING3;相當(dāng)于H3上升沿FALLING2;相當(dāng)于H2下降沿LACCPLDP*0E8HSPLK*0D3FH,ACTRA;PWM5低有效,PWM4低電平,其它高電平SACLCMPR3;C相入,B相出,A相不通電SPLK*0000H,CMPR2SPLK*0000H,CMPR1LDP*0;延時(shí)過(guò)濾干擾LACCASYM;延時(shí)計(jì)數(shù)器ADD*1;加1SACLASYMSUB*10;檢測(cè)是否到計(jì)數(shù)值BDEND,LEQ;沒(méi)到,退出SPLK*10,ASYM;計(jì)數(shù)值已到,測(cè)量感應(yīng)電動(dòng)勢(shì)過(guò)零點(diǎn)LACCFLAG;感應(yīng)電動(dòng)勢(shì)符號(hào)變化?BDEND,NEQ;變了,退出LACCV1,1;沒(méi)變,檢測(cè).V1乘2ADDV1;ACC=3*(BEMFA+NEUTRAL)SUBNEUTRAL;ACC=3*BEMFABDEND,LT;<0符號(hào)沒(méi)變,退出SPLK*1,FLAG;否則符號(hào)改變,置過(guò)零標(biāo)志LACCBCOUNTSACLB2COUNT;更新延遲時(shí)間BEND;退出RISING3LACCPLDP*0E8HSPLK*0DF3H,ACTRA;PWM5低有效,PWM2低電平,其它高電平SACLCMPR3;C相入,A相出,B相不通電SPLK*0000H,CMPR2SPLK*0000H,CMPR1LDP*0;延時(shí)過(guò)濾干擾LACCASYM;延時(shí)計(jì)數(shù)器ADD*1;加1SACLASYMSUB*10;檢測(cè)是否到計(jì)數(shù)值BDEND,LEQ;沒(méi)到,退出SPLK*10,ASYM;計(jì)數(shù)值已到,測(cè)量感應(yīng)電動(dòng)勢(shì)過(guò)零點(diǎn)LDP*0LACCFLAG;感應(yīng)電動(dòng)勢(shì)符號(hào)變化?BDEND,NEQ;變了,退出LACCV2,1;沒(méi)變,檢測(cè).V2乘2ADDV2;ACC=3*(BEMFB+NEUTRAL)SUBNEUTRAL;ACC=3*BEMFBBDEND,GEQ;≥0符號(hào)沒(méi)變,退出SPLK*1,FLAG;否則符號(hào)改變,置過(guò)零標(biāo)志LACCBCOUNTSACLB2COUNT;更新延遲時(shí)間BEND;退出FALLING3LACCPLDP*0E8HSPLK*03FDH,ACTRA;PWM1低有效,PWM6低電平,其它高電平SACLCMPR1;A相入,C相出,B相不通電SPLK*0000H,CMPR2SPLK*0000H,CMPR3LDP*0;延時(shí)過(guò)濾干擾LACCASYM;延時(shí)計(jì)數(shù)器ADD*1;加1SACLASYMSUB*10;檢測(cè)是否到計(jì)數(shù)值BDEND,LEQ;沒(méi)到,退出SPLK*10,ASYM;計(jì)數(shù)值已到,測(cè)量感應(yīng)電動(dòng)勢(shì)過(guò)零點(diǎn)LDP*0LACCFLAG;感應(yīng)電動(dòng)勢(shì)符號(hào)變化?BDEND,NEQ;變了,退出LACCV2,1;沒(méi)變,檢測(cè).V2乘2ADDV2;ACC=3*(BEMFB+NEUTRAL)SUBNEUTRAL;ACC=3*BEMFBBDEND,LT;<0符號(hào)沒(méi)變,退出SPLK*1,FLAG;否則符號(hào)改變,置過(guò)零標(biāo)志LACCBCOUNTSACLB2COUNT;更新延遲時(shí)間BEND;退出RISING2LACCPLDP*0E8HSPLK*03DFH,ACTRA;PWM3低有效,PWM6低電平,其它高電平SACLCMPR2;B相入,C相出,A相不通電SPLK*0000H,CMPR3SPLK*0000H,CMPR1LDP*0;延時(shí)過(guò)濾干擾LACCASYM;延時(shí)計(jì)數(shù)器ADD*1;加1SACLASYMSUB*10;檢測(cè)是否到計(jì)數(shù)值BDEND,LEQ;沒(méi)到,退出SPLK*10,ASYM;計(jì)數(shù)值已到,測(cè)量感應(yīng)電動(dòng)勢(shì)過(guò)零點(diǎn)LDP*0LACCFLAG;感應(yīng)電動(dòng)勢(shì)符號(hào)變化?BDEND,NEQ;變了,退出LACCV1,1;沒(méi)變,檢測(cè).V1乘2ADDV1;ACC=3*(BEMFA+NEUTRAL)SUBNEUTRAL;ACC=3*BEMFABDEND,GEQ;≥0符號(hào)沒(méi)變,退出SPLK*1,FLAG;否則符號(hào)改變,置過(guò)零標(biāo)志LACCBCOUNTSACLB2COUNT;更新延遲時(shí)間BEND;退出RISING1LACCPLDP*0E8HSPLK*0F3DH,ACTRA;PWM1低有效,PWM4低電平,其它高電平SACLCMPR1;A相入,B相出,C相不通電SPLK*0000H,CMPR2SPLK*0000H,CMPR3LDP*0;延時(shí)過(guò)濾干擾LACCASYM;延時(shí)計(jì)數(shù)器ADD*1;加1SACLASYMSUB*10;檢測(cè)是否到計(jì)數(shù)值BDEND,LEQ;沒(méi)到,退出SPLK*10,ASYM;計(jì)數(shù)值已到,測(cè)量感應(yīng)電動(dòng)勢(shì)過(guò)零點(diǎn)LDP*0LACCFLAG;感應(yīng)電動(dòng)勢(shì)符號(hào)變化?BDEND,NEQ;變了,退出LACCV3,1;沒(méi)變,檢測(cè).V3乘2ADDV3;ACC=3*(BEMFC+NEUTRAL)SUBNEUTRAL;ACC=3*BEMFCBDEND,GEQ;≥0符號(hào)沒(méi)變,退出SPLK*1,FLAG;否則符號(hào)改變,置過(guò)零標(biāo)志LACCBCOUNTSACLB2COUNT;更新延遲時(shí)間BEND;退出FALLING1LACCPLDP*0E8HSPLK*0FD3H,ACTRA;PWM3低有效,PWM2低電平,其它高電平SACLCMPR2;B相入,A相出,C相不通電SPLK*0000H,CMPR3SPLK*0000H,CMPR1LDP*0;延時(shí)過(guò)濾干擾LACCASYM;延時(shí)計(jì)數(shù)器ADD*1;加1SACLASYMSUB*10;檢測(cè)是否到計(jì)數(shù)值BDEND,LEQ;沒(méi)到,退出SPLK*10,ASYM;計(jì)數(shù)值已到,測(cè)量感應(yīng)電動(dòng)勢(shì)過(guò)零點(diǎn)LDP*0SPLK*0,FLAGUP;轉(zhuǎn)過(guò)一機(jī)械轉(zhuǎn),置標(biāo)志LACCFLAG;感應(yīng)電動(dòng)勢(shì)符號(hào)變化?BDEND,NEQ;變了,退出LACCV3,1;沒(méi)變,檢測(cè).V3乘2ADDV3;ACC=3*(BEMFC+NEUTRAL)SUBNEUTRAL;ACC=3*BEMFCBDEND,LT;<0符號(hào)沒(méi)變,退出SPLK*1,FLAG;否則符號(hào)改變,置過(guò)零標(biāo)志LACCBCOUNTSACLB2COUNT;更新延遲時(shí)間ENDRET;--------------------------------------速度計(jì)算和調(diào)節(jié)子程序------------------------------------------------SPEED_REGLDP*0SPLK*32,SPEED_COUNT;25CLRCS*MLACC*0OR*0FFFFH;ACC=FFFFH無(wú)符號(hào)數(shù)RPT*15SUBCBCOUNT;除1/12機(jī)械轉(zhuǎn)時(shí)間AND*0FFFFH;屏蔽高位SETCS*MSUBSPEED_REFNEG;速度偏差=SPEED_REF-SPEEDBDPOS,GEQ;檢測(cè)偏差是否超限.≥0跳轉(zhuǎn)ABS;<0則取絕對(duì)值SPLK*-32,SPEED_COUNT;-25POSSACLV_ERRORK;保存速度偏差SUB*03FFH;檢測(cè)上限BDOKPOS,LEQ;不超上限則跳轉(zhuǎn)SPLK*03FFH,V_ERRORK;否則=3FFHOKPOSLTV_ERRORK;-1024<速度偏差<1024MPYSPEED_COUNT;乘25PACSACLV_ERRORK;速度偏差<<5LTV_ERRORK;速度比例調(diào)節(jié)MPY*KPS;乘比例系數(shù)PACADDIDC_REF,16SACHIDC_REF;IDC_REF(K)=IDC_REF(K-1)+KPS*V_ERRORKLACCIDC_REF;檢測(cè)IDC_REF是否超限BDRES,GEQ;≥0跳轉(zhuǎn)SPLK*0,IDC_REF;超下限則=0RESSPLK*0,SPEED_COUNT;速度調(diào)節(jié)環(huán)計(jì)數(shù)器清0RET;--------------------------------------ADC中斷子程序---------------------------------------------------ADCINT;每50微秒轉(zhuǎn)換一次MAR*,AR1;保存現(xiàn)場(chǎng)MAR*+SST*1,*+;保存ST1SST*0,*+;保存ST0SACH*+;保存ACC高16位SACL*;保存ACC低16位;--------------------------------------速度調(diào)節(jié)與否---------------------------------------------------------------LDP*0LACCSTALL;檢測(cè)磁定位是否完成?BDVDC_IDC,EQ;STALL=0(磁定位沒(méi)完成)制止速度調(diào)節(jié)LACCSPEEDFLAG;制止速度調(diào)節(jié)?BDVDC_IDC,NEQ;SPEEDFLAG=1,制止速度調(diào)節(jié)LACCSPEED_COUNT;檢測(cè)是否該速度調(diào)節(jié)?SUB*2000;每100ms進(jìn)展一次速度調(diào)節(jié)BDNO_SPEED_REG,NEQ;時(shí)間沒(méi)到,跳轉(zhuǎn)CALLSPEED_REG;時(shí)間到,調(diào)速度調(diào)節(jié)子程序NO_SPEED_REGLACCSPEED_COUNTADD*1SACLSPEED_COUNT;SPEED_COUNT+1;--------------------------------------讀ADC轉(zhuǎn)換結(jié)果-------------------------------------------------------VDC_IDCLDP*0E0HLACCPIVR;清ADC中斷標(biāo)志LDP*0E8HLACCEVAIFRA;清T1周期中斷標(biāo)志SACLEVAIFRALDP*0E1HLACCADCTRL2OR*0202HSACLADCTRL2LACCRESULT0,10;讀電流值LDP*0SACHIDC_ERRORKLDP*0E1HLACCRESULT1,10;讀A相電壓LDP*0SACHV1LDP*0E1HLACCRESULT2,10;讀B相電壓LDP*0SACHV2LDP*0E1HLACCRESULT3,10;讀C相電壓LDP*0SACHV3;--------------------------------------電流比例調(diào)節(jié)-----------------------------------------------------------SETCS*MLACCIDC_ERRORK,5;Q5格式SUBIDC_REF,5SACLIDC_ERRORK;保存電流偏差,Q5格式LTIDC_ERRORK;電流比例調(diào)節(jié)MPY*KP;Q11格式PAC;ACC=KP*IDC_ERRORKADDP,16SACHP;KP*IDC_ERRORK+P(K-1)LACCP;檢測(cè)是否超限BDSUP_LIM,GTSPLK*0,P;超過(guò)下限,P=0BP_OKSUP_LIMSUB*0500BDP_OK,LTSPLK*0500,P;超過(guò)上限,P=500P_OK;在上下限內(nèi);--------------------------------------換相準(zhǔn)備-----------------------------------------------------------------LDP*0LACCFLAG;過(guò)零?BDNEU,EQ;沒(méi)過(guò)零,跳轉(zhuǎn)LACCB2COUNT;過(guò)零,執(zhí)行延遲時(shí)間SUB*1;延遲時(shí)間減1SACLB2COUNTSETCS*MBDNEU,NEQ;延遲時(shí)間沒(méi)到,跳轉(zhuǎn)LACCCAPT;否則更新?lián)Q相控制字ADD*2SACLCAPTSUB*0CHBDOKCAPT,NEQ;沒(méi)超過(guò)12,跳轉(zhuǎn)SPLK*0,C