diff --git a/linear_pendulum_2009/avr/current_control_final.c b/linear_pendulum_2009/avr/current_control_final.c
index 5d52f9a51db1ff927866e92d0d6738d90a69e9ac..dd4b96c021f2a6a90dd5e1478a4e815ff94ee9b4 100644
--- a/linear_pendulum_2009/avr/current_control_final.c
+++ b/linear_pendulum_2009/avr/current_control_final.c
@@ -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();
diff --git a/linear_pendulum_2009/avr/pccom.c b/linear_pendulum_2009/avr/pccom.c
index 861698bdbde6829a66a76e32b7a2281fef544d9c..4991404d619f1b38308d5886cc0e5275ff704415 100644
--- a/linear_pendulum_2009/avr/pccom.c
+++ b/linear_pendulum_2009/avr/pccom.c
@@ -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!)
diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
index 598efe7b5144fce399dfcfeebf31492d2ae36c59..5024d75bfe7272adfd579bfc083e4b0e7e585880 100644
--- a/linear_pendulum_2009/avr/vel_control.c
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -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