diff --git a/linear_pendulum_2009/avr/current_control.c b/linear_pendulum_2009/avr/current_control.c
index 9f15d316c6019792875600285b46dd595847feea..35cf2b8af5ab8b71325654e70c83994bb7fb9aae 100644
--- a/linear_pendulum_2009/avr/current_control.c
+++ b/linear_pendulum_2009/avr/current_control.c
@@ -17,12 +17,11 @@
 
 
 // reference generation variables
-volatile int16_t ref = 0;
+volatile int16_t ref = 200;
 volatile int16_t refCount = 0;
 volatile int8_t refFlag = 0;
 
 
-
 // control variables
 volatile unsigned char lbyte,hbyte;  
 volatile int16_t y;
@@ -44,7 +43,7 @@ volatile int16_t Ksat = 51;//1; // 7 frac bits, h/Tr
 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 = 1;
+volatile int16_t skipSamples = 30000;
 volatile int16_t countSamples = 0;
 volatile int16_t jj=0;
 volatile int8_t stop = 0;
@@ -156,8 +155,9 @@ SIGNAL(SIG_ADC)
   */
   // 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) {
@@ -165,26 +165,16 @@ SIGNAL(SIG_ADC)
   } else if (u < -128) {
     u = -128;
   }
-  */
-
-  // uppl�sning p� styrsignal [-104,103] f�r att matcha sampelperiod
-  u = ((v*13+32)>>6);
-  
-  if (u > 104) {
-    u = 104;
-  } else if (u < -105) {
-    u = -105;
-  }
 
   if (u < 0) {
-    //PORTC = 0x80+(PORTC & 0x7F);
     PORTC |= 0x80;
-    OCR1BL = (unsigned char) (104-(-u));
+    OCR1BL = (unsigned char) (128-(-u));
   } else {
     PORTC = (PORTC & 0x7F);
-    OCR1BL = (unsigned char) (105-u);
+    OCR1BL = (unsigned char) (127-u);
   }
 
+
   // For logging
   countSamples++;
   if (countSamples == skipSamples) {
@@ -200,8 +190,6 @@ SIGNAL(SIG_ADC)
       stop = 1;
       sendData();
     }
-    if (jj == 30)
-      ref = 100;
   }
 
   PORTA &= 0xef;	//Turn off calculation timer  
@@ -211,7 +199,7 @@ SIGNAL(SIG_ADC)
 
 /* reference square- or triangle wave generator with timer 0 */
 SIGNAL(SIG_OVERFLOW0) {
-  int8_t rectangle = 1;
+  int8_t rectangle = 0;
   refCount++;
   if (rectangle == 1) {
     if (refFlag == 0) {
@@ -227,12 +215,12 @@ SIGNAL(SIG_OVERFLOW0) {
       }
     }
   } else {
-    if (refCount <= 20) {  // ref*2
-      ref -= 2;
+    if (refCount <= 40) {  // ref
+      ref -= 10;
     } else {
-      ref += 2;
+      ref += 10;
     }
-    if (refCount == 40) { // ref*4
+    if (refCount == 80) { // ref*2
       refCount = 0;
     }
   }
@@ -264,25 +252,16 @@ int main()
   
   // 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);
-
-  // 104-quant on ctrl-signal and direction
   outp(0x00,OCR1AH);
-  outp(104,OCR1AL);
-
+  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
-
   outp(0x00,OCR1BH);
-  outp(104,OCR1BL); // to not start motor-rotation before control
+  outp(0x7f,OCR1BL); // to not start motor-rotation before control
 
-  // 104-quant on ctrl-signal and direction
-  
   
   /* Timer 2 (control loop), prescaler 256, clear on compare match (28), -> h = 0.5 ms */
   //  outp(BV(WGM21)|BV(CS22)|BV(CS21),TCCR2);
@@ -323,7 +302,7 @@ int main()
   while (1) {
     if (stop) {
       cli();
-      OCR1BL = 0x7f;
+      OCR1BL = 0xff;
     }
   }
 }