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
......@@ -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();
......
......@@ -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!)
......
......@@ -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