Commit b1d01b07 authored by Pontus Giselsson's avatar Pontus Giselsson
Browse files

PI current control, sync between pwm and control period, 4*pwm = ctrl_period,...

PI current control, sync between pwm and control period, 4*pwm = ctrl_period, longer sampling time than before, not better performance since measurements not good enough
parent 8693434d
......@@ -33,9 +33,9 @@ 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
volatile int16_t K = 74;//5;//375; // 7 frac bits
volatile int16_t Ke = 26;//2;//6; //7 frac bits, K*h/Ti
volatile int16_t Ksat = 44;//1; // 7 frac bits, h/Tr
// logging variables
......@@ -43,13 +43,17 @@ volatile int16_t Ksat = 51;//1; // 7 frac bits, h/Tr
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 = 30000;
volatile int16_t skipSamples = 1000;
volatile int16_t countSamples = 0;
volatile int16_t jj=0;
volatile int8_t stop = 0;
volatile int16_t temp;
// pwm sync variables
volatile int8_t pwmCycles = 0;
/* Routine used to set the red LED */
void setLED(uint8_t on)
{
......@@ -118,19 +122,25 @@ static inline void sendData() {
}
/* Interrupt when AD-conversion completes */
SIGNAL(SIG_ADC)
{
/* Timer 1 compare match, start AD-conversion
* Additional changes: Dont use free running AD-conversion
* Enable overflow interrupts timer1
*/
SIGNAL(SIG_OUTPUT_COMPARE2) {
// Start AD conversion
ADCSRA |= BV(ADSC);
// Code previously in 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));
......@@ -142,39 +152,39 @@ SIGNAL(SIG_ADC)
} 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));
I = I +(int32_t)((Ke*e) + (Ksat)*(511-v));
} else if (v < -512) {
I = I +(int32_t)((Ke*e) + (Ksat)*(-512-v));
I = I +(int32_t)((Ke*e) + (Ksat)*(-512-v));
} else {
I = I +(int32_t)(Ke*e);
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;
}
if (u < 0) {
PORTC |= 0x80;
OCR1BL = (unsigned char) (128-(-u));
} else {
PORTC = (PORTC & 0x7F);
OCR1BL = (unsigned char) (127-u);
OCR1BL = (unsigned char) (127-u);
}
// For logging
countSamples++;
if (countSamples == skipSamples) {
......@@ -183,16 +193,17 @@ SIGNAL(SIG_ADC)
error_log[jj] = e;
jj++;
countSamples = 0;
// Stop after a while
if ((jj == (log_len-1)) & !stop) {
outp(0x7f,OCR1BL);
stop = 1;
sendData();
outp(0x7f,OCR1BL);
stop = 1;
sendData();
}
}
PORTA &= 0xef; //Turn off calculation timer
}
......@@ -244,7 +255,8 @@ int main()
/* Timer section */
// Enable timer0 overflow interrupts
outp(BV(TOIE0),TIMSK);
// and enable timer1 overflow interrupts for pwm-control sync
outp(BV(TOIE0)|BV(OCIE2),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);
......@@ -263,11 +275,11 @@ int main()
outp(0x7f,OCR1BL); // to not start motor-rotation before control
/* 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);
/* Timer 2, 4 times pwm-period, for control sampling, prescaler 8 */
outp(BV(WGM21)|BV(CS21),TCCR2);
outp(0x3f,OCR2);
/* Reset timer 2 */
//outp(0,TCNT2);
outp(0,TCNT2);
/* Timer 0 for reference generation, prescaler = 1024 periodic h = ? */
outp(BV(CS02)|BV(CS00),TCCR0);
......@@ -283,8 +295,15 @@ int main()
/* 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);
// Enable ADC interrupts, start first conversion, prescaler 32, free running mode
//outp(BV(ADEN)|BV(ADATE)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Enable ADC interrupts, start first conversion, prescaler 32, not free running mode
//outp(BV(ADEN)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Enable ADC, start first conversion, prescaler 32, not free running mode
outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// outp(BV(ADEN)|BV(ADSC),ADCSRA);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment