Skip to content

GitLab

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

Final package but with some strange factors

parent 7b394b34
Hide whitespace changes
Inline Side-by-side
Showing with 185 additions and 196 deletions
linear_pendulum_2009/avr/current_control_final.c
View file @ d9d85624
......@@ -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);
......
linear_pendulum_2009/avr/pccom.c
View file @ d9d85624
......@@ -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;
......
linear_pendulum_2009/avr/vel_control.c
View file @ d9d85624
......@@ -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);