diff --git a/linear_pendulum_2009/avr/current_control.c b/linear_pendulum_2009/avr/current_control.c
deleted file mode 100644
index adb0f97160f8068472aec7055d75f8bf6c3d0977..0000000000000000000000000000000000000000
--- a/linear_pendulum_2009/avr/current_control.c
+++ /dev/null
@@ -1,313 +0,0 @@
-/*********************************************
-
- Current regulation - Aleks Ponjavic 18/09/07
- for LTH - reglerteknik
-
- Note that the voltage is actually a duty
- cycle for the PWM. The PWM is running at
- 12V with a duty cycle set by a 10bit value
-
-*********************************************/
-
-#include <avr/io.h>
-#include <avr/signal.h>
-#include <avr/interrupt.h>
-#include <inttypes.h>
-
-
-// reference generation variables
-volatile int16_t ref = 100;
-volatile int16_t refCount = 0;
-volatile int8_t refFlag = 0;
-
-
-// control variables
-volatile unsigned char lbyte,hbyte;
-volatile int16_t y;
-volatile uint8_t alt = 1;
-volatile uint8_t low, high;
-
-//test variables
-volatile int16_t e = 0;
-volatile int16_t v = 0;
-volatile int16_t I = 0;
-volatile int16_t u = 0;
-volatile int16_t K = 74;//5;//375; // 7 frac bits
-volatile int16_t Ke = 26;//2;//6; //7 frac bits, K*h/Ti
-volatile int16_t Ksat = 44;//1; // 7 frac bits, h/Tr
-
-
-// logging variables
-#define log_len 100
-volatile int16_t ctrl_log[log_len];
-volatile int16_t error_log[log_len];
-volatile int32_t I_log[log_len];
-volatile int16_t skipSamples = 1000;
-volatile int16_t countSamples = 0;
-volatile int16_t jj=0;
-volatile int8_t stop = 0;
-volatile int16_t temp;
-
-
-// pwm sync variables
-volatile int8_t pwmCycles = 0;
-
-
-/* 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){}
-
-
-static inline int16_t readInput() {
- uint8_t low, high;
- ADCSRA |= 0x40;
- while (ADCSRA & 0x40);
- low = ADCL;
- high = ADCH;
- return ((high<<8) | low) - 512;
-}
-
-
-/* Write 8-bit output using the PWM-generator */
-static inline void writeOutput(int16_t val) {
- if (val < 0) {
- PORTC = 0x80+(PORTC & 0x7F);
- OCR1BH = 0; //(unsigned char) (-val)&0xff00;
- OCR1BL = (unsigned char) (-val)&0x00ff;
- } else {
- PORTC = (PORTC & 0x7F);
- OCR1BH = 0; //(unsigned char) (val&0xff00);
- OCR1BL = (unsigned char) (val&0x00ff);
- }
-}
-
-
-/* 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));
-
- 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 1 compare match, start AD-conversion
- * Additional changes: Dont use free running AD-conversion
- * Enable overflow interrupts timer1
- */
-SIGNAL(SIG_OUTPUT_COMPARE2) {
- // Start AD conversion
- ADCSRA |= BV(ADSC);
-
- // Code previously in sig_adc
- PORTA |= 0x10; //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
- e = ref+y; //e 9 frac bits
-
- //v = (int16_t)((K*e+I+64)>>7);
- v = (int16_t)(((K*e+64)>>7)+(temp>>4));
- //saturation and update integral part of ctrl
- if (v > 511) {
- temp = temp + (((Ke*e) + (Ksat)*(511-v))>>3);
- } else if (v < -512) {
- temp = temp + (((Ke*e) + (Ksat)*(-512-v))>>3);
- } else {
- temp = temp + ((Ke*e)>>3);
- }
-
- /*
- //saturation and update integral part of ctrl
- if (v > 511) {
- I = I +(int32_t)((Ke*e) + (Ksat)*(511-v));
- } else if (v < -512) {
- I = I +(int32_t)((Ke*e) + (Ksat)*(-512-v));
- } else {
- I = I +(int32_t)(Ke*e);
- }
- */
- // write output, inverting mode means set pwm to 127-ctrl_out
-
-
- // Original styrning med 7 bitar + direction
- u = (v+2)>>2; //7 frac bits to pwm
-
- if (u > 127) {
- u = 127;
- } else if (u < -128) {
- u = -128;
- }
-
- if (u < 0) {
- PORTC |= 0x80;
- OCR1BL = (unsigned char) (128-(-u));
- } else {
- PORTC = (PORTC & 0x7F);
- OCR1BL = (unsigned char) (127-u);
- }
-
-
- // For logging
- countSamples++;
- if (countSamples == skipSamples) {
- ctrl_log[jj] = y;
- I_log[jj] = u;
- error_log[jj] = e;
- jj++;
- countSamples = 0;
-
- // Stop after a while
- if ((jj == (log_len-1)) & !stop) {
- outp(0x7f,OCR1BL);
- stop = 1;
- sendData();
- }
- }
-
- PORTA &= 0xef; //Turn off calculation timer
-
-}
-
-
-
-/* reference square- or triangle wave generator with timer 0 */
-SIGNAL(SIG_OVERFLOW0) {
- int8_t rectangle = 1;
- refCount++;
- if (rectangle == 1) {
- if (refFlag == 0) {
- if (refCount == 20) {
- refFlag = 1;
- ref = -ref;
- refCount = 0;
- }
- } else {
- if (refCount == 40) {
- ref = -ref;
- refCount = 0;
- }
- }
- } else {
- if (refCount <= 200) { // ref
- ref -= 1;
- } else {
- ref += 1;
- }
- if (refCount == 400) { // ref*2
- refCount = 0;
- }
- }
-}
-
-
-int main()
-{
- cli();
- int i,j;
-
- //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,DDRA); // test pin output
-
- /* Timer section */
- // Enable timer0 overflow interrupts
- // and enable timer1 overflow interrupts for pwm-control sync
- outp(BV(TOIE0)|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);
- 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)
- // 7 bit ctrl-signal and direction
- outp(0x00,OCR1AH);
- outp(0x7f,OCR1AL);
-
- outp(0x00,TCNT1H);
- outp(0x00,TCNT1L);
-
- // 7 bit ctrl-signal and direction
- outp(0x00,OCR1BH);
- outp(0x7f,OCR1BL); // to not start motor-rotation before control
-
-
- /* Timer 2, 4 times pwm-period, for control sampling, prescaler 8 */
- outp(BV(WGM21)|BV(CS21),TCCR2);
- outp(0x3f,OCR2);
- /* Reset timer 2 */
- outp(0,TCNT2);
-
- /* Timer 0 for reference generation, prescaler = 1024 periodic h = ? */
- outp(BV(CS02)|BV(CS00),TCCR0);
-
- //Serial communication
- outp(0x00, UCSRA); // USART:
- outp(0x98, 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, MUX3?? for pendulum angle */
- outp(BV(REFS0)|BV(MUX0),ADMUX);
-
- // Enable ADC interrupts, start first conversion, prescaler 32, free running mode
- //outp(BV(ADEN)|BV(ADATE)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);
-
- // Enable ADC interrupts, start first conversion, prescaler 32, not free running mode
- //outp(BV(ADEN)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);
-
- // Enable ADC, start first conversion, prescaler 32, not free running mode
- outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
-
-
- // outp(BV(ADEN)|BV(ADSC),ADCSRA);
-
- /* Wait a little bit, probably not needed...*/
- int tmp;
- // for(i=0;i<2000;i++)
- // for(j=0;j<400;j++)
- // tmp = j*j*j;
-
-
- //Enable interrupts
- sei();
-
- // loop
- while (1) {
- if (stop) {
- cli();
- OCR1BL = 0xff;
- }
- }
-}
diff --git a/linear_pendulum_2009/avr/current_control_final.c b/linear_pendulum_2009/avr/current_control_final.c
index 58f9668a0ea2c0a5bd4d4c628ae43eda4f959d9f..2a6b86828c05f4212301120b83ece47b00c905b3 100644
--- a/linear_pendulum_2009/avr/current_control_final.c
+++ b/linear_pendulum_2009/avr/current_control_final.c
@@ -1,3 +1,9 @@
+/***************************************************************
+
+ Current regulation - Pontus Giselsson, Per-Ola Larsson 18/02/09
+ for LTH - reglerteknik
+
+/***************************************************************
#include <avr/twi.h>
#include <avr/io.h>
#include <avr/signal.h>
@@ -6,27 +12,24 @@
// control variables
-volatile int16_t ref=0; // reference value (from velocity controller)
-volatile int16_t y; // measurement, 9 frac bits
-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 = 111; // 7 frac bits
-volatile int16_t Ke = 38; // 7 frac bits, K*h/Ti
-volatile int8_t intCond = 0;
-
-
-#define V_MAX 508
+volatile int16_t ref=0; // reference value (from velocity controller)
+volatile int16_t y; // measurement, 9 frac bits
+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; // saturated 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 (8 bits)
+volatile int16_t K = 111; // 7 frac bits
+volatile int16_t Ke = 38; // 7 frac bits, K*h/Ti
+volatile int8_t intCond = 0; // flag for conditional integration
+
+#define V_MAX 508 //max/min for saturation
#define V_MIN -508
-
// twi variable
volatile int16_t status;
-
// logging variables
/*
#define log_len 100
@@ -39,9 +42,6 @@ volatile int16_t jj=0;
volatile int8_t stop = 0;
*/
-
-
-
/* Routine used to transmit bytes on the serial port */
/*
static void putchar(unsigned char ch)
@@ -80,7 +80,6 @@ static inline void sendData() {
/* Timer 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */
SIGNAL(SIG_OUTPUT_COMPARE2) {
- PORTB |= 0x40;
// Start AD conversion
ADCSRA |= BV(ADSC);
@@ -159,7 +158,6 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
}
*/
- PORTB &= ~0x40;
}
@@ -175,7 +173,7 @@ int main()
outp(0x40,DDRB); // temp pwm output
/* Timer section */
- // Timer 1, fast PWM no prescaling (non-inverting mode (start low, switch to high))
+ // Timer 1, fast PWM no prescaling (non-inverting mode (start high, switch to low))
outp(BV(COM1B1)|BV(WGM11)|BV(WGM10),TCCR1A);
outp(BV(CS10)|BV(WGM13)|BV(WGM12),TCCR1B);
diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
index 6b3d6222e377a88ac517a5d8326b8ac482ccaffe..ca125ae2938d79e7b8c16f4db9edb718a39ca376 100644
--- a/linear_pendulum_2009/avr/vel_control.c
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -24,8 +24,8 @@ volatile int32_t e = 0; // 11 frac bits
volatile int8_t intCond = 0;
-volatile int32_t K = 800; // 6 frac bits, prop constant
-volatile int32_t Ke = 30; // 6 frac bits, integral constant
+volatile int32_t K = 807; // 6 frac bits, prop constant
+volatile int32_t Ke = 13; // 6 frac bits, integral constant
volatile int8_t fr_comp = (10<<3);
#define V_MAX (120<<4)
#define V_MIN (-120<<4)
@@ -56,7 +56,6 @@ 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) */
int32_t getPosition() {
cli();
@@ -136,6 +135,8 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
TIMSK &= ~(BV(OCIE0)|BV(OCIE1A)); // Disable communication and ctrl-interrupts
sei(); // enable interrupts from encoder-counter
+
+
// Poll UART receiver
uint8_t status = UCSRA;
@@ -153,7 +154,9 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
int16_t toSend = pccom_getNextByteToSend();
if (toSend >= 0) UDR = (char)toSend;
}
+
+
TIFR = (1<<OCF0); // skip pending interrupts from serial comm, (but not from ctrl)
TIMSK |= (BV(OCIE0)|BV(OCIE1A)); // reenable communication and ctrl-interrupts
@@ -221,11 +224,16 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
uSend = u;
sei();
+
+
// 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);
@@ -235,8 +243,7 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
outp((int8_t)u,TWDR); // send 8 bits reference
outp(BV(TWINT)|BV(TWEN),TWCR);
while (!(TWCR&BV(TWINT))) {}
-
-
+
// stop transmission
outp(BV(TWINT)|BV(TWEN)|BV(TWSTO),TWCR);
@@ -272,10 +279,11 @@ int main()
outp(0,TCNT1L);
- /* Timer 0, 10 kHz, Prescaler 8, serial communication */
+ /* Timer 0, 40 kHz, Prescaler 8, serial communication */
outp(BV(WGM01)|BV(CS01),TCCR0);
- outp(184-1,OCR0); // 10 kHz
- /* Reset timer 0 */
+ //outp(184-1,OCR0); // 10 kHz
+ outp(46-1,OCR0); // 40 kHz
+ /* Reset timer 0 */
outp(0,TCNT0);