PID_POSITION_OK.rar_PIC PID_pic motor_pid pic

#include <18F4431.h> #fuses NOWDT,PUT,HS,NOPROTECT,NOLVP #use delay(clock=20000000) #use RS232(baud=9600,PARITY=N ,xmit=PIN_C6, rcv=PIN_C7) #byte QEICON = 0xFB6 // thanh ghi QEICON dieu khien QEI #bit UP_DOWN = 0xFB6.5 //bit trang thai bao huong quay encoder #byte DFLTCON = 0XF60 // thanh ghi DFLTCON cua khoi noise filter #byte POSCNTH = 0xF67 // thanh ghi POSCNTH 8 bit cao dem xung encoder #byte POSCNTL = 0xF66 // thanh ghi POSCNTH 8 bit thap dem xung encoder #byte MAXCNTH = 0xF65 // MAXCOUNT HIGH REGISTER #byte MAXCNTL = 0xF64 // MAXCOUNT LOW REGISTER signed int16 pos_counter = 0,encoder=0,pwm=0,run_encoder=0,speed_limit,p=0,i1=0,i2=0; signed int16 old_encoder = 0; signed int16 d1=0,d2=0; int status=0; signed int8 const Kp=1.2,Ki=0.1,Kd=0.5; // he so ti le void QEI_init() // ham khoi tao module QEI { DFLTCON = 0b00110001; // FILTER FOR QEA, QEB | FILTER CLOCK DIVIDER = 1:2 QEICON = 0b10101000; // x2 update mode MAXCNTH = 0xFF; MAXCNTL = 0xFF; POSCNTH = 0x00; POSCNTL = 0x00; } //============================================================= } } void tien(signed int16 speed) { output_bit(PIN_B0,0); set_power_pwm0_duty(1023-speed); } void lui(signed int16 speed) { output_bit(PIN_B0,1); set_power_pwm0_duty(1023-speed); } void stop() { set_power_pwm0_duty(1023); } void reset_pos_counter() // ham reset gia tri cua position counter { POSCNTL=0x00; POSCNTH=0x00; } /* signed int Make16(unsigned char low,unsigned char high) { signed int encoder_position = 0; encoder_position |= high; encoder_position <<= 8; encoder_position |= low; return encoder_position; }*/ void up_down_encoder() // ham dem xung encoder { pos_counter = make16(POSCNTH,POSCNTL); // ghep 2 thanh ghi dem if(UP_DOWN == 1) status=1; // tien if(UP_DOWN ==0) status=0; // lui } void PID_P(signed int16 encoder_desire,signed int16 speed_limit) // giai thuan PID (ti? le) { signed int16 pwm0 = 1023; // tinh sai so // encoder_desire : gia tri encoder den vi tri can // thanh ghi pos_counter thiet lap gia tri hien tai cua encoder //speed_limit: bien thiet lap toc do encoder = pos_counter - encoder_desire; // tinh luc pwm thuc te p = Kp*encoder; // tinh PID: pwm = p + i2 + d2; //tiep tuc giam he so K de pwm k qua lo'n===>gia toc nho? //pwm = pwm/2; // thiet lap gioi han cho pwm ngan truong hop pwm qua lon if(pwm < -speed_limit) pwm = -speed_limit; if(pwm > speed_limit) pwm = speed_limit; if(pwm < 0) tien(-(pwm)); if(pwm > 0) lui(pwm); if(pwm==0) stop(); } //============================================================================== void PID_I(signed int16 encoder_desire) // giai thuat PID (tich phan) { signed int16 tich_phan = 150;//150; static unsigned char dem_i; i1 = Ki * (i1 + (pos_counter - encoder_desire)); dem_i++; if(dem_i>3) { i2 = i1 / 6;//5; if(i2 > tich_phan) i2 = tich_phan; if(i2 < -tich_phan) i2 = -tich_phan; dem_i=0; } } //============================================================================== void PID_D(signed int16 encoder_desire) // giai thuat PID (dao ham) { signed int16 dao_ham = 200;//0; static unsigned char dem_d; encoder = pos_counter - encoder_desire; dem_d++; if(dem_d > 3) { d1 = Kd*(encoder - old_encoder); old_encoder = encoder; d2 = d1 / 12;//12; if(d2 > dao_ham) d2 = dao_ham; if(d2 < - dao_ham) d2 = -dao_ham; dem_d = 0; } } //============================================================================= void main() { int8 i; QEI_init(); // chia tan xung clock la 16 (prescale 1:4 cua time base) // chon che do free run // postscale la 1 // time base set 0 //period la 250 // compare la 0 // postcale compare la 1 //dead time la 0 setup_power_pwm(PWM_CLOCK_DIV_16 | PWM_FREE_RUN,1,0,254,0,1,0); setup_power_pwm_pins(PWM_ODD_ON, PWM_ODD_ON, PWM_OFF, PWM_OFF); // set_power_pwm_override(0, false, 1); run_encoder = 15000; // setpoint speed_limit = 923; while(1) { // output_bit(PIN_B0,1); // output_bit(PIN_B1,0); // stop(); //lui(200); // set_power_pwm0_duty(1000); up_down_encoder(); PID_I(run_encoder); PID_D(run_encoder); PID_P(run_encoder,speed_limit); // delay_ms(3); // printf("\r,%ld",pos_counter); // printf("\r:,%ld",pwm); // printf("\rI:,%ld",i2); //printf("\rD:,%ld",d2); } }