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

Final package but with some strange factors

parent 7b394b34
...@@ -8,14 +8,21 @@ ...@@ -8,14 +8,21 @@
// control variables // control variables
volatile int16_t ref=0; // reference value (from velocity controller) volatile int16_t ref=0; // reference value (from velocity controller)
volatile int16_t y; // measurement, 9 frac bits 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 e = 0; // control error, 9 frac bits
volatile int16_t v = 0; // temporary ctrl signal, 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 I = 0; // integral part of ctrl, 13 frac bits
volatile int16_t u = 0; // ctrl signal = pwm high time (8 bits) volatile int16_t u = 0; // ctrl signal = pwm high time (8 bits)
volatile int16_t K = 74; // 7 frac bits volatile int16_t K = 74; // 7 frac bits
volatile int16_t Ke = 26; // 7 frac bits, K*h/Ti volatile int16_t Ke = 26; // 7 frac bits, K*h/Ti
volatile int16_t Ksat = 44; // 7 frac bits, h/Tr 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 // twi variable
volatile int16_t status; volatile int16_t status;
...@@ -47,7 +54,7 @@ static void putchar(unsigned char ch) ...@@ -47,7 +54,7 @@ static void putchar(unsigned char ch)
*/ */
/* Send logged data over Serial connection */ /* Send logged data over Serial connection */
/* /*
static inline void sendData() { static inline void sendData() {
int16_t ii = 0; int16_t ii = 0;
while (ii < log_len) { while (ii < log_len) {
...@@ -64,65 +71,72 @@ static inline void sendData() { ...@@ -64,65 +71,72 @@ static inline void sendData() {
putchar((unsigned char) ((I_log[ii]&0x0000ff00)>>8)); putchar((unsigned char) ((I_log[ii]&0x0000ff00)>>8));
putchar((unsigned char) (I_log[ii]&0x000000ff)); putchar((unsigned char) (I_log[ii]&0x000000ff));
ii++; ii++;
} }
} }
*/ */
/* Timer 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */ /* Timer 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */
SIGNAL(SIG_OUTPUT_COMPARE2) { SIGNAL(SIG_OUTPUT_COMPARE2) {
PORTB |= 0x40;
// Start AD conversion // Start AD conversion
ADCSRA |= BV(ADSC); ADCSRA |= BV(ADSC);
// Read previous AD-conversion result // Read previous AD-conversion result
low = inp(ADCL); low = inp(ADCL);
high = inp(ADCH); high = inp(ADCH);
y = ((high<<8) | low) - 512; //y 9 frac bits y = ((int16_t)((high<<8) | low)) - 512; // y 9 frac bits
//control error, (+) since negative measurements y = ((y*3)>>1);
e = ref+y; //e 9 frac bits
// control error
e = ref-y; // e 9 frac bits
// temporary ctrl-signal // temporary ctrl-signal
v = (int16_t)(((K*e+64)>>7)+(I>>4)); v = (((K*e+64)>>7)+((I+8)>>4));
//saturation and update integral part of ctrl with antiwindup // variable that decides if I-part should be updated
if (v > 511) { intCond = 1;
I = I + ((((Ke*e) + (Ksat)*(511-v))+(1<<2))>>3);
} else if (v < -512) { // saturation and update integral part of ctrl with antiwindup
I = I + ((((Ke*e) + (Ksat)*(-512-v))+(1<<2))>>3); 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 { } else {
I = I + ((Ke*e+(1<<2))>>3); vSat = v;
} }
if (intCond)
I = I + (((Ke*e)+(1<<2))>>3);
// ctrl signal, 7 bits + direction // ctrl signal, 7 bits + direction
u = (v+2)>>2; //7 frac bits to pwm u = ((vSat+2)>>2); //7 frac bits to pwm
// saturation of ctrl-signal
if (u > 127) {
u = 127;
} else if (u < -128) {
u = -128;
}
// set pwm switching time // set pwm switching time
if (u < 0) { if (u < 0) {
PORTC |= 0x80; // set direction of motion 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 { } else {
PORTC = (PORTC & 0x7F); // set direction of motion PORTC &= 0x7f; // set direction of motion
OCR1BL = (unsigned char) (127-u); // set length of pwm-high OCR1BL = ((unsigned char) (u)); // set length of pwm-high
} }
// TWI-communication, recieve reference from velocity controller // TWI-communication, recieve reference from velocity controller
if ((BV(TWINT)&inp(TWCR))) { if ((BV(TWINT)&inp(TWCR))) {
status = (inp(TWSR)&0xf8); status = (inp(TWSR)&0xf8);
// status 0x80 means data recieved // status 0x80 means data recieved
if (status == 0x80) { if (status == 0x80) {
ref = (int16_t)((int8_t)(inp(TWDR))); // read 8 bit reference 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 = ref*8; // shift up 2 steps for 10 bits reference in loop
} }
else { else {
} }
...@@ -147,7 +161,8 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { ...@@ -147,7 +161,8 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
} }
} }
*/ */
PORTB &= ~0x40;
} }
...@@ -160,11 +175,11 @@ int main() ...@@ -160,11 +175,11 @@ int main()
outp(0x08,PORTC); // pull up on overtemperature signals outp(0x08,PORTC); // pull up on overtemperature signals
outp(0xa0,DDRC); // output on direction and brake outp(0xa0,DDRC); // output on direction and brake
outp(0x10,DDRD); // output on pwm-signal outp(0x10,DDRD); // output on pwm-signal
outp(0x40,DDRB); // temp pwm output
/* Timer section */ /* Timer section */
// Timer 1, fast PWM no prescaling (non-inverting mode (start low, switch to high))
// Timer 1, fast PWM no prescaling (inverting mode (start low, switch to high)) outp(BV(COM1B1)|BV(WGM11)|BV(WGM10),TCCR1A);
outp(BV(COM1A1)|BV(COM1B1)|BV(COM1A0)|BV(COM1B0)|BV(WGM11)|BV(WGM10),TCCR1A);
outp(BV(CS10)|BV(WGM13)|BV(WGM12),TCCR1B); outp(BV(CS10)|BV(WGM13)|BV(WGM12),TCCR1B);
// Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> 115.2 kHz) // Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> 115.2 kHz)
...@@ -175,6 +190,7 @@ int main() ...@@ -175,6 +190,7 @@ int main()
outp(0x00,OCR1BH); outp(0x00,OCR1BH);
outp(0x7f,OCR1BL); // to not start motor-rotation before control outp(0x7f,OCR1BL); // to not start motor-rotation before control
/* Timer 2, 4 times pwm-period, for control sampling, prescaler 8, 28.8 kHz */ /* Timer 2, 4 times pwm-period, for control sampling, prescaler 8, 28.8 kHz */
outp(BV(WGM21)|BV(CS21),TCCR2); outp(BV(WGM21)|BV(CS21),TCCR2);
......
...@@ -95,11 +95,11 @@ static uint8_t sendConfigPacket(uint8_t position) ...@@ -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 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 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 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 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 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; 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 ...@@ -130,7 +130,7 @@ static uint8_t sendNextPacket() // returns 1 if a packet was available
} }
else if (toPoll & POLL_PEND_ANGLE) { else if (toPoll & POLL_PEND_ANGLE) {
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) { else if (toPoll & POLL_CURRENT_REFERENCE) {
toPoll &= ~POLL_CURRENT_REFERENCE; toPoll &= ~POLL_CURRENT_REFERENCE;
......
...@@ -4,55 +4,65 @@ ...@@ -4,55 +4,65 @@
#include <avr/interrupt.h> #include <avr/interrupt.h>
#include <inttypes.h> #include <inttypes.h>
#include "pccom.h" #include "pccom.h"
#include "vel_control.h" #include "vel_control.h"
// reference variables // reference variables
volatile int32_t ref = 0; // 11 frac bits volatile int32_t ref = 0; // 11 frac bits
volatile int32_t refCtrl = 0; volatile int32_t refCtrl = 0; // ref used in ctrl-loop (=ref sent from simulink)
// velocity control variables // velocity control variables
volatile int32_t u = 0; // 11 frac bits volatile int32_t u = 0; // 11 frac bits
volatile int32_t uSend = 0; volatile int32_t uSend = 0; // ctrl sent to simulink
volatile int32_t v = 0; // 11 frac bits volatile int32_t v = 0; // 11 frac bits
volatile int8_t brake = 0; volatile int32_t vSat = 0;
volatile int32_t I = 0; // 11 frac bits volatile int8_t brake = 0; // brake variable if pos-sample missed
volatile int32_t e = 0; // 11 frac bits volatile int32_t I = 0; // 11 frac bits
volatile int32_t K = 1200; // 6 frac bits volatile int32_t e = 0; // 11 frac bits
volatile int32_t Ke = 45; // 6 frac bits volatile int8_t intCond = 0;
volatile int32_t Ksat = 3; // 6 frac bits
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 // 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 ENCODERY (PIND&0x04) //Positional encoder pins
#define ENCODERX ((PINB&0x02)<<1) //Positional encoder pins #define ENCODERX ((PINB&0x02)<<1) //Positional encoder pins (shift one step for faster comparison with Y)
volatile int16_t pos = 0; // position variables
volatile int16_t posTemp = 0; volatile int16_t pos = 0; // position in tics
volatile int16_t posCtrl = 0; volatile int16_t posTemp = 0; // position sent to simulink
volatile int16_t oldPos = 0; volatile int16_t posCtrl = 0; // position used in ctrl-loop
volatile int8_t newX; volatile int16_t oldPos = 0; // previous pos used for velocity estimation
volatile int8_t newY; volatile int8_t newX; // encoder signal
volatile int8_t oldX; volatile int8_t newY; // encoder signal
volatile int8_t oldY; volatile int8_t oldX; // previous encoder signal
volatile int8_t sum = 0; volatile int8_t oldY; // previous encoder signal
// velocity estimation parameters // velocity estimation parameters
volatile int32_t velEst = 0; // 5 frac bits volatile int32_t velEst = 0; // vel-estimate, 5 frac bits
//volatile int32_t velEstTemp = 0; volatile int32_t velEstSend = 0; // vel-estimate sent to simulink
volatile int32_t velEstSend = 0; int16_t a = 116; // 7 frac bits (parameter in velocity estimate)
int16_t a = 116; //7 frac bits int16_t b = 152; // 5 frac bits (parameter in velocity estimate)
int16_t b = 152; // 5 frac bits
// adc measurement variables // adc measurement variables
volatile int16_t low; volatile int16_t low; // temporary variable for ad-reading
volatile int16_t high; 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() { int32_t getPosition() {
cli(); cli();
posTemp = pos; posTemp = pos;
...@@ -61,38 +71,31 @@ int32_t getPosition() { ...@@ -61,38 +71,31 @@ int32_t getPosition() {
} }
// return velocity (in mm/s) /* return velocity (in mm/s) */
int32_t getVelocity() { int32_t getVelocity() {
return velEstSend; return velEstSend;
} }
// return last angle measurement
/* return last angle measurement */
int16_t getAngle() { int16_t getAngle() {
low = inp(ADCL); low = inp(ADCL);
high = inp(ADCH); 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() { int32_t getCurrentRef() {
return uSend; return uSend;
} }
/* Set new reference value */
/* 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
void setRef(int32_t newRef) { void setRef(int32_t newRef) {
ref = newRef; ref = newRef;
} }
/* Routine used to initialize the positional encoders */ /* Routine used to initialize the positional encoders */
void initPos() void initPos()
{ {
...@@ -100,24 +103,16 @@ void initPos() ...@@ -100,24 +103,16 @@ void initPos()
oldY = ENCODERY; oldY = ENCODERY;
} }
/* Routine used to reset the cart position */ /* Timer 2, Encoder counter, 73 kHz updates position variable */
void resetPos()
{
}
/* Timer 2, Encoder counter, 73 kHz */
SIGNAL(SIG_OUTPUT_COMPARE2) { SIGNAL(SIG_OUTPUT_COMPARE2) {
// Update position from encoder // Update position from encoder
newX = ENCODERX; newX = ENCODERX;
newY = ENCODERY; newY = ENCODERY;
if((newX != oldX) || (newY != oldY)) { //Check if any value changed if((newX != oldX) || (newY != oldY)) { //Check if any value changed
/* /*
sum = (oldX<<2)+oldY+newX+(newY>>2); //Find state sum = (oldX<<2)+oldY+newX+(newY>>2);
if (sum == 2 || sum == 4 || sum == 11 || sum == 13) { //Predetermined values determine direction if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
pos = pos+1; pos = pos+1;
} else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) { } else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) {
pos = pos-1; pos = pos-1;
...@@ -125,6 +120,7 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { ...@@ -125,6 +120,7 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
brake = 1; // emergency brake brake = 1; // emergency brake
} }
*/ */
// Works like if-clauses above, but faster!
if ((oldX == newY) && (oldY != newX)) { if ((oldX == newY) && (oldY != newX)) {
pos = pos+1; pos = pos+1;
} else if ((oldX != newY) && (oldY == newX)) { } else if ((oldX != newY) && (oldY == newX)) {
...@@ -136,18 +132,17 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { ...@@ -136,18 +132,17 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
oldY = newY; oldY = newY;
} }
} }
/* Timer 1, serial communication */ /* Timer 0, serial communication with simulink */
SIGNAL(SIG_OUTPUT_COMPARE0) { 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 // Poll UART receiver
PORTC |= 0x10; // to clock calulation time
uint8_t status = UCSRA; uint8_t status = UCSRA;
if (status & (1<<RXC)) { if (status & (1<<RXC)) {
char ch = UDR; char ch = UDR;
...@@ -164,11 +159,9 @@ SIGNAL(SIG_OUTPUT_COMPARE0) { ...@@ -164,11 +159,9 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
if (toSend >= 0) UDR = (char)toSend; if (toSend >= 0) UDR = (char)toSend;
} }
PORTC &= ~0x10; TIFR = (1<<OCF0); // skip pending interrupts from serial comm, (but not from ctrl)
TIFR = (1<<OCF0);
//TIFR = (1<<OCF1A);
TIMSK |= (BV(OCIE0)|BV(OCIE1A)); TIMSK |= (BV(OCIE0)|BV(OCIE1A)); // reenable communication and ctrl-interrupts
} }
...@@ -177,88 +170,77 @@ SIGNAL(SIG_OUTPUT_COMPARE0) { ...@@ -177,88 +170,77 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
/* Timer 0, control loop , 1 kHz */ /* Timer 0, control loop , 1 kHz */
SIGNAL(SIG_OUTPUT_COMPARE1A) { 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 // velocity estimation in mm/s
// linear velocity estimator
// velocity estimate in mm/s
velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst
oldPos = posCtrl; oldPos = posCtrl;
// emergency stop
ref = ref*(1-brake);
// store velEst and ref to be sent/used here
cli(); cli();
velEstSend = velEst; velEstSend = velEst;
refCtrl = ref; refCtrl = ref;
// ref = ref*(1-brake); // emergency stop
sei(); sei();
// control error // control error
e = refCtrl-((velEst+16)>>5); // mm/s e = refCtrl-((velEst+16)>>5); // mm/s
v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
//saturation and update integral part of ctrl // temporary ctrl-signal
if (v > 2047) { v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
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
// friction compensation // friction compensation
if (refCtrl > 0) { if (refCtrl > 0) {
u = u+10; v = v+fr_comp;
} else if (ref < 0) { } else if (refCtrl < 0) {
u = u-10; v = v-fr_comp;
} }
// Saturation