diff --git a/linear_pendulum_2009/avr/current_control_final.c b/linear_pendulum_2009/avr/current_control_final.c
index 2c0f1bd87476cb98a087c33ed43e24d18763ad67..0da8fa188fec5c285e762388c160a87d558612fd 100644
--- a/linear_pendulum_2009/avr/current_control_final.c
+++ b/linear_pendulum_2009/avr/current_control_final.c
@@ -8,14 +8,21 @@
 // control variables
 volatile int16_t ref=0;   // reference value (from velocity controller)
 volatile int16_t y;       // measurement, 9 frac bits
-volatile uint8_t low, high;  // when reading AD-conversion
+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 = 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
+volatile int8_t intCond = 0;
+
+
+#define V_MAX 508
+#define V_MIN -508
+
 
 // twi variable
 volatile int16_t status;
@@ -47,7 +54,7 @@ static void putchar(unsigned char ch)
 */
 
 /* Send logged data over Serial connection */
- /*
+/*
 static inline void sendData() {
   int16_t ii = 0;
   while (ii < log_len) {
@@ -64,65 +71,72 @@ static inline void sendData() {
     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) {
+
+  PORTB |= 0x40;
   // Start AD conversion
   ADCSRA |= BV(ADSC);
   
   // Read previous AD-conversion result
   low = inp(ADCL);
   high = inp(ADCH);
-  y =  ((high<<8) | low) - 512; //y 9 frac bits
-  
-  //control error, (+) since negative measurements
-  e = ref+y; //e 9 frac bits
+  y = ((int16_t)((high<<8) | low)) - 512; // y 9 frac bits
+
+  y = ((y*3)>>1);
+
+  // control error
+  e = ref-y; // e 9 frac bits
   
   // temporary ctrl-signal
-  v = (int16_t)(((K*e+64)>>7)+(I>>4));
-
-  //saturation and update integral part of ctrl with antiwindup
-  if (v > 511) {
-    I = I + ((((Ke*e) + (Ksat)*(511-v))+(1<<2))>>3);
-  } else if (v < -512) {
-    I = I + ((((Ke*e) + (Ksat)*(-512-v))+(1<<2))>>3);
+  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 {
-    I = I + ((Ke*e+(1<<2))>>3);
+    vSat = v;
   }
-  
+
+  if (intCond)
+    I = I + (((Ke*e)+(1<<2))>>3);
+
   // 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 = ((vSat+2)>>2); //7 frac bits to pwm 
 
   // set pwm switching time
   if (u < 0) {
     PORTC |= 0x80;  // set direction of motion
-    OCR1BL = (unsigned char) (128-(-u)); // set length of pwm-high
+    OCR1BL = ((unsigned char) (-u)); // set length of pwm-high
   } else {
-    PORTC = (PORTC & 0x7F); // set direction of motion
-    OCR1BL = (unsigned char) (127-u); // set length of pwm-high
+    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 for reference in loop
+      ref = (int16_t)((int8_t)inp(TWDR)); // read 8 bit reference
+      ref = ref*8;  // shift up 2 steps for 10 bits reference in loop
     } 
     else {
     }
@@ -147,7 +161,8 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
     }
   }
   */
-
+  
+  PORTB &= ~0x40;
 }
 
 
@@ -160,11 +175,11 @@ int main()
   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 (inverting mode (start low, switch to high))
-  outp(BV(COM1A1)|BV(COM1B1)|BV(COM1A0)|BV(COM1B0)|BV(WGM11)|BV(WGM10),TCCR1A);
+  // Timer 1, fast PWM no prescaling (non-inverting mode (start low, switch to high))
+  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)
@@ -175,6 +190,7 @@ int main()
   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);
diff --git a/linear_pendulum_2009/avr/pccom.c b/linear_pendulum_2009/avr/pccom.c
index 718730220aa6a4e5860edd82c4d24665ede8204d..7d074ad8c4617dcc31cf61cbf30011b6cfa6b4e5 100644
--- a/linear_pendulum_2009/avr/pccom.c
+++ b/linear_pendulum_2009/avr/pccom.c
@@ -95,11 +95,11 @@ static uint8_t sendConfigPacket(uint8_t position)
     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(10)); break;     // Pend angle measurement
+    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;     // Pend angle measurement
+    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;
  
@@ -130,7 +130,7 @@ static uint8_t sendNextPacket() // returns 1 if a packet was available
   }
   else if (toPoll & POLL_PEND_ANGLE) { 
     toPoll &= ~POLL_PEND_ANGLE;
-    serialio_putchannel(2, getAngle()+(1L<<9)); 
+    serialio_putchannel(2, getAngle()+(1L<<10)); 
   }
   else if (toPoll & POLL_CURRENT_REFERENCE) { 
     toPoll &= ~POLL_CURRENT_REFERENCE;
diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
index 204a3a8ff8717e2a1a9e90c5bc3c799f8b36a348..0ba2aeffc61fb84efedfda92dcc56d0e427cd0f4 100644
--- a/linear_pendulum_2009/avr/vel_control.c
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -4,55 +4,65 @@
 #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;
+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;
-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
+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 = 1200;       // 6 frac bits, prop constant
+volatile int32_t Ke = 45;        // 6 frac bits, integral constant
+volatile int32_t Ksat = 1000;       // 6 frac bits, antiwindup constant
+
+//volatile int32_t K = 600;       // 6 frac bits, prop constant
+//volatile int32_t Ke = 15;        // 6 frac bits, integral constant
+//volatile int32_t Ksat = 3;       // 6 frac bits, antiwindup constant
+volatile int8_t fr_comp = (10<<3);
+#define V_MAX (90<<5)
+#define V_MIN (-90<<5)
+
 
 // encoder variables
-//#define ENCODERY  (PIND&(uint8_t)(1<<2))        //Positional encoder pins
-//#define ENCODERX  (PINB&(uint8_t)(1<<1))        //Positional encoder pins
 #define ENCODERY  (PIND&0x04)        //Positional encoder pins
-#define ENCODERX  ((PINB&0x02)<<1)        //Positional encoder pins
-
-volatile int16_t pos = 0;
-volatile int16_t posTemp = 0;
-volatile int16_t posCtrl = 0;
-volatile int16_t oldPos = 0;
-volatile int8_t newX;
-volatile int8_t newY;
-volatile int8_t oldX;
-volatile int8_t oldY;
-volatile int8_t sum = 0;
+#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; // 5 frac bits
-//volatile int32_t velEstTemp = 0; 
-volatile int32_t velEstSend = 0;
-int16_t a = 116; //7 frac bits
-int16_t b = 152; // 5 frac bits
+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;
-volatile int16_t high;
+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) 
+/* return position (in tics) */
 int32_t getPosition() {
   cli();
   posTemp = pos;
@@ -61,38 +71,31 @@ int32_t getPosition() {
 }
 
 
-// return velocity (in mm/s) 
+/* return velocity (in mm/s) */
 int32_t getVelocity() {
   return velEstSend;
 }
 
-// return last angle measurement
+
+/* return last angle measurement */
 int16_t getAngle() {
   low = inp(ADCL);
   high = inp(ADCH);
-  return ((int16_t) ((high<<8) | low) - 512);
+  return (((int16_t) ((high<<8) | low) - 512)-angleOffset);
 }
 
-// return current reference
+
+/* return current-reference */
 int32_t getCurrentRef() {
   return uSend;
 }
 
 
-
-/* Routine used to set the red LED */
-void setLED(uint8_t on)
-{
-  if (on) PORTB &= ~0x80; //Turn on
-  else PORTB |= 0x80;     //Turn off
-}
-
-// Set new reference value
+/* Set new reference value */
 void setRef(int32_t newRef) {
   ref = newRef;
 }
 
-
 /* Routine used to initialize the positional encoders */
 void initPos()
 {
@@ -100,24 +103,16 @@ void initPos()
   oldY = ENCODERY;
 }
 
-/* Routine used to reset the cart position */
-void resetPos()
-{
-  
-}
-
-/* Timer 2, Encoder counter, 73 kHz */
+/* 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);                    //Find state
-      if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {      //Predetermined values determine direction
+      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;
@@ -125,6 +120,7 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
 	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)) {
@@ -136,18 +132,17 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
       oldY = newY;
   }
 
-  
-
 }
 
 
-/* Timer 1, serial communication */
+/* Timer 0, serial communication with simulink */
 SIGNAL(SIG_OUTPUT_COMPARE0) {
 
-  TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));
+  TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));   // Disable communication and ctrl-interrupts
+
+  sei(); // enable interrupts from encoder-counter
 
-  sei(); // enable interrupts from timer 0 and timer 2  
-    PORTC |= 0x10;  // to clock calulation time
+  // Poll UART receiver
   uint8_t status = UCSRA;
   if (status & (1<<RXC)) {
     char ch = UDR;
@@ -164,11 +159,9 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
     if (toSend >= 0) UDR = (char)toSend;
   }
   
-  PORTC &= ~0x10;   
-  TIFR = (1<<OCF0);
-  //TIFR = (1<<OCF1A);
+  TIFR = (1<<OCF0);        // skip pending interrupts from serial comm, (but not from ctrl)
 
-  TIMSK |= (BV(OCIE0)|BV(OCIE1A)); 
+  TIMSK |= (BV(OCIE0)|BV(OCIE1A)); // reenable communication and ctrl-interrupts
 
 }
 
@@ -177,88 +170,77 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
 /* Timer 0, control loop , 1 kHz */
 SIGNAL(SIG_OUTPUT_COMPARE1A) {
   
-  TIMSK &= ~(BV(OCIE1A));
+  posCtrl = pos; // store pos to use in this loop
 
-  posCtrl = pos; // to aviod overwrite while reading
+  sei(); // to enable interupts from encoder counter and communication
 
-  sei(); // to enable interupts from timer2
-
- 
-  // linear velocity estimator
-  // velocity estimate in mm/s
+  // velocity estimation in mm/s
   velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos));  // 5 fracbits on velEst
   oldPos = posCtrl;
-
-  // emergency stop
-  ref = ref*(1-brake);
   
+  // 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
-  
-  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<<1))>>2);
-  }
-  
-  u = (v+8)>>4; //8 frac bits to current loop
+  // temporary ctrl-signal
+  v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
 
   // friction compensation
   if (refCtrl > 0) {
-    u = u+10;
-  } else if (ref < 0) {
-    u = u-10;
+    v = v+fr_comp;
+  } else if (refCtrl < 0) {
+    v = v-fr_comp;
   }
 
-  // Saturation
-  if (u > 127) {
-    u = 127;
-  } else if (u < -128) {
-    u = -128;
+  // 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;
   }
+  
+  // update integral part
+  // I = I + ((((Ke*e) + (Ksat)*(vSat-v))+(1<<1))>>2);
+
+  if (intCond)
+    I = I + (((Ke*e)+(1<<1))>>2);
+
+  // scale ctrl-signal to send over twi
+  u = (vSat+16)>>5; // u=90 gives current = 6.3 A, vSat makes u saturate at 90
 
+  // u that is sent to simulink
   cli();
   uSend = u;
   sei();
 
-  
-  // reset position, velocity estimate and integral part of ctrl if reset button pushed
-  
-  if (!(PIND & 0x40)) {
-    cli();
-    pos = 0; // reset position
-    sei();
-    oldPos = 0;
-    velEst = 0; // reset velocity estimate
-    I = 0; // reset integral part of controller
-    u = 0; // reset ctrl signal
-  }
-  
-  // TWI-communication
+  // 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
-  outp((int8_t)(u&0x000000ff),TWDR);  // send 8 bits reference
+ 
+  // Send reference byte to slave
+  outp((int8_t)u,TWDR);  // send 8 bits reference
   outp(BV(TWINT)|BV(TWEN),TWCR);
   while (!(TWCR&BV(TWINT))) {}
   
@@ -266,13 +248,6 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
   // stop transmission
   outp(BV(TWINT)|BV(TWEN)|BV(TWSTO),TWCR);
 
- 
-
-  //TIFR = (1<<OCF0); 
-  TIFR = (1<<OCF1A); 
-
-  TIMSK |= (BV(OCIE1A));
-
 }
 
 
@@ -281,39 +256,23 @@ int main()
   cli();
   
   //Port directions
-  outp(0x80,DDRB);   // Led output
+  //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 */
+  /* 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);
   
-  // --------old-----------------------------------------------
-  /* Timer 1, Prescaler 1, 73/8 kHz*/
-  
-  //outp(BV(WGM12)|BV(CS10),TCCR1B);
-  //outp(0x06,OCR1AH);
-  //outp(0x40,OCR1AL);
-  //outp(0,TCNT1H);
-  //outp(0,TCNT1L);
-  
-
-  /* Timer 0, 1 kHz Prescaler 64 */
-  
-  //outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
-  //outp(230,OCR0);
-  
-  /* Reset timer 0 */
-  //outp(0,TCNT0);
-  
-  // --------new-----------------------------------------------
-  /* Timer 1, 1 kHz */
+  /* Timer 1, 1 kHz , prescaler 1, ctrl-loop */
   outp(BV(WGM12)|BV(CS10),TCCR1B);
   outp(0x39,OCR1AH);
   outp(0x7f,OCR1AL);
@@ -321,32 +280,30 @@ int main()
   outp(0,TCNT1L);
   
 
-  /* Timer 0, 10 kHz, Prescaler 8 */
+  /* Timer 0, 10 kHz, Prescaler 8, serial communication */
   outp(BV(WGM01)|BV(CS01),TCCR0);
-  //outp(200,OCR0);
   outp(184-1,OCR0); // 10 kHz
-
   /* Reset timer 0 */
   outp(0,TCNT0);
-  //----------------------------------------------------------
 
-  // syncronization (timer1 interrupt 34 micros before timer0 interrupt)
+
+  // syncronization (ctrl interrupt 34 micros before communication interrupt)
   TCNT1 = TCNT0*8+500;
   
   
-  //Serial communication
+  //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, MUX0 for current */
+  
+  /* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */
   outp(BV(REFS0)|BV(MUX0)|BV(MUX1),ADMUX);
   
-  // Enable ADC on mux3, start first conversion, prescaler 128, free running mode
+  // 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);
 
 
@@ -357,11 +314,27 @@ int main()
   // initialize position measurements
   initPos();
 
+  // initialize pc-communication
   pccom_init();
   
   //Enable interrupts
   sei();
 
   // loop
-  while (1) {}
+  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);
+      //angleOffset = -150;
+      sei();
+      oldPos = 0;
+      velEst = 0; // reset velocity estimate
+      I = 0; // reset integral part of controller
+      u = 0; // reset ctrl signal
+    } 
+  }
 }