馬達控制馬達控制 (QEI)(QEI)

上次的圖上次的圖 (( 使用使用 A/D)A/D)

這次的圖這次的圖 (( 用用 QEIQEI 大約是這樣大約是這樣 ))

////馬達正反轉馬達正反轉 11圈圈

//#include//#include <stdio.h><stdio.h> ////呼叫呼叫 stdio(stdio(標準輸入輸出標準輸入輸出 ))指令巨集指令巨集 //#define//#define FCYFCY 20000002000000 #include #include <p30F4011.h><p30F4011.h> ////呼叫呼叫 p30F3011(IC)p30F3011(IC)指令巨集指令巨集 #include #include <qei.h><qei.h>

long icount=0,v2_new=0,v2_old=0,pi=0,j=0;long icount=0,v2_new=0,v2_old=0,pi=0,j=0; double v1=1600.0,v3=0.0,e_new=0.0,e_old=0.0,d_new=0.0,d_old=0.0,kp=20.0,ki=0.0;double v1=1600.0,v3=0.0,e_new=0.0,e_old=0.0,d_new=0.0,d_old=0.0,kp=20.0,ki=0.0; int pot[4]={0x0100,0x0200,1,-1};int pot[4]={0x0100,0x0200,1,-1}; int dd;int dd;

unsigned int s=0; unsigned int s=0; _FWDT(WDT_OFF);_FWDT(WDT_OFF);

void PWM_Initial(void)void PWM_Initial(void) {{

PTPER = 125;PTPER = 125; //PWM = (20MHz / 16/ 125) = 10 K//PWM = (20MHz / 16/ 125) = 10 KHzHz

IEC2bits.PWMIE = 0;IEC2bits.PWMIE = 0; //Disable PWM Interrupt//Disable PWM Interrupt PTCON = 0xA008;PTCON = 0xA008; // 0b 1010 0000 0000 0000// 0b 1010 0000 0000 0000 PWMCON1 = 0x0777;PWMCON1 = 0x0777; // 0b 0000 0111 0111 0111// 0b 0000 0111 0111 0111 PDC1=0;PDC1=0; OVDCON= pot[pi];OVDCON= pot[pi]; ////設定輸出腳位設定輸出腳位 IPC9bits.PWMIP = 6;IPC9bits.PWMIP = 6; }}

void Qei_Initial(void)void Qei_Initial(void) {{ QEICON=0x0700;QEICON=0x0700; //0000 0111 0000 0000 x4//0000 0111 0000 0000 x4 與與 MAXMAX比較比較 DFLTCON=0x0;DFLTCON=0x0; //0000 0000 0000 0000 //0000 0000 0000 0000 POSCNT=0;POSCNT=0; MAXCNT=0xffff;MAXCNT=0xffff; QEICONbits.CNTERR=1;QEICONbits.CNTERR=1; }}

void Init_Timer1( void )void Init_Timer1( void ) {{ T1CON = 0;T1CON = 0;

IFS0bits.T1IF = 0;IFS0bits.T1IF = 0; IPC0bits.T1IP = 7;IPC0bits.T1IP = 7; IEC0bits.T1IE = 1;IEC0bits.T1IE = 1; PR1 = 5000; // 20MHz/8/5000=500 Hz PR1 = 5000; // 20MHz/8/5000=500 Hz 中斷為中斷為 2ms2ms T1CONbits.TCS = 0;T1CONbits.TCS = 0; T1CONbits.TCKPS=1; T1CONbits.TCKPS=1; //TCKPS<1:0>: Timer Input Clock Prescale Select bits//TCKPS<1:0>: Timer Input Clock Prescale Select bits //11 = 1:256 prescale value//11 = 1:256 prescale value //10 = 1:64 prescale value//10 = 1:64 prescale value //01 = 1:8 prescale value//01 = 1:8 prescale value //00 = 1:1 prescale value //00 = 1:1 prescale value T1CONbits.TON=1;T1CONbits.TON=1; }}

void _ISR _T1Interrupt(void)void _ISR _T1Interrupt(void) {{ v2_old=v2_new;v2_old=v2_new; v2_new=POSCNT;v2_new=POSCNT; if(((v2_old-v2_new)<30000)&&((v2_new-v2_old)<30000))if(((v2_old-v2_new)<30000)&&((v2_new-v2_old)<30000)) v2_new=v2_new;v2_new=v2_new; else if((v2_old-v2_new)>30000)else if((v2_old-v2_new)>30000) v2_new=65536+v2_new;v2_new=65536+v2_new; else if(((v2_new-v2_old)>30000)&&(v2_old>=65536))else if(((v2_new-v2_old)>30000)&&(v2_old>=65536)) v2_new=v2_new;v2_new=v2_new; elseelse v2_new=v2_new-65536;v2_new=v2_new-65536;

e_new=v1-v2_new*1.0;e_new=v1-v2_new*1.0; d_new=kp*e_new;d_new=kp*e_new; if(d_new>0.0)if(d_new>0.0) {{ pi=0;pi=0; if(d_new>220.0)if(d_new>220.0) d_new=220.0;d_new=220.0; }} else if(d_new<0.0)else if(d_new<0.0) {{ if(d_new<-220.0)if(d_new<-220.0) d_new=-220.0;d_new=-220.0; pi=1;pi=1; }} dd=d_new*pot[pi+2];dd=d_new*pot[pi+2]; PDC1=dd; OVDCON= pot[pi];PDC1=dd; OVDCON= pot[pi]; s++;s++; IFS0bits.T1IF = 0;IFS0bits.T1IF = 0; }}

int main(void)int main(void) {{ ADPCFG=0xffff;ADPCFG=0xffff; TRISB = 0x01f2;TRISB = 0x01f2; TRISF = 0x0000;TRISF = 0x0000;

PORTF=0x0002;PORTF=0x0002;

PWM_Initial();PWM_Initial(); Qei_Initial();Qei_Initial(); Init_Timer1();Init_Timer1();

while(1)while(1) {{ if (s>4000)if (s>4000) {{ v1=1600.0;v1=1600.0; s=0;s=0; PORTF=0x2;PORTF=0x2; }} else if (s>2000)else if (s>2000) {{ v1=0.0;v1=0.0; PORTF=0x1;PORTF=0x1; }} }} }}

練習練習1.1.自行定義馬達圈數自行定義馬達圈數 ,,並且要正反轉並且要正反轉 !!