Skip to content

GitLab

Commit 63447c91 authored by Pontus Giselsson's avatar Pontus Giselsson
Browse files

Velocity controller with slow simulink communication, program structure must be rebuilt.

parent 37d6b571
Hide whitespace changes
Inline Side-by-side
linear_pendulum_2009/avr/current_control_final.c
View file @ 63447c91
......@@ -6,14 +6,14 @@
// control variables
volatile int16_t ref=0;
volatile int16_t ref=0; // reference value (from velocity controller)
volatile unsigned char lbyte,hbyte;
volatile int16_t y;
volatile int16_t y; // measurement, 9 frac bits
volatile uint8_t low, high;
volatile int16_t e = 0;
volatile int16_t v = 0;
volatile int16_t I = 0;
volatile int16_t u = 0;
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 Ksat = 44; // 7 frac bits, h/Tr
......@@ -33,31 +33,46 @@ volatile int8_t stop = 0;
volatile int16_t status;
/* Interrupt service routine for handling incoming bytes on the serial port
might be needed to catch incoming bytes */
SIGNAL(SIG_UART_RECV){}
/* Send logged data over Serial connection */
static inline void sendData() {
int16_t ii = 0;
while (ii < log_len) {
putchar((unsigned char) ((ctrl_log[ii]&0xff00)>>8));
putchar((unsigned char) (ctrl_log[ii]&0x00ff));
/* Timer 1 compare match, start AD-conversion
* Additional changes: Dont use free running AD-conversion
* Enable overflow interrupts timer1
*/
putchar((unsigned char) ((error_log[ii]&0xff00)>>8));
putchar((unsigned char) (error_log[ii]&0x00ff));
putchar((unsigned char) ((I_log[ii]&0xff000000)>>24));
putchar((unsigned char) ((I_log[ii]&0x00ff0000)>>16));
putchar((unsigned char) ((I_log[ii]&0x0000ff00)>>8));
putchar((unsigned char) (I_log[ii]&0x000000ff));
ii++;
}
}
/* Timer 2 compare match interupt */
SIGNAL(SIG_OUTPUT_COMPARE2) {
// Start AD conversion
ADCSRA |= BV(ADSC);
PORTB |= 0x40; //Turn on calculation timer
// Read input
low = inp(ADCL);
high = inp(ADCH);
y = ((high<<8) | low) - 512; //y 9 frac bits
//control, since negative measurements
//control error, (+) since negative measurements
e = ref+y; //e 9 frac bits
//v = (int16_t)((K*e+I+64)>>7);
v = (int16_t)(((K*e+64)>>7)+(I>>4));
//saturation and update integral part of ctrl
if (v > 511) {
I = I + (((Ke*e) + (Ksat)*(511-v))>>3);
......@@ -67,15 +82,16 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
I = I + ((Ke*e)>>3);
}
// Original styrning med 7 bitar + direction
// 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 = 0;
// set pwm switching time
if (u < 0) {
PORTC |= 0x80;
......@@ -85,7 +101,7 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
OCR1BL = (unsigned char) (127-u);
}
// TWI-communication (need status == 0x60?)
// TWI-communication, recieve reference from velocity controller
if ((BV(TWINT)&inp(TWCR))) {
status = (inp(TWSR)&0xf8);
if (status == 0x80) {
......@@ -97,27 +113,39 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
outp(BV(TWINT)|BV(TWEN)|BV(TWEA),TWCR);
}
PORTB &= 0xbf; //Turn off calculation timer
// For logging purposes
/*
countSamples++;
if (countSamples == skipSamples) {
ctrl_log[jj] = u;
I_log[jj] = I;
error_log[jj] = e;
jj++;
countSamples = 0;
// Stop after a while
if ((jj == (log_len-1)) & !stop) {
outp(0x7f,OCR1BL);
stop = 1;
sendData();
}
}
*/
}
int main()
{
// clear interrupts
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 for motor 1
outp(0x10,DDRD); // output on pwm
outp(0x40,DDRB); // output on timer calculation pin
outp(0x10,DDRA); // test pin output
/* Timer section */
// Enable timer2 compare match interrupts
outp(BV(OCIE2),TIMSK);
// 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);
......@@ -137,15 +165,17 @@ int main()
outp(0x3f,OCR2);
/* Reset timer 2 */
outp(0,TCNT2);
// Enable timer2 compare match interrupts
outp(BV(OCIE2),TIMSK);
//Serial communication
outp(0x00, UCSRA); // USART:
outp(0x98, UCSRB); // USART: RxIntEnable|RxEnable|TxEnable
outp(0x18, UCSRB); // USART: RxIntEnable|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 */
outp(BV(REFS0)|BV(MUX0),ADMUX);
......@@ -158,9 +188,6 @@ int main()
outp(0x02,TWAR); // slave address
outp(BV(TWEA)|BV(TWEN),TWCR); // enable TWI, enable ACK
//Enable interrupts
sei();
......
linear_pendulum_2009/avr/pccom.c
View file @ 63447c91
......@@ -107,13 +107,6 @@ static uint8_t sendNextPacket() // returns 1 if a packet was available
{
static int8_t configPosition = -1;
// static uint16_t toPoll = 0;
// // see if we should fetch new poll mask
// if (toPoll == 0) {
// toPoll = pccom_poll;
// pccom_poll = 0;
// if (toPoll == 0) return 0;
// }
#define toPoll pccom_poll // OK since sender and receiver are mutexed
// Send _first_ requested item (only one packet!)
......
linear_pendulum_2009/avr/vel_control.c
View file @ 63447c91
......@@ -31,6 +31,7 @@ volatile int32_t Ksat = 3; // 6 frac bits
#define ENCODERY (PIND&(uint8_t)(1<<2)) //Positional encoder pins
#define ENCODERX (PINB&(uint8_t)(1<<1)) //Positional encoder pins
volatile int16_t pos = 0;
volatile int16_t posTemp = 0;
volatile int16_t oldPos = 0;
volatile int16_t deltaPos = 0;
volatile int8_t newX;
......@@ -42,33 +43,14 @@ volatile int8_t sum = 0;
// velocity estimation parameters
volatile int32_t velEst = 0; // 5 frac bits
volatile int32_t velEstTemp = 0;
volatile int16_t posTemp = 0;
int16_t a = 116; //7 frac bits
int16_t b = 152; // 5 frac bits
/* 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) {};
}
/* Interrupt service routine for handling incoming bytes on the serial port
might be needed to catch incoming bytes */
SIGNAL(SIG_UART_RECV){}
/* Send logged data over Serial connection */
static inline void sendData() {
}
// return position (in tics)
int32_t getPosition() {
return pos;
posTemp = pos;
return ((int32_t) posTemp);
}
......@@ -141,6 +123,8 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
sei(); // to enable interupts from timer2
TIMSK &= ~BV(OCIE0);
PORTC |= 0x10; // to clock calulation time
// linear velocity estimator
......@@ -214,6 +198,7 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
ref = ref*(1-brake);
// TWI-communication
// send start command
outp(BV(TWINT)|BV(TWEN)|BV(TWSTA),TWCR);
......@@ -236,32 +221,41 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
// stop transmission
outp(BV(TWINT)|BV(TWEN)|BV(TWSTO),TWCR);
// ------- Noncritical section -------
// Poll UART receiver
uint8_t status = UCSRA;
if (status & (1<<RXC)) {
char ch = UDR;
pccom_receiveByte(ch);
if (status & ((1<<FE)|(1<<DOR)|(1<<PE))) {
//main_emergencyStop(); // stop on USART error
}
}
// Poll UART sender
if (UCSRA & (1<<UDRE)) {
int16_t toSend = pccom_getNextByteToSend();
//if (toSend >= 0) UDR = (char)toSend;
while (toSend >= 0) {
UDR = (char)toSend;
while ((UCSRA & (1<<UDRE)) == 0) {} // send all data in buffer
toSend = pccom_getNextByteToSend();
int8_t tempVar = 0;
for (tempVar = 0;tempVar < 10; tempVar++) {
uint8_t status = UCSRA;
if (status & (1<<RXC)) {
char ch = UDR;
pccom_receiveByte(ch);
if (status & ((1<<FE)|(1<<DOR)|(1<<PE))) {
//main_emergencyStop(); // stop on USART error
}
}
// Poll UART sender
if (UCSRA & (1<<UDRE)) {
int16_t toSend = pccom_getNextByteToSend();
//if (toSend >= 0) UDR = (char)toSend;
while (toSend >= 0) {
UDR = (char)toSend;
while ((UCSRA & (1<<UDRE)) == 0) {} // wait for previous send
toSend = pccom_getNextByteToSend();
}
}
}
PORTC &= ~0x10;
PORTC &= ~0x10;
TIMSK |= BV(OCIE0);
}
......@@ -300,10 +294,10 @@ int main()
/* AREF (AREF is 5V) pin external capacitor, MUX0 for current */
// outp(BV(REFS0)|BV(MUX0),ADMUX);
//outp(BV(REFS0)|BV(MUX3),ADMUX);
// Enable ADC, start first conversion, prescaler 32, not free running mode
// outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Enable ADC, start first conversion, prescaler 32, free running mode
//outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
// Initialize Master TWI
......
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