diff --git a/linear_pendulum_2009/avr/Makefile b/linear_pendulum_2009/avr/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..91fe37a78cda4bb5a89de1e7998ec604d9a2c05d
--- /dev/null
+++ b/linear_pendulum_2009/avr/Makefile
@@ -0,0 +1,81 @@
+PROJECT=current_control
+CHIP=atmega16
+
+ARCH=avr
+AVRSOURCE=
+AVRAUTO=compiled/$(ARCH)/
+AVRARCH=compiled/$(ARCH)/
+LIB_TARGET=$(AVRARCH)lib$(TARGET).a
+
+CC=$(ARCH)-gcc
+CXX=$(ARCH)-gcc
+CCFLAGS=-mmcu=$(CHIP) -O5 -I. -I$(AVRAUTO)
+AS=$(ARCH)-as
+ASFLAGS=-mmcu=$(CHIP)
+AR=$(ARCH)-ar
+LD=$(ARCH)-ld
+OBJDUMP=$(ARCH)-objdump
+LDFLAGS= -T $(CHIP).link
+
+all: $(AVRARCH)$(PROJECT)
+
+$(LIB_TARGET)(%.o): $(AVRSOURCE)%.c Makefile
+ $(CC) $(CCFLAGS) -c -o $(AVRARCH)$(%) -I$(AVRSOURCE) -I$(AVRAUTO) $(<)
+ $(AR) r $(@) $(AVRARCH)$(%)
+ rm $(AVRARCH)$(%)
+
+$(LIB_TARGET)(%.o): $(AVRSOURCE)%.cc Makefile
+ $(CXX) $(CCFLAGS) -c -o $(AVRARCH)$(%) -I$(AVRSOURCE) -I$(AVRAUTO) $(<)
+ $(AR) r $(@) $(AVRARCH)$(%)
+ rm $(AVRARCH)$(%)
+
+$(LIB_TARGET)(%.o): $(AVRSOURCE)%.s Makefile
+ $(AS) $(ASFLAGS) -o $(AVRARCH)$(%) $(AVRSOURCE)$(*).s
+ $(AR) r $(@) $(AVRARCH)$(%)
+ rm $(AVRARCH)$(%)
+
+$(AVRARCH)%.o: $(AVRSOURCE)%.s Makefile
+ $(AS) $(ASFLAGS) -o $@ $(AVRSOURCE)$(*).s
+
+$(AVRARCH)%.o: $(AVRSOURCE)%.c Makefile
+ $(CC) $(CCFLAGS) -c -o $@ -I. -I$(AVRAUTO) $<
+
+
+$(AVRARCH)%: $(AVRARCH)%.o Makefile
+ $(CC) $(CCFLAGS) -o $@ $< -lm
+
+$(AVRARCH)%.sr: all $(AVRARCH)%
+ avr-objcopy -O srec $(AVRARCH)/$(*) $@
+
+%.sr: $(AVRARCH)%.sr
+ @/bin/true
+
+%.load: compiled/avr/%.sr
+# /work/andersb/atmel/uisp/uisp-0.2b/src/uisp \
+# -dno-poll -dstk200 --erase --upload if=$(AVRARCH)/$(*).sr
+ uisp \
+ -dprog=stk200 \
+ --erase \
+ --upload if=compiled/avr/$*.sr
+
+%.load.stk500: compiled/avr/%.sr
+ uisp \
+ -dprog=stk500 \
+ --erase \
+ --upload if=compiled/avr/$*.sr
+
+%.dump: all $(AVRARCH)/%
+ $(OBJDUMP) -D $(AVRARCH)/$(*)
+
+$(AVRARCH)fuse.sr: fuse.s
+ avr-gcc -o $(AVRARCH)fuse.o -c fuse.s
+ avr-objcopy -O srec $(AVRARCH)fuse.o $(AVRARCH)fuse.sr
+
+fuse.load: $(AVRARCH)fuse.sr
+ uisp \
+ -dprog=stk200 \
+ --erase \
+ --segment=fuse \
+ --upload if=compiled/avr/fuse.sr
+
+load.stk500: $(PROJECT).load.stk500
\ No newline at end of file
diff --git a/linear_pendulum_2009/avr/compileUploadCurrentControl.sh b/linear_pendulum_2009/avr/compileUploadCurrentControl.sh
new file mode 100755
index 0000000000000000000000000000000000000000..e97cfdfa62c342c42bb1305d2a7bf9a7c07d00fd
--- /dev/null
+++ b/linear_pendulum_2009/avr/compileUploadCurrentControl.sh
@@ -0,0 +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
diff --git a/linear_pendulum_2009/avr/compileUploadVelControl.sh b/linear_pendulum_2009/avr/compileUploadVelControl.sh
new file mode 100755
index 0000000000000000000000000000000000000000..bad2b4b7d48faaecc7859da4f6633d7afcc1dbd8
--- /dev/null
+++ b/linear_pendulum_2009/avr/compileUploadVelControl.sh
@@ -0,0 +1,3 @@
+avr-gcc -mmcu=atmega16 -O -g -Wall -o vel_control.o vel_control.c pccom.c
+avr-objcopy -Osrec vel_control.o vel_control.sr
+uisp -dprog=stk200 --erase --upload if=vel_control.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
new file mode 100644
index 0000000000000000000000000000000000000000..5736fce012fb15b4b8fe9d0aa49e60937ae30168
--- /dev/null
+++ b/linear_pendulum_2009/avr/current_control.c
@@ -0,0 +1,225 @@
+/*
+***************************************************************
+
+ 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>
+#include <avr/interrupt.h>
+#include <inttypes.h>
+
+
+// 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; // 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
+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;
+*/
+
+/* 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) {};
+}
+*/
+
+/* 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 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */
+SIGNAL(SIG_OUTPUT_COMPARE2) {
+
+ // Start AD conversion
+ ADCSRA |= BV(ADSC);
+
+ // Read previous AD-conversion result
+ low = inp(ADCL);
+ high = inp(ADCH);
+ y = ((int16_t)((high<<8) | low)) - 512; // y 9 frac bits
+
+ // control error
+ e = ref-y; // e 9 frac bits
+
+ // temporary ctrl-signal
+ v = (((K*e+64)>>7)+((I+8)>>4));
+
+ // variable that decides if I-part should be updated
+ intCond = 1;
+
+ // saturation and update integral part of ctrl with antiwindup
+ if (v > V_MAX) {
+ vSat = V_MAX;
+ if (e > 0)
+ intCond = 0;
+ } else if (v < V_MIN) {
+ vSat = V_MIN;
+ if (e < 0)
+ intCond = 0;
+ } else {
+ vSat = v;
+ }
+
+ if (intCond)
+ I = I + (((Ke*e)+(1<<2))>>3);
+
+ // ctrl signal, 7 bits + direction
+ u = ((vSat+2)>>2); //7 frac bits to pwm
+
+ // set pwm switching time
+ if (u < 0) {
+ PORTC |= 0x80; // set direction of motion
+ OCR1BL = ((unsigned char) (-u)); // set length of pwm-high
+ } else {
+ PORTC &= 0x7f; // set direction of motion
+ OCR1BL = ((unsigned char) (u)); // set length of pwm-high
+ }
+
+
+ // TWI-communication, recieve reference from velocity controller
+ if ((BV(TWINT)&inp(TWCR))) {
+ status = (inp(TWSR)&0xf8);
+ // status 0x80 means data recieved
+ if (status == 0x80) {
+ ref = (int16_t)((int8_t)inp(TWDR)); // read 8 bit reference
+ ref = (ref<<2); // shift up 2 steps for 10 bits reference in loop
+ }
+ else {
+ }
+ outp(BV(TWINT)|BV(TWEN)|BV(TWEA),TWCR);
+ }
+
+ // 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 (might not be needed)
+ cli();
+
+ //Port directions
+ outp(0x08,PORTC); // pull up on overtemperature signals
+ outp(0xa0,DDRC); // output on direction and brake
+ outp(0x10,DDRD); // output on pwm-signal
+ outp(0x40,DDRB); // temp pwm output
+
+ /* Timer section */
+ // 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);
+
+ // Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> 115.2 kHz)
+ 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, 28.8 kHz */
+ outp(BV(WGM21)|BV(CS21),TCCR2);
+ 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(0x18, UCSRB); // USART: 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 measurement */
+ 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) {}
+}
diff --git a/linear_pendulum_2009/avr/pccom.c b/linear_pendulum_2009/avr/pccom.c
new file mode 100644
index 0000000000000000000000000000000000000000..7d074ad8c4617dcc31cf61cbf30011b6cfa6b4e5
--- /dev/null
+++ b/linear_pendulum_2009/avr/pccom.c
@@ -0,0 +1,199 @@
+// pccom.c: Communication interface to PC via serialio_core.
+
+#include "pccom.h"
+#include "serialio_core.h"
+#include "vel_control.h"
+
+/*
+ * Serial I/O assignments
+ *
+ * AO 0 -- Axis 1 motor voltage
+ * A0 1 -- Axis 1 velocity reference !
+ *
+ * EI 0 -- Axis 1 position !
+ * EI 1 -- Axis 1 filtered velocity !
+ * EI 5 -- Axis 1 position with predicted fraction
+ * EI 6 -- Axis 1 position unquantized
+ *
+ * AI 2 -- Axis 1 current
+ * AI 3 -- Pendulum angle !
+ *
+ * AI 4 -- Axis 1 motor voltage (actual)
+ *
+ * DI 0 -- Axis 1 endpoint sensor
+ */
+
+#define POLL_AXIS1_POSITION 0x0001
+#define POLL_AXIS1_VELOCITY 0x0002
+#define POLL_PEND_ANGLE 0x0004
+#define POLL_CURRENT_REFERENCE 0x0008
+//#define POLL_AXIS1_RESET 0x0008
+#define POLL_CONFIG 0x8000
+
+static volatile uint16_t pccom_poll=0;
+
+
+// ---------------------------------- Receiver --------------------------------
+
+// only call from UART receive interrupt
+static inline void addPoll(uint16_t flags) {
+ pccom_poll |= flags;
+}
+
+
+void pccom_receiveByte(char ch)
+{
+ switch (serialio_RXC(ch)) {
+ case serialio_clearbit: {
+ switch (serialio_channel) {
+ }
+ } break;
+ case serialio_setbit: {
+ switch (serialio_channel) {
+ }
+ } break;
+ case serialio_pollbit: {
+ switch (serialio_channel) {
+ //case 0: { addPoll(POLL_AXIS1_RESET); } break;
+ }
+ } break;
+ case serialio_pollchannel: {
+ switch (serialio_channel) {
+ case 0: { addPoll(POLL_AXIS1_POSITION); } break;
+ case 1: { addPoll(POLL_AXIS1_VELOCITY); } break;
+ case 2: { addPoll(POLL_PEND_ANGLE); } break;
+ case 3: { addPoll(POLL_CURRENT_REFERENCE); } break;
+ case 31: { addPoll(POLL_CONFIG); } break;
+ }
+ } break;
+ case serialio_setchannel: {
+ switch (serialio_channel) {
+ case 0: {
+ setRef(serialio_value - (1L<<12));
+ } break;
+ }
+ } break;
+ case serialio_more: {
+ } break;
+
+ case serialio_error: {
+ } break;
+ }
+}
+
+
+// ----------------------------------- Sender ------------------------------
+// return true if more to send
+static uint8_t sendConfigPacket(uint8_t position)
+{
+ switch (position) {
+ case 0: CONF_ANALOG_IN(0, CONF_RESOLUTION(16)); break; // Position (now reference)
+ case 1: CONF_ANALOG_IN(0, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
+ case 2: CONF_ANALOG_IN(0, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
+
+ case 3: CONF_ANALOG_IN(1, CONF_RESOLUTION(18)); break; // Velocity estimate
+ case 4: CONF_ANALOG_IN(1, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
+ case 5: CONF_ANALOG_IN(1, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
+
+ case 6: CONF_ANALOG_IN(2, CONF_RESOLUTION(11)); break; // Pend angle measurement
+ case 7: CONF_ANALOG_IN(2, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
+ case 8: CONF_ANALOG_IN(2, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
+
+ case 9: CONF_ANALOG_IN(3, CONF_RESOLUTION(8)); break; // Current reference
+ case 10: CONF_ANALOG_IN(3, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
+ case 11: CONF_ANALOG_IN(3, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
+
+ case 12: CONF_ANALOG_OUT(0, CONF_RESOLUTION(13)); break; // Reference to vel-ctrl
+ case 13: CONF_ANALOG_OUT(0, CONF_MIN(CONF_NEGATIVE_VOLT(1))); break;
+ case 14: CONF_ANALOG_OUT(0, CONF_MAX(CONF_POSITIVE_VOLT(1))); break;
+
+ default: CONF_END(); return 0;
+ }
+
+ return 1;
+}
+
+static uint8_t sendNextPacket() // returns 1 if a packet was available
+{
+ static int8_t configPosition = -1;
+
+#define toPoll pccom_poll // OK since sender and receiver are mutexed
+
+ // Send _first_ requested item (only one packet!)
+ if (toPoll & POLL_AXIS1_POSITION) {
+ toPoll &= ~POLL_AXIS1_POSITION;
+ serialio_putchannel(0, getPosition()+(1L<<15));
+ }
+ else if (toPoll & POLL_AXIS1_VELOCITY) {
+ toPoll &= ~POLL_AXIS1_VELOCITY;
+ serialio_putchannel(1, getVelocity()+(1L<<17));
+ }
+ else if (toPoll & POLL_PEND_ANGLE) {
+ toPoll &= ~POLL_PEND_ANGLE;
+ serialio_putchannel(2, getAngle()+(1L<<10));
+ }
+ else if (toPoll & POLL_CURRENT_REFERENCE) {
+ toPoll &= ~POLL_CURRENT_REFERENCE;
+ serialio_putchannel(3, getCurrentRef()+(1L<<7));
+ }
+ else if (toPoll & POLL_CONFIG) {
+ if (configPosition < 0) configPosition = 0; // Start sending config?
+ if (!sendConfigPacket(configPosition)) { // Last packet?
+ configPosition = -1;
+ toPoll &= ~POLL_CONFIG;
+ }
+ else configPosition++; // Advance to next packet
+ }
+ else return 0; // should never happen!
+
+ return 1;
+}
+
+// ---- Send buffering ----
+
+#define PCCOM_SEND_BUFFER_SIZE 6 // just enough for one serialio_putchannel() packet
+
+uint8_t pccom_sendBuffer[PCCOM_SEND_BUFFER_SIZE]; // Updated by serialio_putchar()
+uint8_t pccom_sendBufferPosition = 0; // Updated by pccom_getNextByteToSend()
+uint8_t pccom_sendBufferUsed = 0; // Updated by serialio_putchar(),
+ // and pccom_getNextByteToSend() when
+ // the buffer is empty.
+
+void serialio_putchar(unsigned char ch) {
+ if (pccom_sendBufferUsed < PCCOM_SEND_BUFFER_SIZE) {
+ pccom_sendBuffer[pccom_sendBufferUsed] = ch;
+ pccom_sendBufferUsed++;
+ }
+ else {
+ // Buffer already full -- must never happen!
+ // main_emergencyStop(); // show that something is wrong
+ }
+}
+
+int16_t pccom_getNextByteToSend()
+{
+ if (pccom_sendBufferPosition >= pccom_sendBufferUsed) {
+ // Try to refill buffer
+ pccom_sendBufferPosition = 0;
+ pccom_sendBufferUsed = 0;
+
+ //if (!sendNextPacket()) return -1;
+ sendNextPacket();
+ }
+
+ if (pccom_sendBufferPosition >= pccom_sendBufferUsed) return -1; // no data
+ else {
+ // Return next byte
+ uint8_t data = pccom_sendBuffer[pccom_sendBufferPosition];
+ pccom_sendBufferPosition++;
+ return data;
+ }
+}
+
+
+// ------------------------------ Initialization ------------------------------
+
+void pccom_init()
+{
+ serialio_init();
+}
diff --git a/linear_pendulum_2009/avr/pccom.h b/linear_pendulum_2009/avr/pccom.h
new file mode 100644
index 0000000000000000000000000000000000000000..14c4c75d7da795a47f3e884364c2ab03e3e1353e
--- /dev/null
+++ b/linear_pendulum_2009/avr/pccom.h
@@ -0,0 +1,21 @@
+// pccom.h: Communication interface to PC via serialio_core.
+
+#ifndef __pccom_h
+#define __pccom_h
+
+#include <inttypes.h>
+#include "vel_control.h"
+
+void pccom_init();
+
+// Concurrency constraints:
+// Receiving and sending are mutually exclusive!
+void pccom_receiveByte(char ch);
+int16_t pccom_getNextByteToSend(); // returns -1 for nothing to send
+
+
+// ------------------------------- Callbacks ----------------------------------
+
+void serialio_putchar(unsigned char ch);
+
+#endif
diff --git a/linear_pendulum_2009/avr/serialio_core.h b/linear_pendulum_2009/avr/serialio_core.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ca09ae04477744fc4cec9bb34c4def7e70ca5cf
--- /dev/null
+++ b/linear_pendulum_2009/avr/serialio_core.h
@@ -0,0 +1,197 @@
+// serialio_core.h: Serialio protocol without the hardware bindings.
+
+#ifndef __serialio_core_h
+#define __serialio_core_h
+/*
+ * Digital in/out and poll commands are sent as one byte:
+ *
+ * +-+-+-+-+-+-+-+-+
+ * |0|0 0| chan | Bit clear
+ * +-+-+-+-+-+-+-+-+
+ *
+ * +-+-+-+-+-+-+-+-+
+ * |0|0 1| chan | Bit set
+ * +-+-+-+-+-+-+-+-+
+ *
+ * +-+-+-+-+-+-+-+-+
+ * |0|1 0| chan | Bit get
+ * +-+-+-+-+-+-+-+-+
+ *
+ * +-+-+-+-+-+-+-+-+
+ * |0|1 1| chan | Channel get
+ * +-+-+-+-+-+-+-+-+
+ *
+ *
+ * Channels are sent as 2 to 6 bytes, depending on resolution:
+ *
+ * +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
+ * 2 |1| bit8...bit2 | |0|bit| chan |
+ * +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
+ *
+ * +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
+ * 3 |1|bit15...bit9 | |1| bit8...bit2 | |0|bit| chan |
+ * +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
+ *
+ * ...
+ *
+ * +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
+ * 6 |1|bit31...bit30| |1|bit29...bit23| ... |0|bit| chan |
+ * +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+ +-+-+-+-+-+-+-+-+
+ *
+ *
+ *
+ * Channel 31 is special, as it serves as the configuration channel. When
+ * reading from it multiple responses are sent with the following layout
+ *
+ * +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
+ * | command specific data |cmd|kind |conf chan|
+ * +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
+ *
+ * kind: 000 == end of configuration
+ * 001 == digital in
+ * 010 == digital out
+ * 011 == analog in
+ * 100 == analog out
+ * 101 == counter in
+ *
+ *cmd == 0 (Resolution)
+ *
+ * +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
+ * | | # of bits |0 0|kind |conf chan|
+ * +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
+ *
+ * # of bits (1..32)
+ *
+ *cmd == 1 (Minimum value)
+ *
+ * +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
+ * | minimum |S| unit|0 1|kind |conf chan|
+ * +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
+ *
+ * S (sign): 0 == +
+ * 1 == -
+ * unit: 000 == V
+ * 001 == mV
+ * 010 == uV
+ * 100 == A
+ *
+ *cmd == 2 (Maximum value)
+ *
+ * +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
+ * | maximum |S| unit|1 0|kind |conf chan|
+ * +-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-+
+ *
+ * S (sign): 0 == +
+ * 1 == -
+ * unit: 000 == V
+ * 001 == mV
+ * 010 == uV
+ * 100 == A
+ */
+
+// ----------------------------- Interface -----------------------------------
+
+static void serialio_init();
+
+// ---- Receiving ----
+typedef enum {
+ serialio_error, serialio_more, serialio_clearbit, serialio_setbit,
+ serialio_setchannel, serialio_pollbit, serialio_pollchannel
+} serialio_rxc_status;
+
+static serialio_rxc_status serialio_RXC(unsigned char ch);
+
+// ----- Sending -----
+void serialio_putchar(unsigned char ch); // Callback
+
+static void serialio_putbit(unsigned char channel, unsigned char value);
+static void serialio_putchannel(unsigned char channel, unsigned long value);
+
+// ---- Configuration macros ----
+#define CONF_DIG_IN(channel) (0x20 | (channel)&0x1f)
+#define CONF_DIG_OUT(channel) (0x40 | (channel)&0x1f)
+
+#define CONF_END() serialio_putchannel(31, 0)
+#define CONF_DIGITAL_IN(chan, config) \
+ serialio_putchannel(31, (0x20|(chan&0x1f)|(config&0xffffff00)))
+#define CONF_DIGITAL_OUT(chan, config) \
+ serialio_putchannel(31, (0x40|(chan&0x1f)|(config&0xffffff00)))
+#define CONF_ANALOG_IN(chan, config) \
+ serialio_putchannel(31, (0x60|(chan&0x1f)|(config&0xffffff00)))
+#define CONF_ANALOG_OUT(chan, config) \
+ serialio_putchannel(31, (0x80|(chan&0x1f)|(config&0xffffff00)))
+#define CONF_ENCODER_IN(chan, config) \
+ serialio_putchannel(31, (0xa0|(chan&0x1f)|(config&0xffffff00)))
+#define CONF_RESOLUTION(bits) (((bits)<<10)|0x000)
+#define CONF_MIN(value) ((value)|0x100)
+#define CONF_MAX(value) ((value)|0x200)
+#define CONF_NEGATIVE_VOLT(volt) (((long)(volt)<<14)|0x2000)
+#define CONF_POSITIVE_VOLT(volt) ((long)(volt)<<14)
+#define CONF_NEGATIVE_MILLIVOLT(millivolt) (((long)(millivolt)<<14)|0x2400)
+#define CONF_POSITIVE_MILLIVOLT(millivolt) ((long)(millivolt)<<14|0x400)
+#define CONF_POSITIVE_AMPERE(ampere) (((long)(ampere)<<14)|0x1000)
+
+
+// --------------------------- Implementation ---------------------------------
+
+static volatile unsigned long serialio_value;
+static volatile unsigned char serialio_channel, serialio_length;
+
+static void serialio_putbit(unsigned char channel, unsigned char value)
+{
+ if (value) {
+ serialio_putchar(0x20 | (channel & 0x1f));
+ } else {
+ serialio_putchar(0x00 | (channel & 0x1f));
+ }
+}
+
+static void serialio_putchannel(unsigned char channel, unsigned long value)
+{
+ if (value >= (1L<<30)) { serialio_putchar(0x80 | ((value >> 30) & 0x03)); }
+ if (value >= (1L<<23)) { serialio_putchar(0x80 | ((value >> 23) & 0x7f)); }
+ if (value >= (1L<<16)) { serialio_putchar(0x80 | ((value >> 16) & 0x7f)); }
+
+ if (value >= (1L<< 9)) { serialio_putchar(0x80 | ((value >> 9) & 0x7f)); }
+ //serialio_putchar(0x80 | ((value >> 9) & 0x7f)); // DEBUG
+
+ serialio_putchar(0x80 | ((value >> 2) & 0x7f));
+ serialio_putchar(((value << 5) & 0x60) | (channel & 0x1f));
+}
+
+static void serialio_init()
+{
+ serialio_value = 0;
+ serialio_channel = 255;
+ serialio_length = 0;
+}
+
+static serialio_rxc_status serialio_RXC(unsigned char ch) {
+ unsigned char result = serialio_error;
+
+ if (serialio_length == 0) { serialio_value = 0; }
+ serialio_length++;
+ if ((ch & 0x80) == 0x80) {
+ // Collect yet another byte for later processing
+ serialio_value = (serialio_value << 7) | (ch & 0x7f);
+ result = serialio_more;
+ } else {
+ serialio_value = (serialio_value << 2) | ((ch & 0x60) >> 5);
+ serialio_channel = ch & 0x1f;
+ if (serialio_length == 1) {
+ switch (serialio_value & 0x03) {
+ // Digital output buffer (ULN2803A) is inverting
+ case 0: { result = serialio_clearbit; } break;
+ case 1: { result = serialio_setbit; } break;
+ case 2: { result = serialio_pollbit; } break;
+ case 3: { result = serialio_pollchannel; } break;
+ }
+ } else {
+ result = serialio_setchannel;
+ }
+ serialio_length = 0;
+ }
+ return result;
+}
+
+#endif
diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
new file mode 100644
index 0000000000000000000000000000000000000000..ea57e49244a2fb75427ba2343d565d99cadf03bf
--- /dev/null
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -0,0 +1,336 @@
+/*
+**************************************************************
+
+ 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>
+#include <avr/interrupt.h>
+#include <inttypes.h>
+
+#include "pccom.h"
+#include "vel_control.h"
+
+
+// reference variables
+volatile int32_t ref = 0; // 11 frac bits
+volatile int32_t refCtrl = 0; // ref used in ctrl-loop (=ref sent from simulink)
+
+
+// velocity control variables
+volatile int32_t u = 0; // 11 frac bits
+volatile int32_t uSend = 0; // ctrl sent to simulink
+volatile int32_t v = 0; // 11 frac bits
+volatile int32_t vSat = 0;
+volatile int8_t brake = 0; // brake variable if pos-sample missed
+volatile int32_t I = 0; // 11 frac bits
+volatile int32_t e = 0; // 11 frac bits
+volatile int8_t intCond = 0;
+
+
+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)
+
+
+// encoder variables
+#define ENCODERY (PIND&0x04) //Positional encoder pins
+#define ENCODERX ((PINB&0x02)<<1) //Positional encoder pins (shift one step for faster comparison with Y)
+
+// position variables
+volatile int16_t pos = 0; // position in tics
+volatile int16_t posTemp = 0; // position sent to simulink
+volatile int16_t posCtrl = 0; // position used in ctrl-loop
+volatile int16_t oldPos = 0; // previous pos used for velocity estimation
+volatile int8_t newX; // encoder signal
+volatile int8_t newY; // encoder signal
+volatile int8_t oldX; // previous encoder signal
+volatile int8_t oldY; // previous encoder signal
+
+// velocity estimation parameters
+volatile int32_t velEst = 0; // vel-estimate, 5 frac bits
+volatile int32_t velEstSend = 0; // vel-estimate sent to simulink
+int16_t a = 116; // 7 frac bits (parameter in velocity estimate)
+int16_t b = 152; // 5 frac bits (parameter in velocity estimate)
+
+// adc measurement variables
+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();
+ posTemp = pos;
+ sei();
+ return ((int32_t) posTemp);
+}
+
+
+/* return velocity (in mm/s) */
+int32_t getVelocity() {
+ return velEstSend;
+}
+
+
+/* return last angle measurement */
+int16_t getAngle() {
+ low = inp(ADCL);
+ high = inp(ADCH);
+ return (((int16_t) ((high<<8) | low) - 512)-angleOffset);
+}
+
+
+/* return current-reference */
+int32_t getCurrentRef() {
+ return uSend;
+}
+
+
+/* Set new reference value */
+void setRef(int32_t newRef) {
+ ref = newRef;
+}
+
+/* Routine used to initialize the positional encoders */
+void initPos()
+{
+ oldX = ENCODERX;
+ oldY = ENCODERY;
+}
+
+/* Timer 2, Encoder counter, 73 kHz updates position variable */
+SIGNAL(SIG_OUTPUT_COMPARE2) {
+
+ // Update position from encoder
+ newX = ENCODERX;
+ newY = ENCODERY;
+ if((newX != oldX) || (newY != oldY)) { //Check if any value changed
+ /*
+ sum = (oldX<<2)+oldY+newX+(newY>>2);
+ if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
+ pos = pos+1;
+ } else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) {
+ pos = pos-1;
+ } else {
+ brake = 1; // emergency brake
+ }
+ */
+ // Works like if-clauses above, but faster!
+ if ((oldX == newY) && (oldY != newX)) {
+ pos = pos+1;
+ } else if ((oldX != newY) && (oldY == newX)) {
+ pos = pos-1;
+ } else {
+ brake = 1;
+ }
+ oldX = newX;
+ oldY = newY;
+ }
+
+}
+
+
+/* Timer 0, serial communication with simulink */
+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;
+ 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;
+ }
+
+ TIFR = (1<<OCF0); // skip pending interrupts from serial comm, (but not from ctrl)
+
+ TIMSK |= (BV(OCIE0)|BV(OCIE1A)); // reenable communication and ctrl-interrupts
+
+}
+
+
+
+/* Timer 0, control loop , 1 kHz */
+SIGNAL(SIG_OUTPUT_COMPARE1A) {
+
+ posCtrl = pos; // store pos to use in this loop
+
+ sei(); // to enable interupts from encoder counter and communication
+
+ // velocity estimation in mm/s
+ velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst
+ oldPos = posCtrl;
+
+ // store velEst and ref to be sent/used here
+ cli();
+ velEstSend = velEst;
+ refCtrl = ref;
+ // ref = ref*(1-brake); // emergency stop
+ sei();
+
+ // control error
+ e = refCtrl-((velEst+16)>>5); // mm/s
+
+ // temporary ctrl-signal
+ v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
+
+ // friction compensation
+ if (refCtrl > 0) {
+ v = v+fr_comp;
+ } else if (refCtrl < 0) {
+ v = v-fr_comp;
+ }
+
+ // variable that decides if I-part should be updated
+ intCond = 1;
+
+ // saturation of v
+ if (v > V_MAX) {
+ vSat = V_MAX;
+ if (e > 0)
+ intCond = 0;
+ } else if (v < V_MIN) {
+ vSat = V_MIN;
+ if (e < 0)
+ intCond = 0;
+ } else {
+ vSat = v;
+ }
+
+ if (intCond)
+ I = I + (((Ke*e)+(1<<1))>>2);
+
+ // scale ctrl-signal to send over twi
+ u = (vSat+8)>>4; // u=127 gives current = 6.75 A, vSat makes u saturate at 114
+
+ // u that is sent to simulink
+ cli();
+ 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);
+ while (!(TWCR&BV(TWINT))) {}
+
+ // Send reference byte to slave
+ 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);
+
+}
+
+
+int main()
+{
+ cli();
+
+ //Port directions
+ //outp(0x80,DDRB); // Led output
+ outp(0x10,DDRC); // timer calculation port
+ PORTD = 0x40; // pull up on reset switch
+
+
+ /* Timer section */
+ // Enable timer0, timer1, timer2 compare match interrupts
+ outp(BV(OCIE0)|BV(OCIE1A)|BV(OCIE2),TIMSK);
+
+ /* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */
+ outp(BV(WGM21)|BV(CS20),TCCR2);
+ outp(200,OCR2);
+ /* Reset timer 2 */
+ outp(0,TCNT2);
+
+ /* Timer 1, 1 kHz , prescaler 1, ctrl-loop */
+ outp(BV(WGM12)|BV(CS10),TCCR1B);
+ outp(0x39,OCR1AH);
+ outp(0x7f,OCR1AL);
+ outp(0,TCNT1H);
+ outp(0,TCNT1L);
+
+
+ /* Timer 0, 40 kHz, Prescaler 8, serial communication */
+ outp(BV(WGM01)|BV(CS01),TCCR0);
+ //outp(184-1,OCR0); // 10 kHz
+ outp(46-1,OCR0); // 40 kHz
+ /* Reset timer 0 */
+ outp(0,TCNT0);
+
+
+ // syncronization (ctrl interrupt 34 micros before communication interrupt)
+ TCNT1 = TCNT0*8+500;
+
+
+ //Serial communication initialization
+ outp(0x00, UCSRA); // USART:
+ outp(0x18, UCSRB); // USART: 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, ADC3 for pendulum angle */
+ outp(BV(REFS0)|BV(MUX0)|BV(MUX1),ADMUX);
+
+ // Enable ADC on adc3, start first conversion, prescaler 128, free running mode
+ outp(BV(ADEN)|BV(ADSC)|BV(ADATE)|BV(ADPS2)|BV(ADPS1)|BV(ADPS0),ADCSRA);
+
+
+ // Initialize Master TWI
+ outp(0x10,TWBR); // set SCL-frequency CPU-freq/(16+2*16)
+ outp(BV(TWEN),TWCR); // enable TWI
+
+ // initialize position measurements
+ initPos();
+
+ // initialize pc-communication
+ pccom_init();
+
+ //Enable interrupts
+ sei();
+
+ // loop
+ while (1) {
+ // reset position, velocity estimate and integral part of ctrl if reset button pushed
+ if (!(PIND & 0x40)) {
+ cli();
+ low = inp(ADCL);
+ high = inp(ADCH);
+ pos = 0; // reset position
+ angleOffset = ((int16_t) ((high<<8) | low) - 512);
+ oldPos = 0;
+ velEst = 0; // reset velocity estimate
+ I = 0; // reset integral part of controller
+ u = 0; // reset ctrl signal
+ sei();
+ }
+ }
+}
diff --git a/linear_pendulum_2009/avr/vel_control.h b/linear_pendulum_2009/avr/vel_control.h
new file mode 100644
index 0000000000000000000000000000000000000000..88d833be44856a97496eb28c92d4b9324f451871
--- /dev/null
+++ b/linear_pendulum_2009/avr/vel_control.h
@@ -0,0 +1,11 @@
+#ifndef __vel_control_h
+#define __vel_control_h
+
+
+void setRef(int32_t newRef);
+int32_t getPosition();
+int32_t getVelocity();
+int16_t getAngle();
+int32_t getCurrentRef();
+
+#endif
diff --git a/linear_pendulum_2009/doc/2xcurrent sensor PCB.pdf b/linear_pendulum_2009/doc/2xcurrent sensor PCB.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..eb5ba64e0e56b6f58b228a1a4f98aca8380a3970
Binary files /dev/null and b/linear_pendulum_2009/doc/2xcurrent sensor PCB.pdf differ
diff --git a/linear_pendulum_2009/doc/2xcurrent sensor SCH.pdf b/linear_pendulum_2009/doc/2xcurrent sensor SCH.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..93a5481637f8cb2cfc4ee527388b20f0e0c6491a
Binary files /dev/null and b/linear_pendulum_2009/doc/2xcurrent sensor SCH.pdf differ
diff --git a/linear_pendulum_2009/doc/ACS712.pdf b/linear_pendulum_2009/doc/ACS712.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..7bbecd345ade3a2c4af23eb5d9ce5d9a39d8bd5d
Binary files /dev/null and b/linear_pendulum_2009/doc/ACS712.pdf differ
diff --git a/linear_pendulum_2009/doc/Atmega 16 connectors.pdf b/linear_pendulum_2009/doc/Atmega 16 connectors.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..e85ae8bd2509bb3a81da8861512b7b0c8db14c43
Binary files /dev/null and b/linear_pendulum_2009/doc/Atmega 16 connectors.pdf differ
diff --git a/linear_pendulum_2009/doc/Atmega16 dual powerdrive pendel 2008.pdf b/linear_pendulum_2009/doc/Atmega16 dual powerdrive pendel 2008.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..970c61998a2408f8b7912cef3651e5a760667217
Binary files /dev/null and b/linear_pendulum_2009/doc/Atmega16 dual powerdrive pendel 2008.pdf differ