/*ECE 476 SP 2004 Tom Ling Norbert Huber Final Project*/ #include #include // sprintf #include // delay_ms #include /* Use an 1x16 alphanumeric LCD connected to PORTC as follows: [LCD] [AT90S8515 DIP40] 1 GND- 20 GND 2 +5V- 40 VCC 3 VLC 10k trimpot wiper (trimpot ends go to +5 and gnd) 4 RS - 21 PC0 5 RD - 22 PC1 6 EN - 23 PC2 11 D4 - 25 PC4 12 D5 - 26 PC5 13 D6 - 27 PC6 14 D7 - 28 PC7 */ #asm .equ __lcd_port=0x15 #endasm #include // LCD driver routines void main(void); // main function void initialize(void); //initializations void get_AD(void); //get the readings from the accelerometer void Prop_ctrl(void); //PID control of the motor void button_ctrl(void); //handles button pushed void motor(int,char); //PWM output for the motor void Powerdown(void); //Power down mode void calibration(void); //Adjust PCONST automatically(non-linear control) void cons_disp(void); //display the gain/offsets on the LCD #define begin { #define end } #define RATE 10 //rate for every AD conversion #define CONST 30 //button debounce every 30ms #define THOLD 16 //holding down the button #define LCDwidth 16 //characters #define FACTOR1 1 //Power output when too low #define FACTOR2 0.5 //Power output when too high char lcd_buffer[17]; // LCD display buffer int Ain, Aold; //raw A to D number int ADcount; //counts for every ADC int bcount; //counts for button state machine int timehold; //button hold int btoggle; //toggle the constants handled unsigned char bstate, on, angle_index, angle_actual; unsigned char actual[5] = {60,75,90,105,120}; unsigned char aindex[5] = {85,107,127,149,171}; int j; enum{nopush,maybepush,pushed,maybenopush}; unsigned int PCONST; unsigned int cons[5] = {97,100,103,106,109}; int cal, wait, cflag; int power; int error; int prev_error; //previous error for Derivative control int sum_error; //error sum for Integral control int PWM_duty; char direction; float KP,KD,KI; // Control constants interrupt [TIM0_COMP] void timer0_compare(void) begin if(ADcount>0) ADcount--; if(bcount>0) bcount--; if(wait>0) wait--; end void main(void) begin int i,Atemp; i = 0; Atemp = 0; initialize(); while(1) { if(ADcount <=0) { ADcount = RATE; get_AD(); if(on == 1) Prop_ctrl(); if(on == 0 && OCR2 != 0) Powerdown(); i++; if(i == 80) { //calculate fluctuation to determine whether to turn on calibration Atemp = Ain-Aold; if((Atemp<-10 || Atemp>10) && cal == 1) {cal = 0;} if((Atemp>-10 && Atemp<10) && cal == 0) {cal = 1;} lcd_gotoxy(0,0); sprintf(lcd_buffer,"%d ",Ain); lcd_puts(lcd_buffer); i=0; Aold = Ain; } } if(cal == 1 && wait <= 0 && cflag == 0 && Ain>45 && Ain<210) { calibration(); lcd_gotoxy(15,0); lcd_putsf("c"); } if(cal == 0 || cflag == 1) { lcd_gotoxy(15,0); lcd_putsf(" "); } if(bcount<=0) button_ctrl(); } end void get_AD(void) begin ADCSR.6 = 1; while(ADCSR.6 == 1){} Ain = ADCH; end void Prop_ctrl(void) begin float prop_term; float deriv_term; float integ_term; float sum_terms; prop_term = 0; deriv_term = 0; integ_term = 0; sum_terms = 0; // calculate error error = Ain - angle_index; // Propotional calculation of PD prop_term = KP*(float)error; sum_terms += prop_term; // Derivative calculation of PD if (KD != 0) { deriv_term = KD*(float)(error - prev_error); prev_error = error; sum_terms += deriv_term; } // Integeral term calc of PID if (KI != 0 && Ain>45 && Ain<200) { if(error == 0) sum_error = 0; if(error*prev_error<0) sum_error = 0; if(sum_error<2000 && sum_error>-2000) sum_error += error; integ_term = KI*(float)sum_error; sum_terms += integ_term; } // Take the absolute value and set direction bit if (sum_terms > 0) { direction = 0; } else { direction = 1; sum_terms *= -1; } power = (int)sum_terms; if(power == 0 && error !=0) power = 1; lcd_gotoxy(4,1); sprintf(lcd_buffer,"%d ", error); lcd_puts(lcd_buffer); motor(power, direction); end void motor(int power, char direction) begin if(direction == 1) { PWM_duty = power*FACTOR1 + PCONST; if (PWM_duty >255) PWM_duty = 255; OCR2 = (char)PWM_duty; } else { PWM_duty = PCONST - power*FACTOR2; if(Ain >210) PWM_duty = PCONST - power*FACTOR2*2; if(Ain >240) PWM_duty = 50; if(PWM_duty <0) PWM_duty = 0; OCR2 = (char)PWM_duty; } lcd_gotoxy(13,1); sprintf(lcd_buffer,"%d ", OCR2); lcd_puts(lcd_buffer); end void Powerdown(void) begin if(Ain > 95) PWM_duty = Ain-30; else PWM_duty = Ain; if(PWM_duty > 95) PWM_duty = 95; if(Ain<25) PWM_duty =0; OCR2 = (char)PWM_duty; lcd_gotoxy(13,1); sprintf(lcd_buffer,"%d ", OCR2); lcd_puts(lcd_buffer); end void calibration(void) begin if(direction == 0 && error > 3 && prev_error>2) { PCONST--; } else if(direction == 1 && error*-1 > 3 && prev_error*-1>2) { PCONST++; } if(btoggle ==0) cons_disp(); if (error>30 || error<-30) wait = 600; else if(error<10 && error>-10) wait = 1500; else wait = 1000; if (error>50 || error<-50) wait = 300; end void button_ctrl(void) begin bcount = CONST; /* button 0: toggle on/off button 1: increase angle by 15 degrees button 2: decrease angle by 15 degrees button 3: toggle constants: PCONST, KP, KD, KI button 4: increase the constants button 5: decrease the constants button 6: calibration mode on/off button 7: reset PCONST to preset value */ switch(bstate) { case nopush: timehold = THOLD; if(PINB != 0xff) bstate = maybepush; break; case maybepush: if(PINB != 0xff) { bstate = pushed; timehold--; if(PINB == ~0x01) { if(on == 1) { on = 0; lcd_gotoxy(0,1); lcd_putsf("off"); sum_error = 0; error = 0; prev_error = 0; cflag = 1; lcd_gotoxy(15,0); lcd_putsf(" "); } else { on =1; lcd_gotoxy(0,1); cflag = 0; lcd_putsf("on "); } } if(PINB == ~0x02) { if(j<4) j++; angle_index = aindex[j]; PCONST = cons[j]; lcd_gotoxy(9,1); sprintf(lcd_buffer,"%d ",angle_index); lcd_puts(lcd_buffer); } if(PINB == ~0x04) { if(j>0) j--; angle_index = aindex[j]; PCONST = cons[j]; lcd_gotoxy(9,1); sprintf(lcd_buffer,"%d ",angle_index); lcd_puts(lcd_buffer); } if(PINB == ~0x08) { btoggle++; if(btoggle >= 4) btoggle = 0; lcd_gotoxy(5,0); switch(btoggle) { case 0: lcd_putsf("C"); cons_disp(); break; case 1: lcd_putsf("P"); cons_disp(); break; case 2: lcd_putsf("D"); cons_disp(); break; case 3: lcd_putsf("I"); cons_disp(); break; } } if(PINB == ~0x10) { switch(btoggle) { case 0: PCONST++; cons_disp(); break; case 1: KP = KP + 0.1; cons_disp(); break; case 2: KD = KD + 0.01; cons_disp(); break; case 3: KI = KI +0.001; cons_disp(); break; } } if(PINB == ~0x20) { switch(btoggle) { case 0: PCONST--; cons_disp(); break; case 1: KP = KP - 0.1; cons_disp(); break; case 2: KD = KD - 0.01; cons_disp(); break; case 3: KI = KI - 0.001; cons_disp(); break; } } if(PINB == ~0x40) { if(cflag == 0) { cflag = 1; } else if(cflag == 1) { cflag = 0; } } if(PINB == ~0x80) { PCONST = cons[j]; if(btoggle == 0) cons_disp(); } } else bstate = nopush; break; case pushed: if(PINB == ~0x10) begin timehold--; if(timehold<=0 && btoggle==0) begin PCONST++; timehold = THOLD; cons_disp(); end end if(PINB == ~0x20) begin timehold--; if(timehold<=0 && btoggle==0) begin PCONST--; timehold = THOLD; cons_disp(); end end if(PINB == 0xff) bstate = maybenopush; break; case maybenopush: if(PINB == 0xff) bstate = nopush; else bstate = pushed; break; } end void cons_disp(void) begin lcd_gotoxy(7,0); if(btoggle == 0) sprintf(lcd_buffer,"%d ",PCONST); else if(btoggle == 1) sprintf(lcd_buffer,"%3.1f ",KP); else if(btoggle == 2) sprintf(lcd_buffer,"%4.2f ",KD); else if(btoggle == 3) sprintf(lcd_buffer,"%5.3f ",KI); lcd_puts(lcd_buffer); end void initialize(void) begin // initialize the LCD for 16 char wide lcd_init(LCDwidth); //initialize the display lcd_clear(); //clear the display lcd_gotoxy(0,1); lcd_putsf("off"); //set up timer 0 TIMSK=2; //turn on timer 0 cmp match ISR OCR0 = 250; //set the compare re to 250 time ticks //prescalar to 64 and turn on clear-on-match TCCR0=0b00001011; // clock/64 and enable interrupt //init the A to D converter //channel zero / left adj /5V ref ADMUX = 0b11100000; //enable ADC and set prescaler to 1/64*8MHz=125,000 //and set int enable ADCSR = 0b11000111; //set up timer 2 OCR2 = 0; //set the compare re to 0 time ticks //prescalar to 64 and turn on clear-on-match TCCR2 = 0b01101100; // clock/64 and enable interrupt //PORTD.7 is PWM output DDRD = 0xff; //PORT B are button inputs DDRB = 0x00; //PORT A: analog input DDRA = 0x00; ADcount = RATE; bcount = CONST; bstate = nopush; on = 0; j = 2; angle_index = aindex[2]; angle_actual = actual[2]; PCONST = cons[2]; KP = 0.1; KD = 0.01; KI = 0.001; wait = 0; error = 0; prev_error = 0; sum_error = 0; cal = 0; cflag = 0; btoggle = 0; lcd_gotoxy(5,0); lcd_putsf("C"); lcd_gotoxy(7,0); sprintf(lcd_buffer,"%d ",PCONST); lcd_puts(lcd_buffer); lcd_gotoxy(9,1); sprintf(lcd_buffer,"%d ",angle_index); lcd_puts(lcd_buffer); #asm sei #endasm end