/********************************************* Current regulation - Aleks Ponjavic 18/09/07 for LTH - reglerteknik Note that the voltage is actually a duty cycle for the PWM. The PWM is running at 12V with a duty cycle set by a 10bit value *********************************************/ //#include //Only include if sin will be used, takes up memory! #include #include #include #include // reference generation variables volatile int16_t ref = 0; volatile int16_t refCount = 0; volatile int8_t refFlag = 0; // control variables volatile unsigned char lbyte,hbyte; volatile int16_t y; volatile uint8_t alt = 1; volatile uint8_t low, high; //test variables volatile int16_t e = 0; volatile int16_t v = 0; volatile int16_t I = 0; volatile int16_t u = 0; volatile int16_t K = 51;//5;//375; // 7 frac bits volatile int16_t Ke = 20;//2;//6; //7 frac bits, K*h/Ti volatile int16_t Ksat = 51;//1; // 7 frac bits, h/Tr // 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 = 1; volatile int16_t countSamples = 0; volatile int16_t jj=0; volatile int8_t stop = 0; volatile int16_t temp; /* Routine used to set the red LED */ void setLED(uint8_t on) { if (on) PORTB &= 0x7f; //Turn on else PORTB |= 0x80; //Turn off } /* 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) {}; } /* Interrupt service routine for handling incoming bytes on the serial port might be needed to catch incoming bytes */ SIGNAL(SIG_UART_RECV){} static inline int16_t readInput() { uint8_t low, high; ADCSRA |= 0x40; while (ADCSRA & 0x40); low = ADCL; high = ADCH; return ((high<<8) | low) - 512; } /* Write 8-bit output using the PWM-generator */ static inline void writeOutput(int16_t val) { if (val < 0) { PORTC = 0x80+(PORTC & 0x7F); OCR1BH = 0; //(unsigned char) (-val)&0xff00; OCR1BL = (unsigned char) (-val)&0x00ff; } else { PORTC = (PORTC & 0x7F); OCR1BH = 0; //(unsigned char) (val&0xff00); OCR1BL = (unsigned char) (val&0x00ff); } } /* 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++; } } /* Interrupt when AD-conversion completes */ SIGNAL(SIG_ADC) { PORTA |= 0x10; //Turn on calculation timer // Read input low = inp(ADCL); high = inp(ADCH); y = ((high<<8) | low) - 512; //y 9 frac bits //control, since negative measurements e = ref+y; //e 9 frac bits //v = (int16_t)((K*e+I+64)>>7); v = (int16_t)(((K*e+64)>>7)+(temp>>4)); //saturation and update integral part of ctrl if (v > 511) { temp = temp + (((Ke*e) + (Ksat)*(511-v))>>3); } else if (v < -512) { temp = temp + (((Ke*e) + (Ksat)*(-512-v))>>3); } else { temp = temp + ((Ke*e)>>3); } /* //saturation and update integral part of ctrl if (v > 511) { I = I +(int32_t)((Ke*e) + (Ksat)*(511-v)); } else if (v < -512) { I = I +(int32_t)((Ke*e) + (Ksat)*(-512-v)); } else { I = I +(int32_t)(Ke*e); } */ // write output, inverting mode means set pwm to 127-ctrl_out // Original styrning med 7 bitar + direction /* u = (v+2)>>2; //7 frac bits to pwm if (u > 127) { u = 127; } else if (u < -128) { u = -128; } */ // upplösning på styrsignal [-104,103] för att matcha sampelperiod u = ((v*13+32)>>6); if (u > 104) { u = 104; } else if (u < -105) { u = -105; } if (u < 0) { //PORTC = 0x80+(PORTC & 0x7F); PORTC |= 0x80; OCR1BL = (unsigned char) (104-(-u)); } else { PORTC = (PORTC & 0x7F); OCR1BL = (unsigned char) (105-u); } // For logging countSamples++; if (countSamples == skipSamples) { ctrl_log[jj] = y; I_log[jj] = u; error_log[jj] = e; jj++; countSamples = 0; // Stop after a while if ((jj == (log_len-1)) & !stop) { outp(0x7f,OCR1BL); stop = 1; sendData(); } if (jj == 30) ref = 100; } PORTA &= 0xef; //Turn off calculation timer } /* reference square- or triangle wave generator with timer 0 */ SIGNAL(SIG_OVERFLOW0) { int8_t rectangle = 1; refCount++; if (rectangle == 1) { if (refFlag == 0) { if (refCount == 10) { refFlag = 1; ref = -ref; refCount = 0; } } else { if (refCount == 20) { ref = -ref; refCount = 0; } } } else { if (refCount <= 20) { // ref*2 ref -= 2; } else { ref += 2; } if (refCount == 40) { // ref*4 refCount = 0; } } } int main() { cli(); int i,j; //Port directions outp(0x80,PORTB); // LED off outp(0x80,DDRB); // output on LED outp(0x08,PORTC); // pull up on overtemp signals outp(0xa0,DDRC); // output on dir and brake outp(0x80,PORTD); // pull up on reset switch outp(0x10,DDRD); // output on pwm for motor 1 outp(0x10,DDRA); // test pin output /* Timer section */ // Enable timer0 overflow interrupts outp(BV(TOIE0),TIMSK); // Timer 1, fast PWM no prescaling (inverting mode (start low, switch to high)) outp(BV(COM1A1)|BV(COM1B1)|BV(COM1A0)|BV(COM1B0)|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-> h_pwm=8.61 micros) // 7 bit ctrl-signal and direction //outp(0x00,OCR1AH); //outp(0x7f,OCR1AL); // 104-quant on ctrl-signal and direction outp(0x00,OCR1AH); outp(104,OCR1AL); outp(0x00,TCNT1H); outp(0x00,TCNT1L); // 7 bit ctrl-signal and direction //outp(0x00,OCR1BH); //outp(0x7f,OCR1BL); // to not start motor-rotation before control outp(0x00,OCR1BH); outp(104,OCR1BL); // to not start motor-rotation before control // 104-quant on ctrl-signal and direction /* Timer 2 (control loop), prescaler 256, clear on compare match (28), -> h = 0.5 ms */ // outp(BV(WGM21)|BV(CS22)|BV(CS21),TCCR2); //outp(28,OCR2); /* Reset timer 2 */ //outp(0,TCNT2); /* Timer 0 for reference generation, prescaler = 1024 periodic h = ? */ outp(BV(CS02)|BV(CS00),TCCR0); //Serial communication outp(0x00, UCSRA); // USART: outp(0x98, UCSRB); // USART: RxIntEnable|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, MUX3?? for pendulum angle */ outp(BV(REFS0)|BV(MUX0),ADMUX); // Enable ADC interrupts, start first conversion (what do adps0-2 do), prescaler 32 outp(BV(ADEN)|BV(ADATE)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA); // outp(BV(ADEN)|BV(ADSC),ADCSRA); /* Wait a little bit, probably not needed...*/ int tmp; // for(i=0;i<2000;i++) // for(j=0;j<400;j++) // tmp = j*j*j; //Enable interrupts sei(); // loop while (1) { if (stop) { cli(); OCR1BL = 0x7f; } } }