Tải bản đầy đủ - 0 (trang)
Phục lục 1: Chương trình vi điều khiển PIC

Phục lục 1: Chương trình vi điều khiển PIC

Tải bản đầy đủ - 0trang

if(mode==0){

if(k==10){

k =0;

TMR_temp = TMR1;

if(chieu==0)

pos = (TMR_temp+old_xung);

else pos = (old_xung-TMR_temp);

pos_old = pos;

if((count_send++)==5){

count_send = 0;

IN= 0;

if(tx==1)printf("\f%d\n",pos);

TMR_temp = TMR1; // cap nhat lai position

if(chieu==0)

pos_old = (TMR_temp+old_xung);

else pos_old = (old_xung-TMR_temp);

}

pid();

}

}

else {//mode==1:velocity

if(k==10){

k =0;

TMR_temp = TMR1;

TMR1=0;

vel=TMR_temp;

vel_old = vel;

pid_vel();

if((count_send++)==5){

count_send = 0;

IN= 0;

if(tx==1)printf("\f%d\n",vel);

TMR_temp = TMR1; // cap nhat lai velocity

}

}

}

}

if(RCIF&&RCIE){

get_str();

}

//ngat onchange

if (INTF&&INTE){

INTF = 0;

TMR_temp = TMR1;

if(chieu==0){old_xung +=TMR_temp;RA3=1;}

else {old_xung-=TMR_temp; RA3=0;}



INTEDG =~INTEDG;

chieu =~chieu;

reset_counter();

}

}

void pid(){

err = sp - pos;

sum_err +=err;

if(sum_err>32000) sum_err=32000;

if(sum_err<-32000) sum_err=-32000;

d_err = err - old_err;

old_err = err;

pid_out = kp*(float)err+ki*(float)sum_err +kd*(float)d_err;

if(pid_out>0){RA1 =0;RA2 = 1;} //QUAY THUAN

if(pid_out<0){RA1 = 1;RA2 = 0;}

if(pos==sp){

sum_err=0;

RA1 = 1;RA2 = 1;}

if(pos>(sp*0.9)&&pos
kt_t=0;

RA1 = 1;RA2 = 1;

}

if(pid_out<0)pid_int = -((int)pid_out);

else pid_int = (int)pid_out;

if(pid_int>6000)pid_int = 6000;//6165

//

if(pid_int <4000) pid_int = 4000;

TMR2ON = 0;

CCPR1L = pid_int/80;

TMR2ON = 1;

}

void pid_vel(){

err_vel = s_vel - vel;

sum_err +=err_vel;

if(sum_err_vel>32000) sum_err_vel=32000;

if(sum_err_vel<-32000) sum_err_vel=-32000;

d_err_vel = err_vel - old_err_vel;

old_err_vel = err_vel;

pid_out_vel += kp*(float)err_vel+ki*(float)sum_err_vel +kd*(float)d_err_vel;

//if(pid_out_vel>6000)pid_out_vel= 6000;//6165

TMR2ON = 0;

CCPR1L = pid_out_vel/45;

TMR2ON = 1;

}

void init(){



ANSEL = ANSELH = 0;

//khai bao Counter1

TMR1CS=1;//clock ngoai

T1OSCEN = 0;

T1CKPS1=0;//scale la 1

T1CKPS0=0;

T1SYNC=0; //k dong bo

TMR1ON=0;

TMR1IE=1;

TMR1IF=0;

TMR1=0;

PEIE=1;

// khai bao RS232

TXSTA = 0x24;

RCSTA = 0x90;

BRG16 = 1; //chon baund rate 20MhZ/(20*(520+1))~9600

SPBRGH = 2;

SPBRG = 8;

RCIF=0;

RCIE=1;

PEIE=1;

GIE=1;

// khai bao timer0 dinh thoi

INTEDG = 1; // ngat tren canh len cua INT

T0CS = 0;

PSA = 0;

PS2 = 0;

PS1 = 0;

PS0 = 1;

TMR0 = 0;// dinh thoi 2ms

T0IF = 0;

T0IE = 0;

//KHAI BAO PWM

TRISC2=0;

TRISC1=0;

PR2=249;//TAN SO PWM LA 10K

T2CKPS0=0;

T2CKPS1=0;//PRESCALE=16;

CCP1CON=0X0C;

TMR2IF=0;

TMR2ON=0;//tat timer2 roi moi thay doi do rong xung

CCPR1L=0;//PWM

TMR2ON=1;//bat timer2 lai

// khoi tao ngatINT

TRISA1 = TRISA2 =TRISA3= 0;



RA1 = RA2 =RA3= 0;

TRISB0=1;

ANS12=0;

INTEDG=0; // NGAT CANH XUONG

INTF=0;

INTE=1;

GIE = 1;

//lcd_init();

_delay(100);

}

void get_str(){

RCIF=0;

RxBuf[RwPtr] = RCREG;

RwPtr++;

RwPtr &= 0x1F;

//dulieu=RCREG;

re_char = RCREG;

if(re_char == 'p'){

str_count = 0;

kp = atof(str);

_delay(10);

xoa_mang();

}

else if(re_char =='i' ){

str_count = 0;

ki = atof(str);

_delay(10);

xoa_mang();

}

else if(re_char == 'd'){

str_count = 0;

kd = atof(str);

_delay(10);

xoa_mang();

}

else if(re_char == 'v'){

str_count = 0;

s_vel = atoi(str);

_delay(10);

xoa_mang();

}

else if(re_char == 'o'){

str_count = 0;

sp = atoi(str);

_delay(10);



//p



//i



//d



//velocity



//position



xoa_mang();

}

else if(re_char == 'm'){

//mode

str_count = 0;

mode=1;

_delay(10);

xoa_mang();

}

else if(re_char == 'n'){

//mode

str_count = 0;

mode=0;

_delay(10);

xoa_mang();

}

else if(re_char=='l'){

str_count = 0;

tx=0;

_delay(10);

xoa_mang();

}

else if(re_char=='k'){

str_count = 0;

tx=1;

_delay(10);

xoa_mang();

}

else if(re_char =='s'){

// s : start

old_xung =vel=vel_old= pos = pos_old = 0;

kt_t=1;

TMR2ON = 0;

CCPR1L =150;

TMR2ON = 1;

RA1 = 0; // quay thuan

RA2 = 1;

TMR1ON=0;

T0IE = 0;

TMR1 = 0;

TMR0 = 0;

TMR_temp=0;

chieu = 0;

INTEDG=0; // NGAT CANH XUONG

err = 0;

sum_err = 0;

d_err = 0;

count_send = 0;



TMR1ON=1; // cho phep counter

T0IE = 1;

// Cho phep timer0 dinh thoi gian

}

else str[str_count++] = re_char;

}

void reset_counter (){

TMR1 = 0;

}

void delay_ms( unsigned int x)

{

for (unsigned long i=0;i
_delay(1000);

}

char uart_getc()

{

while(!RCIF);

return RCREG;

}

void uart_putc(char c)

{

while(!TXIF);

TXREG = c;

}

void putch(char c)

{

if (IN == 0) uart_putc(c);

}

void xoa_mang(){

for(int i=0;i<7;i++)

str[i] = 0;

}

Phục lục 2: chương trình giám sát hệ thống

#pragma endregion

bool s;

private: System::Void run_Click(System::Object^ sender, System::EventArgs^ e) {

if(s)

{

sp->Write("l");

run->Text="Run";

tbps->Enabled=true;

s=0;



}

else

{

sp->Write("s"+"k");

axTChart2->Series(0)->AddXY(0,Convert::ToDouble(tbps>Text),"",2000);// vi tri set

axTChart2->Series(1)->AddXY(0,0,"",2000);

axTChart2->Series(2)->AddXY(0,0,"",2000);

axTChart2->Series(0)>AddXY(time+2000,Convert::ToDouble(tbps->Text),"",2000);

run->Text="Pause";

s=1;

}

}

private: System::Void stop_Click(System::Object^ sender, System::EventArgs^ e) {

sp->Write("x");

tran1=" ";

tran2=" ";

tran3=" ";

tran4=" ";

tran5=" ";

tran6=" ";

}

private: System::Void quit_Click(System::Object^ sender, System::EventArgs^ e) {

this->Close();

}

private: System::Void Form1_Load(System::Object^ sender, System::EventArgs^ e)

{

run->Enabled=false;

stop->Enabled=false;

lbcom->Enabled=true;

array ^ a1=

System::IO::Ports::SerialPort::GetPortNames();

for(int i =0;iLength;i++)

{

lbcom->Items->Add(a1[i]);

}

}

private: System::Void connect_Click(System::Object^ sender, System::EventArgs^ e)

{

sp->PortName=Convert::ToString(lbcom->SelectedItem);



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

Phục lục 1: Chương trình vi điều khiển PIC

Tải bản đầy đủ ngay(0 tr)

×