Don't ad-block us - support your favorite websites. We have safe, unobstrusive, robotics related ads that you actually want to see - see here for more.
0 Members and 1 Guest are viewing this topic.
Chip type : ATmega8Program type : ApplicationAVR Core Clock frequency: 8.000000 MHzMemory model : SmallExternal RAM size : 0Data Stack size : 256*****************************************************/#include <mega8.h>#include <delay.h>#ifndef RXB8#define RXB8 1#endif#ifndef TXB8#define TXB8 0#endif#ifndef UPE#define UPE 2#endif#ifndef DOR#define DOR 3#endif#ifndef FE#define FE 4#endif#ifndef UDRE#define UDRE 5#endif#ifndef RXC#define RXC 7#endif#define FRAMING_ERROR (1<<FE)#define PARITY_ERROR (1<<UPE)#define DATA_OVERRUN (1<<DOR)#define DATA_REGISTER_EMPTY (1<<UDRE)#define RX_COMPLETE (1<<RXC)// USART Receiver buffer#define RX_BUFFER_SIZE 8char rx_buffer[RX_BUFFER_SIZE];#if RX_BUFFER_SIZE<256unsigned char rx_wr_index,rx_rd_index,rx_counter;#elseunsigned int rx_wr_index,rx_rd_index,rx_counter;#endif// This flag is set on USART Receiver buffer overflowbit rx_buffer_overflow;// USART Receiver interrupt service routineinterrupt [USART_RXC] void usart_rx_isr(void){char status,data;status=UCSRA;data=UDR;if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0) { rx_buffer[rx_wr_index]=data; if (++rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0; if (++rx_counter == RX_BUFFER_SIZE) { rx_counter=0; rx_buffer_overflow=1; }; };}#ifndef _DEBUG_TERMINAL_IO_// Get a character from the USART Receiver buffer#define _ALTERNATE_GETCHAR_#pragma used+char getchar(void){char data;while (rx_counter==0);data=rx_buffer[rx_rd_index];if (++rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;#asm("cli")--rx_counter;#asm("sei")return data;}#pragma used-#endif// USART Transmitter buffer#define TX_BUFFER_SIZE 8char tx_buffer[TX_BUFFER_SIZE];#if TX_BUFFER_SIZE<256unsigned char tx_wr_index,tx_rd_index,tx_counter;#elseunsigned int tx_wr_index,tx_rd_index,tx_counter;#endif// USART Transmitter interrupt service routineinterrupt [USART_TXC] void usart_tx_isr(void){if (tx_counter) { --tx_counter; UDR=tx_buffer[tx_rd_index]; if (++tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0; };}#ifndef _DEBUG_TERMINAL_IO_// Write a character to the USART Transmitter buffer#define _ALTERNATE_PUTCHAR_#pragma used+void putchar(char c){while (tx_counter == TX_BUFFER_SIZE);#asm("cli")if (tx_counter || ((UCSRA & DATA_REGISTER_EMPTY)==0)) { tx_buffer[tx_wr_index]=c; if (++tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0; ++tx_counter; }else UDR=c;#asm("sei")}#pragma used-#endif// Standard Input/Output functions#include <stdio.h>#define FIRST_ADC_INPUT 5#define LAST_ADC_INPUT 5int adc_data[LAST_ADC_INPUT-FIRST_ADC_INPUT+1];#define ADC_VREF_TYPE 0x00// ADC interrupt service routine// with auto input scanninginterrupt [ADC_INT] void adc_isr(void){static unsigned char input_index=0;// Read the AD conversion resultadc_data[input_index]=ADCW;// Select next ADC inputif (++input_index > (LAST_ADC_INPUT-FIRST_ADC_INPUT)) input_index=0;ADMUX=(FIRST_ADC_INPUT | (ADC_VREF_TYPE & 0xff))+input_index;// Delay needed for the stabilization of the ADC input voltagedelay_us(10);// Start the AD conversionADCSRA|=0x40;}// MOtor output handls motor pins except PWM// direction and fast decay includedvoid M_out(char state){ if(state==0) { PORTB=0; } else if(state==2) { PORTB=7; PORTD=0x00; } else if(state==1){ PORTB=6; PORTD=0xf0; } }// Timer2 output compare interrupt service routine// This timer is the RTC timer for the control loop// this varialble carries the desired position// variable that carries the desired pos for loop functionint desired_pos;int i_error;interrupt [TIM2_COMP] void timer2_comp_isr(void){// Place your code here // Holds the position error int p_error; p_error = desired_pos-adc_data[0]; // now perform PID control //asume first it's a P controller //assume P =1 if(p_error<0){ M_out(2); p_error*=-1; } else M_out(1); if (p_error<=1){ p_error=0; i_error=0; } else{ if(p_error<=40){ // add PI controller if(i_error<450) i_error+=p_error; else i_error=450; p_error*=40; p_error+=i_error*4; //else if(p_error<=20) // p_error*=50; } else if(p_error<=50) p_error*=20; else p_error=1023; } //if(p_error<=6) // p_error=0; OCR1AH=p_error/256; OCR1AL=p_error;}// Declare your global variables herevoid main(void){// Declare your local variables here// Input/Output Ports initialization// Port B initialization// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=Out Func1=Out Func0=Out // State7=T State6=T State5=T State4=T State3=T State2=1 State1=1 State0=1 PORTB=0x07;DDRB=0x07;// Port C initialization// Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In // State6=T State5=T State4=T State3=T State2=T State1=T State0=T PORTC=0x00;DDRC=0x00;// Port D initialization// Func7=Out Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In // State7=0 State6=T State5=T State4=T State3=T State2=T State1=T State0=T PORTD=0x00;DDRD=0x80;// Timer/Counter 0 initialization// Clock source: System Clock// Clock value: Timer 0 StoppedTCCR0=0x00;TCNT0=0x00;// Timer/Counter 1 initialization// Clock source: System Clock// Clock value: 8000.000 kHz// Mode: Fast PWM top=03FFh// OC1A output: Inverted// OC1B output: Discon.// Noise Canceler: Off// Input Capture on Falling Edge// Timer1 Overflow Interrupt: Off// Input Capture Interrupt: Off// Compare A Match Interrupt: Off// Compare B Match Interrupt: OffTCCR1A=0xC3;TCCR1B=0x09;TCNT1H=0x00;TCNT1L=0x00;ICR1H=0x00;ICR1L=0x00;OCR1AH=0x00;OCR1AL=0x00;OCR1BH=0x00;OCR1BL=0x00;// Timer/Counter 2 initialization// Clock source: System Clock// Clock value: 7.813 kHz// Mode: CTC top=OCR2// OC2 output: DisconnectedASSR=0x00;TCCR2=0x0F;TCNT2=0x00;OCR2=0x0F;// External Interrupt(s) initialization// INT0: Off// INT1: OffMCUCR=0x00;// Timer(s)/Counter(s) Interrupt(s) initializationTIMSK=0x80;// USART initialization// Communication Parameters: 8 Data, 1 Stop, No Parity// USART Receiver: On// USART Transmitter: On// USART Mode: Asynchronous// USART Baud Rate: 2400 (Double Speed Mode)UCSRA=0x02;UCSRB=0xD8;UCSRC=0x86;UBRRH=0x01;UBRRL=0xA0;// Analog Comparator initialization// Analog Comparator: Off// Analog Comparator Input Capture by Timer/Counter 1: OffACSR=0x80;SFIOR=0x00;// ADC initialization// ADC Clock frequency: 500.000 kHz// ADC Voltage Reference: AREF pinADMUX=FIRST_ADC_INPUT | (ADC_VREF_TYPE & 0xff);ADCSRA=0xCC;// Global enable interrupts#asm("sei")//initialize control loop variablesdelay_ms(10);desired_pos=adc_data[0];//test statement//desired_pos=500;while (1) { // Place your code here desired_pos=getchar()*4; };}
Depending if your error is negative/positive you set up some GPIO pins.
If your error is small enough (<= 1) you set the error and integral state error as if it was 0.Otherwise, if it's a considerable error, you set up 3 cases of error: below 40, 40-50, above 50. In each of these 3 cases you have different values of gains.Only in the first case (below 40) you activate PI control. You limit the integral state error with "if(i_error<450)".In the second case you have proportional gain only (value 20) and in the last case you set the error (or better said in this case output control value) to 1023 [for limiting, or saturation?].In the end you write the calculated output to the PWM timers? Did I understand correctly?
Why do you have so many stages or cases?
When you are bored and have nothing better to do could you look into my PID algorithm (link above Pid.c) to see if I have slipped a mistake somewhere and also what could I improve? [function is called pid_execute]
Kp = 14where does 0.7142 come from?
My Pi controller looks like this: 14 + 10 * 1/sSo I thought you referred to this.