diff --git a/linear_pendulum_2009/avr/compile.sh b/linear_pendulum_2009/avr/compile.sh
index 2fdb97c3223e09e8343c5c6d0d78ce3c3dfc777d..1ad797813b1ad18a20d7909948f27d4fab3eda9c 100755
--- a/linear_pendulum_2009/avr/compile.sh
+++ b/linear_pendulum_2009/avr/compile.sh
@@ -1 +1 @@
-avr-gcc -mmcu=atmega16 -g -Wall -o current_control current_control.c
\ No newline at end of file
+avr-gcc -mmcu=atmega16 -g -Wall -o current_control_final current_control_final.c
\ No newline at end of file
diff --git a/linear_pendulum_2009/avr/compileUpload.sh b/linear_pendulum_2009/avr/compileUpload.sh
index e97cfdfa62c342c42bb1305d2a7bf9a7c07d00fd..56e4b6568e3b8d69ae00125146362a087fd9c16d 100755
--- a/linear_pendulum_2009/avr/compileUpload.sh
+++ b/linear_pendulum_2009/avr/compileUpload.sh
@@ -1,3 +1,3 @@
-avr-gcc -mmcu=atmega16 -g -Wall -o current_control current_control.c
-avr-objcopy -Osrec current_control current_control.sr
-uisp -dprog=stk200 --erase --upload if=current_control.sr
\ No newline at end of file
+avr-gcc -mmcu=atmega16 -g -Wall -o current_control_final current_control_final.c
+avr-objcopy -Osrec current_control_final current_control_final.sr
+uisp -dprog=stk200 --erase --upload if=current_control_final.sr
\ No newline at end of file
diff --git a/linear_pendulum_2009/avr/current_control.c b/linear_pendulum_2009/avr/current_control.c
index ac8d49b1b6a6a41a1f2b0bb083d8eeb65acf118d..adb0f97160f8068472aec7055d75f8bf6c3d0977 100644
--- a/linear_pendulum_2009/avr/current_control.c
+++ b/linear_pendulum_2009/avr/current_control.c
@@ -9,7 +9,6 @@
*********************************************/
-//#include <math.h> //Only include if sin will be used, takes up memory!
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
@@ -17,7 +16,7 @@
// reference generation variables
-volatile int16_t ref = 200;
+volatile int16_t ref = 100;
volatile int16_t refCount = 0;
volatile int8_t refFlag = 0;
@@ -54,14 +53,6 @@ volatile int16_t temp;
volatile int8_t pwmCycles = 0;
-/* Routine used to set the red LED */
-void setLED(uint8_t on)
-{
- if (on) PORTB &= 0x7f; //Turn on
- else PORTB |= 0x80; //Turn off
-}
-
-
/* Routine used to transmit bytes on the serial port */
static void putchar(unsigned char ch)
{
@@ -141,7 +132,6 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
//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
@@ -166,7 +156,6 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
// 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
@@ -210,28 +199,28 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
/* reference square- or triangle wave generator with timer 0 */
SIGNAL(SIG_OVERFLOW0) {
- int8_t rectangle = 0;
+ int8_t rectangle = 1;
refCount++;
if (rectangle == 1) {
if (refFlag == 0) {
- if (refCount == 10) {
+ if (refCount == 20) {
refFlag = 1;
ref = -ref;
refCount = 0;
}
} else {
- if (refCount == 20) {
+ if (refCount == 40) {
ref = -ref;
refCount = 0;
}
}
} else {
- if (refCount <= 40) { // ref
- ref -= 10;
+ if (refCount <= 200) { // ref
+ ref -= 1;
} else {
- ref += 10;
+ ref += 1;
}
- if (refCount == 80) { // ref*2
+ if (refCount == 400) { // ref*2
refCount = 0;
}
}
@@ -244,11 +233,8 @@ int main()
int i,j;
//Port directions
- outp(0x80,PORTB); // LED off
- outp(0x80,DDRB); // output on LED
outp(0x08,PORTC); // pull up on overtemp signals
outp(0xa0,DDRC); // output on dir and brake
- outp(0x80,PORTD); // pull up on reset switch
outp(0x10,DDRD); // output on pwm for motor 1
outp(0x10,DDRA); // test pin output
diff --git a/linear_pendulum_2009/avr/current_control_final.c b/linear_pendulum_2009/avr/current_control_final.c
new file mode 100644
index 0000000000000000000000000000000000000000..5d52f9a51db1ff927866e92d0d6738d90a69e9ac
--- /dev/null
+++ b/linear_pendulum_2009/avr/current_control_final.c
@@ -0,0 +1,169 @@
+#include <avr/twi.h>
+#include <avr/io.h>
+#include <avr/signal.h>
+#include <avr/interrupt.h>
+#include <inttypes.h>
+
+
+// control variables
+volatile int16_t ref=0;
+volatile unsigned char lbyte,hbyte;
+volatile int16_t y;
+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 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
+
+
+// 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;
+
+// twi variables
+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){}
+
+
+/* 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);
+
+ 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
+ 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);
+ } else if (v < -512) {
+ I = I + (((Ke*e) + (Ksat)*(-512-v))>>3);
+ } else {
+ I = I + ((Ke*e)>>3);
+ }
+
+ // 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;
+ }
+ //u = 0;
+ // set pwm switching time
+ if (u < 0) {
+ PORTC |= 0x80;
+ OCR1BL = (unsigned char) (128-(-u));
+ } else {
+ PORTC = (PORTC & 0x7F);
+ OCR1BL = (unsigned char) (127-u);
+ }
+
+ // TWI-communication (need status == 0x60?)
+ if ((BV(TWINT)&inp(TWCR))) {
+ status = (inp(TWSR)&0xf8);
+ if (status == 0x80) {
+ ref = (int16_t)((int8_t)(inp(TWDR))); // read reference
+ ref = ref << 2; // shifted 2 steps for 10 bits for reference
+ }
+ else {
+ }
+ outp(BV(TWINT)|BV(TWEN)|BV(TWEA),TWCR);
+ }
+
+ PORTB &= 0xbf; //Turn off calculation timer
+
+}
+
+
+int main()
+{
+ 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(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);
+ 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)
+ outp(0x00,OCR1AH);
+ outp(0x7f,OCR1AL);
+ outp(0x00,TCNT1H);
+ outp(0x00,TCNT1L);
+ 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);
+
+ //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 */
+ outp(BV(REFS0)|BV(MUX0),ADMUX);
+
+ // Enable ADC, start first conversion, prescaler 32, not free running mode
+ outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
+
+
+ // Initialize TWI
+ outp(0x02,TWBR); // set SCL-frequency CPU-freq/(16+2*2)
+ outp(0x02,TWAR); // slave address
+ outp(BV(TWEA)|BV(TWEN),TWCR); // enable TWI, enable ACK
+
+
+
+
+ //Enable interrupts
+ sei();
+
+ // loop
+ while (1) {}
+}