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

Step response again, with lower pwm resultion in order to match pwm-cycle with...

Step response again, with lower pwm resultion in order to match pwm-cycle with sampling period, an attempt to reduce noise (but not very succesful).
parent 2adcd4c2
......@@ -17,7 +17,7 @@
// reference generation variables
volatile int16_t ref = 100;
volatile int16_t ref = 0;
volatile int16_t refCount = 0;
volatile int8_t refFlag = 0;
......@@ -32,10 +32,10 @@ volatile uint8_t low, high;
//test variables
volatile int16_t e = 0;
volatile int16_t v = 0;
volatile int32_t I = 0;
volatile int16_t I = 0;
volatile int16_t u = 0;
volatile int16_t K = 20;//5;//375; // 7 frac bits
volatile int16_t Ke = 8;//2;//6; //7 frac bits, K*h/Ti
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
......@@ -44,7 +44,7 @@ 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 = 200;
volatile int16_t skipSamples = 1;
volatile int16_t countSamples = 0;
volatile int16_t jj=0;
volatile int8_t stop = 0;
......@@ -58,12 +58,6 @@ void setLED(uint8_t on)
else PORTB |= 0x80; //Turn off
}
/* Routine used to set pin PD7 */
void setPA4(uint8_t on)
{
if (on == 0) PORTA &= 0xef; //Turn off
else PORTA |= 0x10; //Turn on
}
/* Routine used to transmit bytes on the serial port */
static void putchar(unsigned char ch)
......@@ -128,67 +122,89 @@ static inline void sendData() {
/* Interrupt when AD-conversion completes */
SIGNAL(SIG_ADC)
{
setPA4(1);
PORTA |= 0x10; //Turn on calculation timer
// Read input
low = inp(ADCL);
high = inp(ADCH);
y = ((high<<8) | low) - 512; //y 9 frac bits
// Scale incoming current measurement
//y = y*3; //!!!!!!!!!!!!!!!!!!!!!!!
// Scale incoming current measurement
y += 70;
//control, since negative measurements
e = ref+y; //e 9 frac bits
v = (int16_t)(((K*e+(1<<6)) >> 7)+(int16_t)((I+(1<<6))>>7));
//saturation/rounding to 8 bit
if (v > 511)
u = 511;
else if (v < -512)
u = -512;
else
u = v;
I = I +((int32_t)Ke)*((int32_t)e) + ((int32_t)Ksat)*((int32_t)(u-v)); //16 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
u = (u)>>2; //7 frac bits to pwm
// 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;
}
*/
// upplsning p styrsignal [-104,103] fr 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);
OCR1BL = (unsigned char) (128-(-u));
//PORTC = 0x80+(PORTC & 0x7F);
PORTC |= 0x80;
OCR1BL = (unsigned char) (104-(-u));
} else {
PORTC = (PORTC & 0x7F);
OCR1BL = (unsigned char) (127-u);
OCR1BL = (unsigned char) (105-u);
}
// For logging
countSamples++;
if (countSamples == skipSamples) {
ctrl_log[jj] = y;
I_log[jj] = I;
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;
}
if ((jj == (log_len-1)) & !stop) {
outp(0x7f,OCR1BL);
stop = 1;
sendData();
}
setPA4(0);
PORTA &= 0xef; //Turn off calculation timer
}
......@@ -247,13 +263,25 @@ int main()
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(0x7f,OCR1AL);
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(0x7f,OCR1BL); // to not start motor-rotation before control
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 */
......
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