diff --git a/EENX15_LQR/EENX15_LQR.ino b/EENX15_LQR/EENX15_LQR.ino
index 1d18b71..1f25e42 100644
--- a/EENX15_LQR/EENX15_LQR.ino
+++ b/EENX15_LQR/EENX15_LQR.ino
@@ -100,6 +100,7 @@ void setup() {
   pinMode(MotorPinB, OUTPUT);
   pinMode(MotorSpeedB, OUTPUT);
   pinMode(MotorBrakeB, OUTPUT);
+
 }
  
 void loop() {
@@ -148,9 +149,10 @@ void printInfo(){
 void setSpeed(){
   if(abs(safe_angle)<50 ){
     //speed = 8*safe_angle;
-    float position_m = countA/174.76;
+    float position_m = -countA/174.76;
     float angle_r = angle_pitch_output * 0.318;
-    speed = 22 * inputToControlSystem(position_m, angle_r);
+    speed = -22 * inputToControlSystem(position_m, angle_r);
+    //speed = -22 * inputToControlSystem(0, 1);
     if(speed<0){
       digitalWrite(MotorPinB, CW);
       digitalWrite(MotorPinA, CCW);
diff --git a/EENX15_LQR/LQR.ino b/EENX15_LQR/LQR.ino
index 85a7dbd..0a2047a 100644
--- a/EENX15_LQR/LQR.ino
+++ b/EENX15_LQR/LQR.ino
@@ -8,12 +8,18 @@
   // Expression: A
   //  Referenced by: '<Root>/Gain4'
 
-const double Gain4_Gain [16] = { 0.0, 0.0, 0.0, 0.0, 1.0, -0.20780947085442231, 0.0, -0.52810302415000854, 0.0, 13.239785742831822, 0.0, 58.601480177829842, 0.0, 0.0, 1.0, 0.0 };
+const double Gain4_Gain [16] = { 0.0, 0.0, 0.0, 0.0, 
+  1.0, -0.20780947085442231, 0.0, -0.52810302415000854,
+  0.0, 13.239785742831822, 0.0, 58.601480177829842, 
+  0.0, 0.0, 1.0, 0.0 };
+
+//const double Gain4_Gain [16] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
 
   // Expression: C
   //  Referenced by: '<Root>/Gain6'
 
 const double Gain6_Gain [8] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 };
+//const double Gain6_Gain [8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
 
   // Expression: L
   //  Referenced by: '<Root>/Gain2'
@@ -21,6 +27,9 @@ const double Gain6_Gain [8] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 };
 const double Gain2_Gain [8] = { 116.63033952875418, 3387.8673967111704, -1.4473912197449676,
     -115.34372132703447, -1.0534041975488044, -48.223441605702455,
     117.16185100039935, 3490.0480780568214 };
+//const double Gain2_Gain [8] = { 116.63033952875418, 338.78673967111704, -1.4473912197449676,
+//    -115.34372132703447, -1.0534041975488044, -48.223441605702455,
+//    117.16185100039935, 34.900480780568214 };
 
   // Expression: B
   //  Referenced by: '<Root>/Gain3'
@@ -39,14 +48,14 @@ double Sum4[4];
 // v ///////////////////////////////////
 
 double tmp[2];
-int rtb_Saturation;
+double rtb_Saturation = 0.0;
 
 // | ///////////////////////////////////
 // | //Row 140-143 in Arduino_skal.cpp
 // v ///////////////////////////////////
 
 // Denna funktion bör anropas när styrka + riktning till motorer ska bestämmas.
-int saturatedSignalToMotors(){
+double saturatedSignalToMotors(){
   rtb_Saturation = ((-31.622776601683942 * Integrator1_CSTATE[0] +
              -21.286439360075747 * Integrator1_CSTATE[1]) +
              80.789376267003959 * Integrator1_CSTATE[2]) +
@@ -59,12 +68,13 @@ int saturatedSignalToMotors(){
   } else {
     rtb_Saturation = 0.0 - rtb_Saturation;
   }
+  Serial.print("Saturation  = "); Serial.println(rtb_Saturation);
   return rtb_Saturation;
 }
 // | ///////////////////////////////////
 // | //Row 165-188 in Arduino_skal.cpp
 // v ///////////////////////////////////
-int inputToControlSystem(float position_m, float angle_r){
+double inputToControlSystem(float position_m, float angle_r){
   float posAndAng[] = {position_m, angle_r};
   for (int i = 0; i < 2; i++) {
     tmp[i] = posAndAng[i] - (((Gain6_Gain[i + 2] *
@@ -90,7 +100,10 @@ int inputToControlSystem(float position_m, float angle_r){
             (Gain4_Gain[i + 4] * Integrator1_CSTATE[1] +
             Gain4_Gain[i] * Integrator1_CSTATE[0])));
   }
-  
+  Serial.print("Sum4 0 = "); Serial.println(Sum4[0]);
+  Serial.print("Sum4 1 = "); Serial.println(Sum4[1]);
+  Serial.print("Sum4 2 = "); Serial.println(Sum4[2]);
+  Serial.print("Sum4 3 = "); Serial.println(Sum4[3]);
   Arduino_skal_derivatives();
   return saturatedSignalToMotors();
 }
@@ -100,13 +113,25 @@ int inputToControlSystem(float position_m, float angle_r){
 // v ///////////////////////////////////
 
 void Arduino_skal_derivatives()
-{/*
+{
+  /*
   XDot *_rtXdot;
   _rtXdot = ((XDot *) (&rtM)->derivs);
-
   // Derivatives for Integrator: '<Root>/Integrator1'
-  _rtXdot->Integrator1_CSTATE[0] = rtDW.Sum4[0];
-  _rtXdot->Integrator1_CSTATE[1] = rtDW.Sum4[1];
-  _rtXdot->Integrator1_CSTATE[2] = rtDW.Sum4[2];
-  _rtXdot->Integrator1_CSTATE[3] = rtDW.Sum4[3];*/
+  _rtXdot->Integrator1_CSTATE[0] = Sum4[0];
+  _rtXdot->Integrator1_CSTATE[1] = Sum4[1];
+  _rtXdot->Integrator1_CSTATE[2] = Sum4[2];
+  _rtXdot->Integrator1_CSTATE[3] = Sum4[3];
+
+  
+  Integrator1_CSTATE[0] = Sum4[0] * (fastTimer/1000.0);
+  Integrator1_CSTATE[1] = Sum4[1] * (fastTimer/1000.0);
+  Integrator1_CSTATE[2] = Sum4[2] * (fastTimer/1000.0);
+  Integrator1_CSTATE[3] = Sum4[3] * (fastTimer/1000.0);
+*/
+  // Derivatives for Integrator: '<Root>/Integrator1'
+  for (int i = 0; i < 4; i++) {
+    Integrator1_CSTATE[i] = Sum4[i] * 80/1000.0;
+    Serial.print("Integrator: "); Serial.println(Integrator1_CSTATE[i]);
+  }
 }