Select Git revision
labcomm2006_dynamic_buffer_writer.c
current_control.c 5.73 KiB
/*
***************************************************************
Current regulation - Pontus Giselsson, Per-Ola Larsson 18/02/09
for LTH - reglerteknik
***************************************************************
*/
#include <avr/twi.h>
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.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(SIG_OUTPUT_COMPARE2) {
// 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) {}
}