From 7b394b340f9365f9c8eae9f5732b9de6c9a992b6 Mon Sep 17 00:00:00 2001
From: Pontus Giselsson <pontus.giselsson@control.lth.se>
Date: Thu, 12 Feb 2009 13:23:38 +0000
Subject: [PATCH] Added mutex for shared variables
---
.../avr/current_control_final.c | 72 +++++----
linear_pendulum_2009/avr/pccom.c | 34 +++-
linear_pendulum_2009/avr/vel_control.c | 152 ++++++++++--------
linear_pendulum_2009/avr/vel_control.h | 2 +
4 files changed, 153 insertions(+), 107 deletions(-)
diff --git a/linear_pendulum_2009/avr/current_control_final.c b/linear_pendulum_2009/avr/current_control_final.c
index dd4b96c..2c0f1bd 100644
--- a/linear_pendulum_2009/avr/current_control_final.c
+++ b/linear_pendulum_2009/avr/current_control_final.c
@@ -7,19 +7,22 @@
// control variables
volatile int16_t ref=0; // reference value (from velocity controller)
-volatile unsigned char lbyte,hbyte;
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 v = 0; // temporary ctrl signal, 9 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 K = 74; // 7 frac bits
-volatile int16_t Ke = 26; //7 frac bits, K*h/Ti
+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
+// twi variable
+volatile int16_t status;
+
// logging variables
+/*
#define log_len 100
volatile int16_t ctrl_log[log_len];
volatile int16_t error_log[log_len];
@@ -28,12 +31,23 @@ volatile int16_t skipSamples = 1000;
volatile int16_t countSamples = 0;
volatile int16_t jj=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 */
+ /*
static inline void sendData() {
int16_t ii = 0;
while (ii < log_len) {
@@ -54,16 +68,16 @@ static inline void sendData() {
ii++;
}
}
+ */
-
-/* Timer 2 compare match interupt */
+/* Timer 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */
SIGNAL(SIG_OUTPUT_COMPARE2) {
// Start AD conversion
ADCSRA |= BV(ADSC);
- // Read input
+ // Read previous AD-conversion result
low = inp(ADCL);
high = inp(ADCH);
y = ((high<<8) | low) - 512; //y 9 frac bits
@@ -71,15 +85,16 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
//control error, (+) since negative measurements
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
+ //saturation and update integral part of ctrl with antiwindup
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) {
- I = I + (((Ke*e) + (Ksat)*(-512-v))>>3);
+ I = I + ((((Ke*e) + (Ksat)*(-512-v))+(1<<2))>>3);
} else {
- I = I + ((Ke*e)>>3);
+ I = I + ((Ke*e+(1<<2))>>3);
}
// ctrl signal, 7 bits + direction
@@ -94,19 +109,20 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
// set pwm switching time
if (u < 0) {
- PORTC |= 0x80;
- OCR1BL = (unsigned char) (128-(-u));
+ PORTC |= 0x80; // set direction of motion
+ OCR1BL = (unsigned char) (128-(-u)); // set length of pwm-high
} else {
- PORTC = (PORTC & 0x7F);
- OCR1BL = (unsigned char) (127-u);
+ PORTC = (PORTC & 0x7F); // set direction of motion
+ OCR1BL = (unsigned char) (127-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 reference
- ref = ref << 2; // shifted 2 steps for 10 bits for 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
}
else {
}
@@ -137,13 +153,13 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
int main()
{
- // clear interrupts
+ // clear interrupts (might not be needed)
cli();
//Port directions
- outp(0x08,PORTC); // pull up on overtemp signals
- outp(0xa0,DDRC); // output on dir and brake
- outp(0x10,DDRD); // output on pwm
+ outp(0x08,PORTC); // pull up on overtemperature signals
+ outp(0xa0,DDRC); // output on direction and brake
+ outp(0x10,DDRD); // output on pwm-signal
/* Timer section */
@@ -151,7 +167,7 @@ int main()
outp(BV(COM1A1)|BV(COM1B1)|BV(COM1A0)|BV(COM1B0)|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-> 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(0x7f,OCR1AL);
outp(0x00,TCNT1H);
@@ -160,7 +176,7 @@ int main()
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(0x3f,OCR2);
/* Reset timer 2 */
@@ -171,12 +187,12 @@ int main()
//Serial communication
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(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, MUX0 for current measurement */
outp(BV(REFS0)|BV(MUX0),ADMUX);
// Enable ADC, start first conversion, prescaler 32, not free running mode
diff --git a/linear_pendulum_2009/avr/pccom.c b/linear_pendulum_2009/avr/pccom.c
index 4991404..7187302 100644
--- a/linear_pendulum_2009/avr/pccom.c
+++ b/linear_pendulum_2009/avr/pccom.c
@@ -25,7 +25,8 @@
#define POLL_AXIS1_POSITION 0x0001
#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_CONFIG 0x8000
@@ -60,7 +61,8 @@ void pccom_receiveByte(char ch)
switch (serialio_channel) {
case 0: { addPoll(POLL_AXIS1_POSITION); } 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;
}
} break;
@@ -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 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 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 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 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 7: 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 12: CONF_ANALOG_OUT(0, CONF_RESOLUTION(13)); break; // Reference to vel-ctrl
+ case 13: CONF_ANALOG_OUT(0, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
+ case 14: CONF_ANALOG_OUT(0, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
default: CONF_END(); return 0;
}
@@ -118,6 +128,14 @@ static uint8_t sendNextPacket() // returns 1 if a packet was available
toPoll &= ~POLL_AXIS1_VELOCITY;
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) {
if (configPosition < 0) configPosition = 0; // Start sending config?
if (!sendConfigPacket(configPosition)) { // Last packet?
diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
index 3b10f60..204a3a8 100644
--- a/linear_pendulum_2009/avr/vel_control.c
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -11,14 +11,12 @@
// reference variables
volatile int32_t ref = 0; // 11 frac bits
-volatile int16_t refFlag = 0;
-volatile int16_t deltaRef = 1;
-volatile int16_t refCount = 0;
-volatile int32_t refTest = 0;
+volatile int32_t refCtrl = 0;
// 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
@@ -35,6 +33,7 @@ volatile int32_t Ksat = 3; // 6 frac bits
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;
@@ -44,24 +43,43 @@ volatile int8_t sum = 0;
// velocity estimation parameters
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 b = 152; // 5 frac bits
+// adc measurement variables
+volatile int16_t low;
+volatile int16_t high;
// return position (in tics)
int32_t getPosition() {
+ cli();
posTemp = pos;
+ sei();
return ((int32_t) posTemp);
}
// return velocity (in mm/s)
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 */
void setLED(uint8_t on)
{
@@ -82,16 +100,16 @@ void initPos()
oldY = ENCODERY;
}
-/* Routine used to track the cart position */
-void setPos()
+/* Routine used to reset the cart position */
+void resetPos()
{
-
+
}
/* Timer 2, Encoder counter, 73 kHz */
SIGNAL(SIG_OUTPUT_COMPARE2) {
-
+
// Update position from encoder
newX = ENCODERX;
@@ -118,7 +136,7 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
oldY = newY;
}
-
+
}
@@ -126,12 +144,10 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
/* Timer 1, serial communication */
SIGNAL(SIG_OUTPUT_COMPARE0) {
- TIMSK &= ~(BV(OCIE1A)|BV(OCIE0));
-
- sei(); // enable interrupts from timer 0 and timer 2
-
- //PORTC |= 0x10; // to clock calulation time
+ TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));
+ sei(); // enable interrupts from timer 0 and timer 2
+ PORTC |= 0x10; // to clock calulation time
uint8_t status = UCSRA;
if (status & (1<<RXC)) {
char ch = UDR;
@@ -147,31 +163,42 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
int16_t toSend = pccom_getNextByteToSend();
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 */
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
- // velocity estimate in mm/s
- velEst = (((a*velEst+64)>>7)+b*(pos-oldPos)); // 5 fracbits on velEst
- oldPos = pos;
+ // velocity estimate in mm/s
+ velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst
+ oldPos = posCtrl;
+
+ // emergency stop
+ ref = ref*(1-brake);
+
+ cli();
+ velEstSend = velEst;
+ refCtrl = ref;
+ sei();
// control error
- //e = ref-((int16_t)((velEst+16)>>5)); // mm/s
- e = ref-((velEst+16)>>5); // mm/s
+ e = refCtrl-((velEst+16)>>5); // mm/s
v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
@@ -181,13 +208,13 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
} else if (v < -2048) {
I = I + ((((Ke*e) + (Ksat)*(-2048-v))+(1<<1))>>2);
} 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
// friction compensation
- if (ref > 0) {
+ if (refCtrl > 0) {
u = u+10;
} else if (ref < 0) {
u = u-10;
@@ -200,42 +227,23 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
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 (refCount%1 == 0)
- ref = ref+deltaRef;
- if (refCount == 500) {
- refFlag = 1;
- deltaRef = -deltaRef;
- refCount = 0;
- }
- } else {
- if (refCount%1 == 0)
- ref = ref+deltaRef;
- if (refCount == 1000) {
- refCount = 0;
- deltaRef = -deltaRef;
- }
- }
- */
-
- /*
- if (refCount == 1000) {
- ref = 0;
+ 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
}
- */
-
-
- ref = ref*(1-brake);
-
-
// TWI-communication
// send start command
outp(BV(TWINT)|BV(TWEN)|BV(TWSTA),TWCR);
@@ -260,9 +268,10 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
- TIMSK |= (BV(OCIE0)|BV(OCIE1A));
-
+ //TIFR = (1<<OCF0);
+ TIFR = (1<<OCF1A);
+ TIMSK |= (BV(OCIE1A));
}
@@ -307,21 +316,22 @@ int main()
/* Timer 1, 1 kHz */
outp(BV(WGM12)|BV(CS10),TCCR1B);
outp(0x39,OCR1AH);
- outp(0x99,OCR1AL);
+ outp(0x7f,OCR1AL);
outp(0,TCNT1H);
outp(0,TCNT1L);
- /* Timer 0, 10 kHz, Prescaler 64 */
- outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
+ /* Timer 0, 10 kHz, Prescaler 8 */
+ outp(BV(WGM01)|BV(CS01),TCCR0);
//outp(200,OCR0);
- outp(23,OCR0); // 10 kHz
+ outp(184-1,OCR0); // 10 kHz
/* Reset timer 0 */
outp(0,TCNT0);
//----------------------------------------------------------
-
+ // syncronization (timer1 interrupt 34 micros before timer0 interrupt)
+ TCNT1 = TCNT0*8+500;
//Serial communication
@@ -334,10 +344,10 @@ int main()
/* AREF (AREF is 5V) pin external capacitor, MUX0 for current */
- //outp(BV(REFS0)|BV(MUX3),ADMUX);
+ outp(BV(REFS0)|BV(MUX0)|BV(MUX1),ADMUX);
- // Enable ADC, start first conversion, prescaler 32, free running mode
- //outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
+ // Enable ADC on mux3, start first conversion, prescaler 128, free running mode
+ outp(BV(ADEN)|BV(ADSC)|BV(ADATE)|BV(ADPS2)|BV(ADPS1)|BV(ADPS0),ADCSRA);
// Initialize Master TWI
diff --git a/linear_pendulum_2009/avr/vel_control.h b/linear_pendulum_2009/avr/vel_control.h
index 3ffacdd..88d833b 100644
--- a/linear_pendulum_2009/avr/vel_control.h
+++ b/linear_pendulum_2009/avr/vel_control.h
@@ -5,5 +5,7 @@
void setRef(int32_t newRef);
int32_t getPosition();
int32_t getVelocity();
+int16_t getAngle();
+int32_t getCurrentRef();
#endif
--
GitLab