/* *************************************************************** Current regulation - Pontus Giselsson, Per-Ola Larsson 18/02/09 for LTH - reglerteknik *************************************************************** */ #include <util/twi.h> #include <avr/io.h> #include <avr/interrupt.h> #include <inttypes.h> #include <compat/deprecated.h> // control variables volatile int16_t ref=0; // reference value (from velocity controller) volatile int16_t y; // measurement, 9 frac bits volatile uint16_t low, high; // when reading AD-conversion volatile int16_t e = 0; // control error, 9 frac bits volatile int16_t v = 0; // temporary ctrl signal, 9 frac bits volatile int16_t vSat = 0; // saturated temporary ctrl signal, 9 frac bits volatile int16_t I = 0; // integral part of ctrl, 13 frac bits volatile int16_t u = 0; // ctrl signal = pwm high time (8 bits) volatile int16_t K = 111; // 7 frac bits volatile int16_t Ke = 38; // 7 frac bits, K*h/Ti volatile int8_t intCond = 0; // flag for conditional integration #define V_MAX 508 //max/min for saturation #define V_MIN -508 // twi variable volatile int16_t status; // logging variables /* #define log_len 100 volatile int16_t ctrl_log[log_len]; volatile int16_t error_log[log_len]; volatile int32_t I_log[log_len]; volatile int16_t skipSamples = 1000; volatile int16_t countSamples = 0; volatile int16_t jj=0; volatile int8_t stop = 0; */ /* Routine used to transmit bytes on the serial port */ /* static void putchar(unsigned char ch) { while ((inp(UCSRA) & 0x20) == 0) {}; outp(ch, UDR); while ((inp(UCSRA) & 0x20) == 0) {}; } */ /* Send logged data over Serial connection */ /* static inline void sendData() { int16_t ii = 0; while (ii < log_len) { putchar((unsigned char) ((ctrl_log[ii]&0xff00)>>8)); putchar((unsigned char) (ctrl_log[ii]&0x00ff)); putchar((unsigned char) ((error_log[ii]&0xff00)>>8)); putchar((unsigned char) (error_log[ii]&0x00ff)); putchar((unsigned char) ((I_log[ii]&0xff000000)>>24)); putchar((unsigned char) ((I_log[ii]&0x00ff0000)>>16)); putchar((unsigned char) ((I_log[ii]&0x0000ff00)>>8)); putchar((unsigned char) (I_log[ii]&0x000000ff)); ii++; } } */ /* Timer 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */ SIGNAL(TIMER2_COMP_vect) { // Start AD conversion ADCSRA |= _BV(ADSC); // Read previous AD-conversion result low = inp(ADCL); high = inp(ADCH); y = ((int16_t)((high<<8) | low)) - 512; // y 9 frac bits // control error e = ref-y; // e 9 frac bits // temporary ctrl-signal v = (((K*e+64)>>7)+((I+8)>>4)); // variable that decides if I-part should be updated intCond = 1; // saturation and update integral part of ctrl with antiwindup if (v > V_MAX) { vSat = V_MAX; if (e > 0) intCond = 0; } else if (v < V_MIN) { vSat = V_MIN; if (e < 0) intCond = 0; } else { vSat = v; } if (intCond) I = I + (((Ke*e)+(1<<2))>>3); // ctrl signal, 7 bits + direction u = ((vSat+2)>>2); //7 frac bits to pwm // set pwm switching time if (u < 0) { PORTC |= 0x80; // set direction of motion OCR1BL = ((unsigned char) (-u)); // set length of pwm-high } else { PORTC &= 0x7f; // set direction of motion OCR1BL = ((unsigned char) (u)); // set length of pwm-high } // TWI-communication, recieve reference from velocity controller if ((_BV(TWINT)&inp(TWCR))) { status = (inp(TWSR)&0xf8); // status 0x80 means data recieved if (status == 0x80) { ref = (int16_t)((int8_t)inp(TWDR)); // read 8 bit reference ref = (ref<<2); // shift up 2 steps for 10 bits reference in loop } else { } outp(_BV(TWINT)|_BV(TWEN)|_BV(TWEA),TWCR); } // For logging purposes /* countSamples++; if (countSamples == skipSamples) { ctrl_log[jj] = u; I_log[jj] = I; error_log[jj] = e; jj++; countSamples = 0; // Stop after a while if ((jj == (log_len-1)) & !stop) { outp(0x7f,OCR1BL); stop = 1; sendData(); } } */ } int main() { // clear interrupts (might not be needed) cli(); //Port directions outp(0x08,PORTC); // pull up on overtemperature signals outp(0xa0,DDRC); // output on direction and brake outp(0x10,DDRD); // output on pwm-signal outp(0x40,DDRB); // temp pwm output /* Timer section */ // Timer 1, fast PWM no prescaling (non-inverting mode (start high, switch to low)) outp(_BV(COM1B1)|_BV(WGM11)|_BV(WGM10),TCCR1A); outp(_BV(CS10)|_BV(WGM13)|_BV(WGM12),TCCR1B); // Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> 115.2 kHz) outp(0x00,OCR1AH); outp(0x7f,OCR1AL); outp(0x00,TCNT1H); outp(0x00,TCNT1L); outp(0x00,OCR1BH); outp(0x7f,OCR1BL); // to not start motor-rotation before control /* Timer 2, 4 times pwm-period, for control sampling, prescaler 8, 28.8 kHz */ outp(_BV(WGM21)|_BV(CS21),TCCR2); outp(0x3f,OCR2); /* Reset timer 2 */ outp(0,TCNT2); // Enable timer2 compare match interrupts outp(_BV(OCIE2),TIMSK); //Serial communication outp(0x00, UCSRA); // USART: outp(0x18, UCSRB); // USART: RxEnable|TxEnable outp(0x86, UCSRC); // USART: 8bit, no parity outp(0x00, UBRRH); // USART: 115200 @ 14.7456MHz outp(7,UBRRL); // USART: 115200 @ 14.7456MHz /* AREF (AREF is 5V) pin external capacitor, MUX0 for current measurement */ outp(_BV(REFS0)|_BV(MUX0),ADMUX); // Enable ADC, start first conversion, prescaler 32, not free running mode outp(_BV(ADEN)|_BV(ADSC)|_BV(ADPS2)|_BV(ADPS0),ADCSRA); // Initialize TWI outp(0x02,TWBR); // set SCL-frequency CPU-freq/(16+2*2) outp(0x02,TWAR); // slave address outp(_BV(TWEA)|_BV(TWEN),TWCR); // enable TWI, enable ACK //Enable interrupts sei(); // loop while (1) {} }