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

Added mutex for shared variables

parent 36135468
......@@ -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 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 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
......
......@@ -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;
......@@ -93,9 +95,17 @@ 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_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 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 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?
......
......@@ -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,8 +100,8 @@ void initPos()
oldY = ENCODERY;
}
/* Routine used to track the cart position */
void setPos()
/* Routine used to reset the cart position */
void resetPos()
{
}
......@@ -126,12 +144,10 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
/* Timer 1, serial communication */
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
PORTC |= 0x10; // to clock calulation time
uint8_t status = UCSRA;
if (status & (1<<RXC)) {
char ch = UDR;
......@@ -147,9 +163,12 @@ 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));
}
......@@ -158,20 +177,28 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
/* Timer 0, control loop , 1 kHz */
SIGNAL(SIG_OUTPUT_COMPARE1A) {
TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));
TIMSK &= ~(BV(OCIE1A));
sei(); // to enable interupts from timer2
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;
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,41 +227,22 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
u = -128;
}
//u = 2;
/*
// reference calculations
refCount++;
cli();
uSend = u;
sei();
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;
}
}
*/
// reset position, velocity estimate and integral part of ctrl if reset button pushed
/*
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
......@@ -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
......
......@@ -5,5 +5,7 @@
void setRef(int32_t newRef);
int32_t getPosition();
int32_t getVelocity();
int16_t getAngle();
int32_t getCurrentRef();
#endif
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