Commit 7b394b34 authored by Pontus Giselsson's avatar Pontus Giselsson
Browse files

Added mutex for shared variables

parent 36135468
...@@ -7,19 +7,22 @@ ...@@ -7,19 +7,22 @@
// 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 unsigned char lbyte,hbyte;
volatile int16_t y; // measurement, 9 frac bits volatile int16_t y; // measurement, 9 frac bits
volatile uint8_t low, high; volatile uint8_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 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 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
// twi variable
volatile int16_t status;
// logging variables // logging variables
/*
#define log_len 100 #define log_len 100
volatile int16_t ctrl_log[log_len]; volatile int16_t ctrl_log[log_len];
volatile int16_t error_log[log_len]; volatile int16_t error_log[log_len];
...@@ -28,12 +31,23 @@ volatile int16_t skipSamples = 1000; ...@@ -28,12 +31,23 @@ volatile int16_t skipSamples = 1000;
volatile int16_t countSamples = 0; volatile int16_t countSamples = 0;
volatile int16_t jj=0; volatile int16_t jj=0;
volatile int8_t stop = 0; volatile int8_t stop = 0;
*/
// twi variables
volatile int16_t status;
/* 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 */ /* 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) {
...@@ -54,16 +68,16 @@ static inline void sendData() { ...@@ -54,16 +68,16 @@ static inline void sendData() {
ii++; ii++;
} }
} }
*/
/* Timer 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */
/* Timer 2 compare match interupt */
SIGNAL(SIG_OUTPUT_COMPARE2) { SIGNAL(SIG_OUTPUT_COMPARE2) {
// Start AD conversion // Start AD conversion
ADCSRA |= BV(ADSC); ADCSRA |= BV(ADSC);
// Read input // 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 = ((high<<8) | low) - 512; //y 9 frac bits
...@@ -71,15 +85,16 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { ...@@ -71,15 +85,16 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
//control error, (+) since negative measurements //control error, (+) since negative measurements
e = ref+y; //e 9 frac bits e = ref+y; //e 9 frac bits
// temporary ctrl-signal
v = (int16_t)(((K*e+64)>>7)+(I>>4)); v = (int16_t)(((K*e+64)>>7)+(I>>4));
//saturation and update integral part of ctrl //saturation and update integral part of ctrl with antiwindup
if (v > 511) { if (v > 511) {
I = I + (((Ke*e) + (Ksat)*(511-v))>>3); I = I + ((((Ke*e) + (Ksat)*(511-v))+(1<<2))>>3);
} else if (v < -512) { } else if (v < -512) {
I = I + (((Ke*e) + (Ksat)*(-512-v))>>3); I = I + ((((Ke*e) + (Ksat)*(-512-v))+(1<<2))>>3);
} else { } else {
I = I + ((Ke*e)>>3); I = I + ((Ke*e+(1<<2))>>3);
} }
// ctrl signal, 7 bits + direction // ctrl signal, 7 bits + direction
...@@ -94,19 +109,20 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { ...@@ -94,19 +109,20 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
// set pwm switching time // set pwm switching time
if (u < 0) { if (u < 0) {
PORTC |= 0x80; PORTC |= 0x80; // set direction of motion
OCR1BL = (unsigned char) (128-(-u)); OCR1BL = (unsigned char) (128-(-u)); // set length of pwm-high
} else { } else {
PORTC = (PORTC & 0x7F); PORTC = (PORTC & 0x7F); // set direction of motion
OCR1BL = (unsigned char) (127-u); OCR1BL = (unsigned char) (127-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
if (status == 0x80) { if (status == 0x80) {
ref = (int16_t)((int8_t)(inp(TWDR))); // read reference ref = (int16_t)((int8_t)(inp(TWDR))); // read 8 bit reference
ref = ref << 2; // shifted 2 steps for 10 bits for reference ref = ref << 2; // shift up 2 steps for 10 bits for reference in loop
} }
else { else {
} }
...@@ -137,13 +153,13 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { ...@@ -137,13 +153,13 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
int main() int main()
{ {
// clear interrupts // clear interrupts (might not be needed)
cli(); cli();
//Port directions //Port directions
outp(0x08,PORTC); // pull up on overtemp signals outp(0x08,PORTC); // pull up on overtemperature signals
outp(0xa0,DDRC); // output on dir and brake outp(0xa0,DDRC); // output on direction and brake
outp(0x10,DDRD); // output on pwm outp(0x10,DDRD); // output on pwm-signal
/* Timer section */ /* Timer section */
...@@ -151,7 +167,7 @@ int main() ...@@ -151,7 +167,7 @@ int main()
outp(BV(COM1A1)|BV(COM1B1)|BV(COM1A0)|BV(COM1B0)|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-> h_pwm=8.61 micros) // Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> 115.2 kHz)
outp(0x00,OCR1AH); outp(0x00,OCR1AH);
outp(0x7f,OCR1AL); outp(0x7f,OCR1AL);
outp(0x00,TCNT1H); outp(0x00,TCNT1H);
...@@ -160,7 +176,7 @@ int main() ...@@ -160,7 +176,7 @@ int main()
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 */ /* 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);
outp(0x3f,OCR2); outp(0x3f,OCR2);
/* Reset timer 2 */ /* Reset timer 2 */
...@@ -171,12 +187,12 @@ int main() ...@@ -171,12 +187,12 @@ int main()
//Serial communication //Serial communication
outp(0x00, UCSRA); // USART: outp(0x00, UCSRA); // USART:
outp(0x18, UCSRB); // USART: RxIntEnable|RxEnable|TxEnable outp(0x18, UCSRB); // USART: RxEnable|TxEnable
outp(0x86, UCSRC); // USART: 8bit, no parity outp(0x86, UCSRC); // USART: 8bit, no parity
outp(0x00, UBRRH); // USART: 115200 @ 14.7456MHz outp(0x00, UBRRH); // USART: 115200 @ 14.7456MHz
outp(7,UBRRL); // 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, MUX0 for current measurement */
outp(BV(REFS0)|BV(MUX0),ADMUX); outp(BV(REFS0)|BV(MUX0),ADMUX);
// Enable ADC, start first conversion, prescaler 32, not free running mode // Enable ADC, start first conversion, prescaler 32, not free running mode
......
...@@ -25,7 +25,8 @@ ...@@ -25,7 +25,8 @@
#define POLL_AXIS1_POSITION 0x0001 #define POLL_AXIS1_POSITION 0x0001
#define POLL_AXIS1_VELOCITY 0x0002 #define POLL_AXIS1_VELOCITY 0x0002
//#define POLL_PEND_ANGLE 0x0004 #define POLL_PEND_ANGLE 0x0004
#define POLL_CURRENT_REFERENCE 0x0008
//#define POLL_AXIS1_RESET 0x0008 //#define POLL_AXIS1_RESET 0x0008
#define POLL_CONFIG 0x8000 #define POLL_CONFIG 0x8000
...@@ -60,7 +61,8 @@ void pccom_receiveByte(char ch) ...@@ -60,7 +61,8 @@ void pccom_receiveByte(char ch)
switch (serialio_channel) { switch (serialio_channel) {
case 0: { addPoll(POLL_AXIS1_POSITION); } break; case 0: { addPoll(POLL_AXIS1_POSITION); } break;
case 1: { addPoll(POLL_AXIS1_VELOCITY); } break; case 1: { addPoll(POLL_AXIS1_VELOCITY); } break;
//case 2: { addPoll(POLL_PEND_ANGLE); } break; case 2: { addPoll(POLL_PEND_ANGLE); } break;
case 3: { addPoll(POLL_CURRENT_REFERENCE); } break;
case 31: { addPoll(POLL_CONFIG); } break; case 31: { addPoll(POLL_CONFIG); } break;
} }
} break; } break;
...@@ -89,13 +91,21 @@ static uint8_t sendConfigPacket(uint8_t position) ...@@ -89,13 +91,21 @@ static uint8_t sendConfigPacket(uint8_t position)
case 1: CONF_ANALOG_IN(0, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break; case 1: CONF_ANALOG_IN(0, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
case 2: CONF_ANALOG_IN(0, CONF_MAX(CONF_POSITIVE_VOLT(1))); break; case 2: CONF_ANALOG_IN(0, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
case 3: CONF_ANALOG_IN(1, CONF_RESOLUTION(18)); break; // Velocity estimate case 3: CONF_ANALOG_IN(1, CONF_RESOLUTION(18)); break; // Velocity estimate
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 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 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 6: CONF_ANALOG_OUT(0, CONF_RESOLUTION(13)); break; // Reference to vel-ctrl case 12: CONF_ANALOG_OUT(0, CONF_RESOLUTION(13)); break; // Reference to vel-ctrl
case 7: CONF_ANALOG_OUT(0, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break; case 13: CONF_ANALOG_OUT(0, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
case 8: CONF_ANALOG_OUT(0, CONF_MAX(CONF_POSITIVE_VOLT(1))); break; case 14: CONF_ANALOG_OUT(0, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
default: CONF_END(); return 0; default: CONF_END(); return 0;
} }
...@@ -118,6 +128,14 @@ static uint8_t sendNextPacket() // returns 1 if a packet was available ...@@ -118,6 +128,14 @@ static uint8_t sendNextPacket() // returns 1 if a packet was available
toPoll &= ~POLL_AXIS1_VELOCITY; toPoll &= ~POLL_AXIS1_VELOCITY;
serialio_putchannel(1, getVelocity()+(1L<<17)); serialio_putchannel(1, getVelocity()+(1L<<17));
} }
else if (toPoll & POLL_PEND_ANGLE) {
toPoll &= ~POLL_PEND_ANGLE;
serialio_putchannel(2, getAngle()+(1L<<9));
}
else if (toPoll & POLL_CURRENT_REFERENCE) {
toPoll &= ~POLL_CURRENT_REFERENCE;
serialio_putchannel(3, getCurrentRef()+(1L<<7));
}
else if (toPoll & POLL_CONFIG) { else if (toPoll & POLL_CONFIG) {
if (configPosition < 0) configPosition = 0; // Start sending config? if (configPosition < 0) configPosition = 0; // Start sending config?
if (!sendConfigPacket(configPosition)) { // Last packet? if (!sendConfigPacket(configPosition)) { // Last packet?
......
...@@ -11,14 +11,12 @@ ...@@ -11,14 +11,12 @@
// reference variables // reference variables
volatile int32_t ref = 0; // 11 frac bits volatile int32_t ref = 0; // 11 frac bits
volatile int16_t refFlag = 0; volatile int32_t refCtrl = 0;
volatile int16_t deltaRef = 1;
volatile int16_t refCount = 0;
volatile int32_t refTest = 0;
// 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 v = 0; // 11 frac bits volatile int32_t v = 0; // 11 frac bits
volatile int8_t brake = 0; volatile int8_t brake = 0;
volatile int32_t I = 0; // 11 frac bits volatile int32_t I = 0; // 11 frac bits
...@@ -35,6 +33,7 @@ volatile int32_t Ksat = 3; // 6 frac bits ...@@ -35,6 +33,7 @@ volatile int32_t Ksat = 3; // 6 frac bits
volatile int16_t pos = 0; volatile int16_t pos = 0;
volatile int16_t posTemp = 0; volatile int16_t posTemp = 0;
volatile int16_t posCtrl = 0;
volatile int16_t oldPos = 0; volatile int16_t oldPos = 0;
volatile int8_t newX; volatile int8_t newX;
volatile int8_t newY; volatile int8_t newY;
...@@ -44,24 +43,43 @@ volatile int8_t sum = 0; ...@@ -44,24 +43,43 @@ volatile int8_t sum = 0;
// velocity estimation parameters // velocity estimation parameters
volatile int32_t velEst = 0; // 5 frac bits volatile int32_t velEst = 0; // 5 frac bits
volatile int32_t velEstTemp = 0; //volatile int32_t velEstTemp = 0;
volatile int32_t velEstSend = 0;
int16_t a = 116; //7 frac bits int16_t a = 116; //7 frac bits
int16_t b = 152; // 5 frac bits int16_t b = 152; // 5 frac bits
// adc measurement variables
volatile int16_t low;
volatile int16_t high;
// return position (in tics) // return position (in tics)
int32_t getPosition() { int32_t getPosition() {
cli();
posTemp = pos; posTemp = pos;
sei();
return ((int32_t) posTemp); return ((int32_t) posTemp);
} }
// return velocity (in mm/s) // return velocity (in mm/s)
int32_t getVelocity() { int32_t getVelocity() {
return velEst; return velEstSend;
}
// return last angle measurement
int16_t getAngle() {
low = inp(ADCL);
high = inp(ADCH);
return ((int16_t) ((high<<8) | low) - 512);
}
// return current reference
int32_t getCurrentRef() {
return uSend;
} }
/* Routine used to set the red LED */ /* Routine used to set the red LED */
void setLED(uint8_t on) void setLED(uint8_t on)
{ {
...@@ -82,16 +100,16 @@ void initPos() ...@@ -82,16 +100,16 @@ void initPos()
oldY = ENCODERY; oldY = ENCODERY;
} }
/* Routine used to track the cart position */ /* Routine used to reset the cart position */
void setPos() void resetPos()
{ {
} }
/* Timer 2, Encoder counter, 73 kHz */ /* 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;
...@@ -118,7 +136,7 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { ...@@ -118,7 +136,7 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
oldY = newY; oldY = newY;
} }
} }
...@@ -126,12 +144,10 @@ SIGNAL(SIG_OUTPUT_COMPARE2) { ...@@ -126,12 +144,10 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
/* Timer 1, serial communication */ /* Timer 1, serial communication */
SIGNAL(SIG_OUTPUT_COMPARE0) { SIGNAL(SIG_OUTPUT_COMPARE0) {
TIMSK &= ~(BV(OCIE1A)|BV(OCIE0)); TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));
sei(); // enable interrupts from timer 0 and timer 2
//PORTC |= 0x10; // to clock calulation time
sei(); // enable interrupts from timer 0 and timer 2
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;
...@@ -147,31 +163,42 @@ SIGNAL(SIG_OUTPUT_COMPARE0) { ...@@ -147,31 +163,42 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
int16_t toSend = pccom_getNextByteToSend(); int16_t toSend = pccom_getNextByteToSend();
if (toSend >= 0) UDR = (char)toSend; if (toSend >= 0) UDR = (char)toSend;
} }
//PORTC &= ~0x10;
TIMSK |= (BV(OCIE1A)|BV(OCIE0)); PORTC &= ~0x10;
TIFR = (1<<OCF0);
//TIFR = (1<<OCF1A);
TIMSK |= (BV(OCIE0)|BV(OCIE1A));
} }
/* Timer 0, control loop , 1 kHz */ /* Timer 0, control loop , 1 kHz */
SIGNAL(SIG_OUTPUT_COMPARE1A) { SIGNAL(SIG_OUTPUT_COMPARE1A) {
TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));
sei(); // to enable interupts from timer2 TIMSK &= ~(BV(OCIE1A));
posCtrl = pos; // to aviod overwrite while reading
sei(); // to enable interupts from timer2
// linear velocity estimator // linear velocity estimator
// velocity estimate in mm/s // velocity estimate in mm/s
velEst = (((a*velEst+64)>>7)+b*(pos-oldPos)); // 5 fracbits on velEst velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst
oldPos = pos; oldPos = posCtrl;
// emergency stop
ref = ref*(1-brake);
cli();
velEstSend = velEst;
refCtrl = ref;
sei();
// control error // control error
//e = ref-((int16_t)((velEst+16)>>5)); // mm/s e = refCtrl-((velEst+16)>>5); // mm/s
e = ref-((velEst+16)>>5); // mm/s
v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4)); v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
...@@ -181,13 +208,13 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) { ...@@ -181,13 +208,13 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
} else if (v < -2048) { } else if (v < -2048) {
I = I + ((((Ke*e) + (Ksat)*(-2048-v))+(1<<1))>>2); I = I + ((((Ke*e) + (Ksat)*(-2048-v))+(1<<1))>>2);
} else { } else {
I = I + ((Ke*e+(1<<3))>>2); I = I + ((Ke*e+(1<<1))>>2);
} }
u = (v+8)>>4; //8 frac bits to current loop u = (v+8)>>4; //8 frac bits to current loop
// friction compensation // friction compensation
if (ref > 0) { if (refCtrl > 0) {
u = u+10; u = u+10;
} else if (ref < 0) { } else if (ref < 0) {
u = u-10; u = u-10;
...@@ -200,42 +227,23 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) { ...@@ -200,42 +227,23 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
u = -128; u = -128;
} }
//u = 2; cli();
uSend = u;
sei();
/*
// reference calculations
refCount++;
// reset position, velocity estimate and integral part of ctrl if reset button pushed
if (refFlag == 0) { if (!(PIND & 0x40)) {
if (refCount%1 == 0) cli();
ref = ref+deltaRef; pos = 0; // reset position
if (refCount == 500) { sei();
refFlag = 1; oldPos = 0;
deltaRef = -deltaRef; velEst = 0; // reset velocity estimate
refCount = 0; I = 0; // reset integral part of controller
}