diff --git a/linear_pendulum_2009/avr/current_control_final.c b/linear_pendulum_2009/avr/current_control_final.c index 2c0f1bd87476cb98a087c33ed43e24d18763ad67..0da8fa188fec5c285e762388c160a87d558612fd 100644 --- a/linear_pendulum_2009/avr/current_control_final.c +++ b/linear_pendulum_2009/avr/current_control_final.c @@ -8,14 +8,21 @@ // control variables volatile int16_t ref=0; // reference value (from velocity controller) volatile int16_t y; // measurement, 9 frac bits -volatile uint8_t low, high; // when reading AD-conversion +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; 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 = 74; // 7 frac bits volatile int16_t Ke = 26; // 7 frac bits, K*h/Ti volatile int16_t Ksat = 44; // 7 frac bits, h/Tr +volatile int8_t intCond = 0; + + +#define V_MAX 508 +#define V_MIN -508 + // twi variable volatile int16_t status; @@ -47,7 +54,7 @@ static void putchar(unsigned char ch) */ /* Send logged data over Serial connection */ - /* +/* static inline void sendData() { int16_t ii = 0; while (ii < log_len) { @@ -64,65 +71,72 @@ static inline void sendData() { 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) { + + PORTB |= 0x40; // Start AD conversion ADCSRA |= BV(ADSC); // Read previous AD-conversion result low = inp(ADCL); high = inp(ADCH); - y = ((high<<8) | low) - 512; //y 9 frac bits - - //control error, (+) since negative measurements - e = ref+y; //e 9 frac bits + y = ((int16_t)((high<<8) | low)) - 512; // y 9 frac bits + + y = ((y*3)>>1); + + // control error + e = ref-y; // e 9 frac bits // temporary ctrl-signal - v = (int16_t)(((K*e+64)>>7)+(I>>4)); - - //saturation and update integral part of ctrl with antiwindup - if (v > 511) { - I = I + ((((Ke*e) + (Ksat)*(511-v))+(1<<2))>>3); - } else if (v < -512) { - I = I + ((((Ke*e) + (Ksat)*(-512-v))+(1<<2))>>3); + 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 { - I = I + ((Ke*e+(1<<2))>>3); + vSat = v; } - + + if (intCond) + I = I + (((Ke*e)+(1<<2))>>3); + // ctrl signal, 7 bits + direction - u = (v+2)>>2; //7 frac bits to pwm - - // saturation of ctrl-signal - if (u > 127) { - u = 127; - } else if (u < -128) { - u = -128; - } + 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) (128-(-u)); // set length of pwm-high + OCR1BL = ((unsigned char) (-u)); // set length of pwm-high } else { - PORTC = (PORTC & 0x7F); // set direction of motion - OCR1BL = (unsigned char) (127-u); // set length of pwm-high + 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 for reference in loop + ref = (int16_t)((int8_t)inp(TWDR)); // read 8 bit reference + ref = ref*8; // shift up 2 steps for 10 bits reference in loop } else { } @@ -147,7 +161,8 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { } } */ - + + PORTB &= ~0x40; } @@ -160,11 +175,11 @@ int main() 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 (inverting mode (start low, switch to high)) - outp(BV(COM1A1)|BV(COM1B1)|BV(COM1A0)|BV(COM1B0)|BV(WGM11)|BV(WGM10),TCCR1A); + // Timer 1, fast PWM no prescaling (non-inverting mode (start low, switch to high)) + 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) @@ -175,6 +190,7 @@ int main() 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); diff --git a/linear_pendulum_2009/avr/pccom.c b/linear_pendulum_2009/avr/pccom.c index 718730220aa6a4e5860edd82c4d24665ede8204d..7d074ad8c4617dcc31cf61cbf30011b6cfa6b4e5 100644 --- a/linear_pendulum_2009/avr/pccom.c +++ b/linear_pendulum_2009/avr/pccom.c @@ -95,11 +95,11 @@ static uint8_t sendConfigPacket(uint8_t position) case 4: CONF_ANALOG_IN(1, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break; case 5: CONF_ANALOG_IN(1, CONF_MAX(CONF_POSITIVE_VOLT(1))); break; - case 6: CONF_ANALOG_IN(2, CONF_RESOLUTION(10)); break; // Pend angle measurement + case 6: CONF_ANALOG_IN(2, CONF_RESOLUTION(11)); break; // Pend angle measurement case 7: CONF_ANALOG_IN(2, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break; case 8: CONF_ANALOG_IN(2, CONF_MAX(CONF_POSITIVE_VOLT(1))); break; - case 9: CONF_ANALOG_IN(3, CONF_RESOLUTION(8)); break; // Pend angle measurement + case 9: CONF_ANALOG_IN(3, CONF_RESOLUTION(8)); break; // Current reference case 10: CONF_ANALOG_IN(3, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break; case 11: CONF_ANALOG_IN(3, CONF_MAX(CONF_POSITIVE_VOLT(1))); break; @@ -130,7 +130,7 @@ static uint8_t sendNextPacket() // returns 1 if a packet was available } else if (toPoll & POLL_PEND_ANGLE) { toPoll &= ~POLL_PEND_ANGLE; - serialio_putchannel(2, getAngle()+(1L<<9)); + serialio_putchannel(2, getAngle()+(1L<<10)); } else if (toPoll & POLL_CURRENT_REFERENCE) { toPoll &= ~POLL_CURRENT_REFERENCE; diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c index 204a3a8ff8717e2a1a9e90c5bc3c799f8b36a348..0ba2aeffc61fb84efedfda92dcc56d0e427cd0f4 100644 --- a/linear_pendulum_2009/avr/vel_control.c +++ b/linear_pendulum_2009/avr/vel_control.c @@ -4,55 +4,65 @@ #include <avr/interrupt.h> #include <inttypes.h> - #include "pccom.h" #include "vel_control.h" // reference variables -volatile int32_t ref = 0; // 11 frac bits -volatile int32_t refCtrl = 0; +volatile int32_t ref = 0; // 11 frac bits +volatile int32_t refCtrl = 0; // ref used in ctrl-loop (=ref sent from simulink) // velocity control variables -volatile int32_t u = 0; // 11 frac bits -volatile int32_t uSend = 0; -volatile int32_t v = 0; // 11 frac bits -volatile int8_t brake = 0; -volatile int32_t I = 0; // 11 frac bits -volatile int32_t e = 0; // 11 frac bits -volatile int32_t K = 1200; // 6 frac bits -volatile int32_t Ke = 45; // 6 frac bits -volatile int32_t Ksat = 3; // 6 frac bits +volatile int32_t u = 0; // 11 frac bits +volatile int32_t uSend = 0; // ctrl sent to simulink +volatile int32_t v = 0; // 11 frac bits +volatile int32_t vSat = 0; +volatile int8_t brake = 0; // brake variable if pos-sample missed +volatile int32_t I = 0; // 11 frac bits +volatile int32_t e = 0; // 11 frac bits +volatile int8_t intCond = 0; + + +volatile int32_t K = 1200; // 6 frac bits, prop constant +volatile int32_t Ke = 45; // 6 frac bits, integral constant +volatile int32_t Ksat = 1000; // 6 frac bits, antiwindup constant + +//volatile int32_t K = 600; // 6 frac bits, prop constant +//volatile int32_t Ke = 15; // 6 frac bits, integral constant +//volatile int32_t Ksat = 3; // 6 frac bits, antiwindup constant +volatile int8_t fr_comp = (10<<3); +#define V_MAX (90<<5) +#define V_MIN (-90<<5) + // encoder variables -//#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins -//#define ENCODERX (PINB&(uint8_t)(1<<1)) //Positional encoder pins #define ENCODERY (PIND&0x04) //Positional encoder pins -#define ENCODERX ((PINB&0x02)<<1) //Positional encoder pins - -volatile int16_t pos = 0; -volatile int16_t posTemp = 0; -volatile int16_t posCtrl = 0; -volatile int16_t oldPos = 0; -volatile int8_t newX; -volatile int8_t newY; -volatile int8_t oldX; -volatile int8_t oldY; -volatile int8_t sum = 0; +#define ENCODERX ((PINB&0x02)<<1) //Positional encoder pins (shift one step for faster comparison with Y) + +// position variables +volatile int16_t pos = 0; // position in tics +volatile int16_t posTemp = 0; // position sent to simulink +volatile int16_t posCtrl = 0; // position used in ctrl-loop +volatile int16_t oldPos = 0; // previous pos used for velocity estimation +volatile int8_t newX; // encoder signal +volatile int8_t newY; // encoder signal +volatile int8_t oldX; // previous encoder signal +volatile int8_t oldY; // previous encoder signal // velocity estimation parameters -volatile int32_t velEst = 0; // 5 frac bits -//volatile int32_t velEstTemp = 0; -volatile int32_t velEstSend = 0; -int16_t a = 116; //7 frac bits -int16_t b = 152; // 5 frac bits +volatile int32_t velEst = 0; // vel-estimate, 5 frac bits +volatile int32_t velEstSend = 0; // vel-estimate sent to simulink +int16_t a = 116; // 7 frac bits (parameter in velocity estimate) +int16_t b = 152; // 5 frac bits (parameter in velocity estimate) // adc measurement variables -volatile int16_t low; -volatile int16_t high; +volatile int16_t low; // temporary variable for ad-reading +volatile int16_t high; // temporary variable for ad-reading +volatile int16_t angleOffset = 0; + -// return position (in tics) +/* return position (in tics) */ int32_t getPosition() { cli(); posTemp = pos; @@ -61,38 +71,31 @@ int32_t getPosition() { } -// return velocity (in mm/s) +/* return velocity (in mm/s) */ int32_t getVelocity() { return velEstSend; } -// return last angle measurement + +/* return last angle measurement */ int16_t getAngle() { low = inp(ADCL); high = inp(ADCH); - return ((int16_t) ((high<<8) | low) - 512); + return (((int16_t) ((high<<8) | low) - 512)-angleOffset); } -// return current reference + +/* return current-reference */ int32_t getCurrentRef() { return uSend; } - -/* Routine used to set the red LED */ -void setLED(uint8_t on) -{ - if (on) PORTB &= ~0x80; //Turn on - else PORTB |= 0x80; //Turn off -} - -// Set new reference value +/* Set new reference value */ void setRef(int32_t newRef) { ref = newRef; } - /* Routine used to initialize the positional encoders */ void initPos() { @@ -100,24 +103,16 @@ void initPos() oldY = ENCODERY; } -/* Routine used to reset the cart position */ -void resetPos() -{ - -} - -/* Timer 2, Encoder counter, 73 kHz */ +/* Timer 2, Encoder counter, 73 kHz updates position variable */ SIGNAL(SIG_OUTPUT_COMPARE2) { - - // Update position from encoder newX = ENCODERX; newY = ENCODERY; if((newX != oldX) || (newY != oldY)) { //Check if any value changed /* - sum = (oldX<<2)+oldY+newX+(newY>>2); //Find state - if (sum == 2 || sum == 4 || sum == 11 || sum == 13) { //Predetermined values determine direction + sum = (oldX<<2)+oldY+newX+(newY>>2); + if (sum == 2 || sum == 4 || sum == 11 || sum == 13) { pos = pos+1; } else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) { pos = pos-1; @@ -125,6 +120,7 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { brake = 1; // emergency brake } */ + // Works like if-clauses above, but faster! if ((oldX == newY) && (oldY != newX)) { pos = pos+1; } else if ((oldX != newY) && (oldY == newX)) { @@ -136,18 +132,17 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { oldY = newY; } - - } -/* Timer 1, serial communication */ +/* Timer 0, serial communication with simulink */ SIGNAL(SIG_OUTPUT_COMPARE0) { - TIMSK &= ~(BV(OCIE0)|BV(OCIE1A)); + TIMSK &= ~(BV(OCIE0)|BV(OCIE1A)); // Disable communication and ctrl-interrupts + + sei(); // enable interrupts from encoder-counter - sei(); // enable interrupts from timer 0 and timer 2 - PORTC |= 0x10; // to clock calulation time + // Poll UART receiver uint8_t status = UCSRA; if (status & (1<<RXC)) { char ch = UDR; @@ -164,11 +159,9 @@ SIGNAL(SIG_OUTPUT_COMPARE0) { if (toSend >= 0) UDR = (char)toSend; } - PORTC &= ~0x10; - TIFR = (1<<OCF0); - //TIFR = (1<<OCF1A); + TIFR = (1<<OCF0); // skip pending interrupts from serial comm, (but not from ctrl) - TIMSK |= (BV(OCIE0)|BV(OCIE1A)); + TIMSK |= (BV(OCIE0)|BV(OCIE1A)); // reenable communication and ctrl-interrupts } @@ -177,88 +170,77 @@ SIGNAL(SIG_OUTPUT_COMPARE0) { /* Timer 0, control loop , 1 kHz */ SIGNAL(SIG_OUTPUT_COMPARE1A) { - TIMSK &= ~(BV(OCIE1A)); + posCtrl = pos; // store pos to use in this loop - posCtrl = pos; // to aviod overwrite while reading + sei(); // to enable interupts from encoder counter and communication - sei(); // to enable interupts from timer2 - - - // linear velocity estimator - // velocity estimate in mm/s + // velocity estimation in mm/s velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst oldPos = posCtrl; - - // emergency stop - ref = ref*(1-brake); + // store velEst and ref to be sent/used here cli(); velEstSend = velEst; refCtrl = ref; + // ref = ref*(1-brake); // emergency stop sei(); + // control error e = refCtrl-((velEst+16)>>5); // mm/s - - v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4)); - //saturation and update integral part of ctrl - if (v > 2047) { - I = I + ((((Ke*e) + (Ksat)*(2047-v))+(1<<1))>>2); - } else if (v < -2048) { - I = I + ((((Ke*e) + (Ksat)*(-2048-v))+(1<<1))>>2); - } else { - I = I + ((Ke*e+(1<<1))>>2); - } - - u = (v+8)>>4; //8 frac bits to current loop + // temporary ctrl-signal + v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4)); // friction compensation if (refCtrl > 0) { - u = u+10; - } else if (ref < 0) { - u = u-10; + v = v+fr_comp; + } else if (refCtrl < 0) { + v = v-fr_comp; } - // Saturation - if (u > 127) { - u = 127; - } else if (u < -128) { - u = -128; + // variable that decides if I-part should be updated + intCond = 1; + + // saturation of v + 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; } + + // update integral part + // I = I + ((((Ke*e) + (Ksat)*(vSat-v))+(1<<1))>>2); + + if (intCond) + I = I + (((Ke*e)+(1<<1))>>2); + + // scale ctrl-signal to send over twi + u = (vSat+16)>>5; // u=90 gives current = 6.3 A, vSat makes u saturate at 90 + // u that is sent to simulink cli(); uSend = u; sei(); - - // reset position, velocity estimate and integral part of ctrl if reset button pushed - - if (!(PIND & 0x40)) { - cli(); - pos = 0; // reset position - sei(); - oldPos = 0; - velEst = 0; // reset velocity estimate - I = 0; // reset integral part of controller - u = 0; // reset ctrl signal - } - - // TWI-communication + // TWI-communication to set current reference on the other atmel // send start command outp(BV(TWINT)|BV(TWEN)|BV(TWSTA),TWCR); while (!(TWCR&BV(TWINT))) {} - - - + // Contact slave outp(0x02,TWDR); // slave is 0x02 (sla+w) outp(BV(TWINT)|BV(TWEN),TWCR); while (!(TWCR&BV(TWINT))) {} - - - // Send reference byte - outp((int8_t)(u&0x000000ff),TWDR); // send 8 bits reference + + // Send reference byte to slave + outp((int8_t)u,TWDR); // send 8 bits reference outp(BV(TWINT)|BV(TWEN),TWCR); while (!(TWCR&BV(TWINT))) {} @@ -266,13 +248,6 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) { // stop transmission outp(BV(TWINT)|BV(TWEN)|BV(TWSTO),TWCR); - - - //TIFR = (1<<OCF0); - TIFR = (1<<OCF1A); - - TIMSK |= (BV(OCIE1A)); - } @@ -281,39 +256,23 @@ int main() cli(); //Port directions - outp(0x80,DDRB); // Led output + //outp(0x80,DDRB); // Led output outp(0x10,DDRC); // timer calculation port + PORTD = 0x40; // pull up on reset switch + + /* Timer section */ // Enable timer0, timer1, timer2 compare match interrupts outp(BV(OCIE0)|BV(OCIE1A)|BV(OCIE2),TIMSK); - /* Timer 2, 73 kHz Prescaler 1 */ + /* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */ outp(BV(WGM21)|BV(CS20),TCCR2); outp(200,OCR2); /* Reset timer 2 */ outp(0,TCNT2); - // --------old----------------------------------------------- - /* Timer 1, Prescaler 1, 73/8 kHz*/ - - //outp(BV(WGM12)|BV(CS10),TCCR1B); - //outp(0x06,OCR1AH); - //outp(0x40,OCR1AL); - //outp(0,TCNT1H); - //outp(0,TCNT1L); - - - /* Timer 0, 1 kHz Prescaler 64 */ - - //outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0); - //outp(230,OCR0); - - /* Reset timer 0 */ - //outp(0,TCNT0); - - // --------new----------------------------------------------- - /* Timer 1, 1 kHz */ + /* Timer 1, 1 kHz , prescaler 1, ctrl-loop */ outp(BV(WGM12)|BV(CS10),TCCR1B); outp(0x39,OCR1AH); outp(0x7f,OCR1AL); @@ -321,32 +280,30 @@ int main() outp(0,TCNT1L); - /* Timer 0, 10 kHz, Prescaler 8 */ + /* Timer 0, 10 kHz, Prescaler 8, serial communication */ outp(BV(WGM01)|BV(CS01),TCCR0); - //outp(200,OCR0); outp(184-1,OCR0); // 10 kHz - /* Reset timer 0 */ outp(0,TCNT0); - //---------------------------------------------------------- - // syncronization (timer1 interrupt 34 micros before timer0 interrupt) + + // syncronization (ctrl interrupt 34 micros before communication interrupt) TCNT1 = TCNT0*8+500; - //Serial communication + //Serial communication initialization 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 */ + + /* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */ outp(BV(REFS0)|BV(MUX0)|BV(MUX1),ADMUX); - // Enable ADC on mux3, start first conversion, prescaler 128, free running mode + // Enable ADC on adc3, start first conversion, prescaler 128, free running mode outp(BV(ADEN)|BV(ADSC)|BV(ADATE)|BV(ADPS2)|BV(ADPS1)|BV(ADPS0),ADCSRA); @@ -357,11 +314,27 @@ int main() // initialize position measurements initPos(); + // initialize pc-communication pccom_init(); //Enable interrupts sei(); // loop - while (1) {} + while (1) { + // reset position, velocity estimate and integral part of ctrl if reset button pushed + if (!(PIND & 0x40)) { + cli(); + low = inp(ADCL); + high = inp(ADCH); + pos = 0; // reset position + angleOffset = ((int16_t) ((high<<8) | low) - 512); + //angleOffset = -150; + sei(); + oldPos = 0; + velEst = 0; // reset velocity estimate + I = 0; // reset integral part of controller + u = 0; // reset ctrl signal + } + } }