diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
new file mode 100644
index 0000000000000000000000000000000000000000..17e6e1f192e4e2a58fb7f1188771ddb5f7382f4e
--- /dev/null
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -0,0 +1,296 @@
+#include <avr/twi.h>
+#include <avr/io.h>
+#include <avr/signal.h>
+#include <avr/interrupt.h>
+#include <inttypes.h>
+
+
+// reference variables
+volatile int32_t ref = 0; // 11 frac bits
+volatile int16_t refFlag = 0;
+volatile int16_t deltaRef = 2;
+volatile int16_t refCount = 0;
+
+
+// velocity control variables
+volatile int32_t u = 0; // 11 frac bits
+volatile int32_t v = 0; // 11 frac bits
+volatile int8_t brake = 0;
+volatile int32_t I = 0; // 11 frac bits
+volatile int32_t e = 0; // 11 frac bits
+volatile int32_t K = 1200; // 6 frac bits
+volatile int32_t Ke = 45; // 6 frac bits
+volatile int32_t Ksat = 3; // 6 frac bits
+
+// encoder variables
+#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 oldPos = 0;
+volatile int16_t deltaPos = 0;
+volatile int8_t newX;
+volatile int8_t newY;
+volatile int8_t oldX;
+volatile int8_t oldY;
+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() {
+
+}
+
+
+
+/* Routine used to set the red LED */
+void setLED(uint8_t on)
+{
+ if (on) PORTB &= ~0x80; //Turn on
+ else PORTB |= 0x80; //Turn off
+}
+
+/* Routine used to initialize the positional encoders */
+void initPos()
+{
+ oldX = ENCODERX;
+ oldY = ENCODERY;
+}
+
+/* Routine used to track the cart position */
+void setPos()
+{
+
+}
+
+/* Timer 2, Encoder */
+SIGNAL(SIG_OUTPUT_COMPARE2) {
+
+ deltaPos = 0;
+ // 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); //Find state
+ if (sum == 2 || sum == 4 || sum == 11 || sum == 13) { //Predetermined values determine direction
+ //pos = pos+1;
+ deltaPos = 1;
+ } else if (sum == 1 || sum == 7 || sum == 8 || sum == 14) {
+ //pos = pos-1;
+ deltaPos = -1;
+ } else {
+ brake = 1;
+ // emergency brake
+ }
+ oldX = newX;
+ oldY = newY;
+ }
+
+
+ // velocity estimation cut-off frequency 500 Hz
+ pos = pos+deltaPos; // update oldPos, 0 frac bits on pos and oldPos
+
+}
+
+
+
+/* Timer 0, control loop */
+SIGNAL(SIG_OUTPUT_COMPARE0) {
+
+ sei(); // to enable interupts from timer2
+
+ PORTC |= 0x10; // to clock calulation time
+
+ // linear velocity estimator
+ // velocity estimate in mm/s
+ velEst = (((a*velEst+64)>>7)+b*(pos-oldPos)); // 5 fracbits on velEst
+ oldPos = pos;
+
+ // control error
+ //e = ref-((int16_t)((velEst+16)>>5)); // mm/s
+ e = ref-((velEst+16)>>5); // mm/s
+
+ v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
+
+ //saturation and update integral part of ctrl
+ if (v > 2047) {
+ I = I + ((((Ke*e) + (Ksat)*(2047-v))+(1<<1))>>2);
+ } else if (v < -2048) {
+ I = I + ((((Ke*e) + (Ksat)*(-2048-v))+(1<<1))>>2);
+ } else {
+ I = I + ((Ke*e+(1<<3))>>2);
+ }
+
+ u = (v+8)>>4; //8 frac bits to current loop
+
+ // friction compensation
+ if (ref > 0) {
+ u = u+10;
+ } else if (ref < 0) {
+ u = u-10;
+ }
+
+ // Saturation
+ if (u > 127) {
+ u = 127;
+ } else if (u < -128) {
+ u = -128;
+ }
+
+ // reference calculations
+ refCount++;
+
+
+ if (refFlag == 0) {
+ if (refCount%1 == 0)
+ ref = ref+deltaRef;
+ if (refCount == 500) {
+ refFlag = 1;
+ deltaRef = -deltaRef;
+ refCount = 0;
+ }
+ } else {
+ if (refCount%1 == 0)
+ ref = ref+deltaRef;
+ if (refCount == 1000) {
+ refCount = 0;
+ deltaRef = -deltaRef;
+ }
+ }
+
+
+ /*
+ if (refCount == 1000) {
+ ref = 0;
+ }
+ */
+
+
+ pos = pos*(1-brake);
+
+
+ // TWI-communication
+ // 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
+ outp((int8_t)(u&0x000000ff),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);
+
+
+
+ //velEstTemp = velEst;
+ /*
+ putchar((unsigned char) ((((velEstTemp+16)>>5)&0x0000ff00)>>8));
+ putchar((unsigned char) (((velEstTemp+16)>>5)&0x000000ff));
+
+ //putchar((unsigned char) ((deltaPos&0xff00)>>8));
+ //putchar((unsigned char) (deltaPos&0x00ff));
+ */
+
+ putchar((unsigned char) ((velEst&0xff000000)>>24));
+ putchar((unsigned char) ((velEst&0x00ff0000)>>16));
+
+ putchar((unsigned char) ((velEst&0x0000ff00)>>8));
+ putchar((unsigned char) (velEst&0x000000ff));
+
+ putchar((unsigned char) ((I&0xff000000)>>24));
+ putchar((unsigned char) ((I&0x00ff0000)>>16));
+
+ putchar((unsigned char) ((I&0x0000ff00)>>8));
+ putchar((unsigned char) (I&0x000000ff));
+
+
+ PORTC &= ~0x10;
+
+}
+
+
+int main()
+{
+ cli();
+
+ //Port directions
+ outp(0x80,DDRB); // Led output
+ outp(0x10,DDRC); // timer calculation port
+
+ /* Timer section */
+ // Enable timer2 compare match interrupts
+ outp(BV(OCIE0)|BV(OCIE2),TIMSK);
+
+ /* Timer 2, 59 kHz Prescaler 1 */
+ outp(BV(WGM21)|BV(CS20),TCCR2);
+ outp(200,OCR2);
+ /* Reset timer 2 */
+ outp(0,TCNT2);
+
+ /* Timer 0, 1 kHz Prescaler 64 */
+ outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
+ outp(230,OCR0);
+ /* Reset timer 0 */
+ outp(0,TCNT0);
+
+
+ //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 Master TWI
+ outp(0x10,TWBR); // set SCL-frequency CPU-freq/(16+2*16)
+ outp(BV(TWEN),TWCR); // enable TWI
+
+ // initialize position measurements
+ initPos();
+
+ //Enable interrupts
+ sei();
+
+ // loop
+ while (1) {}
+}