/*********************************************
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 <math.h> //Only include if sin will be used, takes up memory!
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.h>
// 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;
}
}
}