diff --git a/linear_pendulum_2009/avr/current_control_final.c b/linear_pendulum_2009/avr/current_control_final.c
index 0da8fa188fec5c285e762388c160a87d558612fd..58f9668a0ea2c0a5bd4d4c628ae43eda4f959d9f 100644
--- a/linear_pendulum_2009/avr/current_control_final.c
+++ b/linear_pendulum_2009/avr/current_control_final.c
@@ -14,9 +14,8 @@ 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 int16_t K = 111;  // 7 frac bits
+volatile int16_t Ke = 38;  // 7 frac bits, K*h/Ti
 volatile int8_t intCond = 0;
 
 
@@ -90,8 +89,6 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
   high = inp(ADCH);
   y = ((int16_t)((high<<8) | low)) - 512; // y 9 frac bits
 
-  y = ((y*3)>>1);
-
   // control error
   e = ref-y; // e 9 frac bits
   
@@ -136,7 +133,7 @@ SIGNAL(SIG_OUTPUT_COMPARE2) {
     // status 0x80 means data recieved
     if (status == 0x80) {
       ref = (int16_t)((int8_t)inp(TWDR)); // read 8 bit reference
-      ref = ref*8;  // shift up 2 steps for 10 bits reference in loop
+      ref = (ref<<2);  // shift up 2 steps for 10 bits reference in loop
     } 
     else {
     }
diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
index 0ba2aeffc61fb84efedfda92dcc56d0e427cd0f4..6b3d6222e377a88ac517a5d8326b8ac482ccaffe 100644
--- a/linear_pendulum_2009/avr/vel_control.c
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -24,16 +24,11 @@ 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 int32_t K = 800;       // 6 frac bits, prop constant
+volatile int32_t Ke = 30;        // 6 frac bits, integral constant
 volatile int8_t fr_comp = (10<<3);
-#define V_MAX (90<<5)
-#define V_MIN (-90<<5)
+#define V_MAX (120<<4)
+#define V_MIN (-120<<4)
 
 
 // encoder variables
@@ -215,14 +210,11 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) {
     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 = (vSat+8)>>4; // u=127 gives current = 6.75 A, vSat makes u saturate at 114
 
   // u that is sent to simulink
   cli();