diff --git a/EENX15_LQR/Arduino_skal.h b/EENX15_LQR/Arduino_skal.h
index 099605c..a92c6b6 100644
--- a/EENX15_LQR/Arduino_skal.h
+++ b/EENX15_LQR/Arduino_skal.h
@@ -156,7 +156,7 @@ class Arduino_skalModelClass {
       uint32_T clockTick0;
       time_T stepSize0;
       uint32_T clockTick1;
-      SimTimeStep simTimeStep;
+//      SimTimeStep simTimeStep;
       boolean_T stopRequestedFlag;
       time_T *t;
       time_T tArray[2];
diff --git a/EENX15_LQR/LQR.ino b/EENX15_LQR/LQR.ino
index 6e8628d..56869a3 100644
--- a/EENX15_LQR/LQR.ino
+++ b/EENX15_LQR/LQR.ino
@@ -4,50 +4,41 @@
 // | ///////////////////////////////////
 // | //Row 24-52 in Arduino_skal_data.cpp
 // v ///////////////////////////////////
-const double Arduino_skalModelClass::ConstP rtConstP = {
-  // Expression: [100;200]
-  //  Referenced by: '<Root>/vartejag'
-
-  { 100.0, 200.0 },
 
   // Expression: A
   //  Referenced by: '<Root>/Gain4'
 
-  { 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 };
 
   // Expression: C
   //  Referenced by: '<Root>/Gain6'
 
-  { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 },
+const double Gain6_Gain [8] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 };
 
   // Expression: L
   //  Referenced by: '<Root>/Gain2'
 
-  { 116.63033952875418, 3387.8673967111704, -1.4473912197449676,
+const double Gain2_Gain [8] = { 116.63033952875418, 3387.8673967111704, -1.4473912197449676,
     -115.34372132703447, -1.0534041975488044, -48.223441605702455,
-    117.16185100039935, 3490.0480780568214 },
+    117.16185100039935, 3490.0480780568214 };
 
   // Expression: B
   //  Referenced by: '<Root>/Gain3'
 
-  { 0.0, 2.078094708544223, 0.0, 5.2810302415000852 }
-};
+const double Gain3_Gain [4] = { 0.0, 2.078094708544223, 0.0, 5.2810302415000852 };
+
 
 // | ///////////////////////////////////
 // | //Row 261-264 in Arduino_skal.cpp
 // v ///////////////////////////////////
-
-rtX.Integrator1_CSTATE[0] = 0.0;
-rtX.Integrator1_CSTATE[1] = 0.0;
-rtX.Integrator1_CSTATE[2] = 0.0;
-rtX.Integrator1_CSTATE[3] = 0.0;
+double Integrator1_CSTATE [4] = {0.0, 0.0, 0.0, 0.0};
+double Sum4[4];
 
 // | ///////////////////////////////////
 // | //Row 123-124 in Arduino_skal.cpp
 // v ///////////////////////////////////
 
-real_T tmp[2];
+double tmp[2];
 int rtb_Saturation;
 
 // | ///////////////////////////////////
@@ -56,10 +47,10 @@ int rtb_Saturation;
 
 // Denna funktion bör anropas när styrka + riktning till motorer ska bestämmas.
 int saturatedSignalToMotors(){
-  rtb_Saturation = ((-31.622776601683942 * rtX.Integrator1_CSTATE[0] +
-             -21.286439360075747 * rtX.Integrator1_CSTATE[1]) +
-             80.789376267003959 * rtX.Integrator1_CSTATE[2]) +
-             13.42463576551093 * rtX.Integrator1_CSTATE[3];
+  rtb_Saturation = ((-31.622776601683942 * Integrator1_CSTATE[0] +
+             -21.286439360075747 * Integrator1_CSTATE[1]) +
+             80.789376267003959 * Integrator1_CSTATE[2]) +
+             13.42463576551093 * Integrator1_CSTATE[3];
   
   if (0.0 - rtb_Saturation > 11.5) {
     rtb_Saturation = 11.5;
@@ -75,16 +66,16 @@ int saturatedSignalToMotors(){
 // v ///////////////////////////////////
 int inputToControlSystem(float position_m, float angle_r){
   float posAndAng[] = {position_m, angle_r};
-  for (i = 0; i < 2; i++) {
-    tmp[i] = rtConstP.posAndAng[i] - (((rtConstP.Gain6_Gain[i + 2] *
-      rtX.Integrator1_CSTATE[1] + rtConstP.Gain6_Gain[i] *
-      rtX.Integrator1_CSTATE[0]) + rtConstP.Gain6_Gain[i + 4] *
-      rtX.Integrator1_CSTATE[2]) + rtConstP.Gain6_Gain[i + 6] *
-      rtX.Integrator1_CSTATE[3]);
+  for (int i = 0; i < 2; i++) {
+    tmp[i] = posAndAng[i] - (((Gain6_Gain[i + 2] *
+      Integrator1_CSTATE[1] + Gain6_Gain[i] *
+      Integrator1_CSTATE[0]) + Gain6_Gain[i + 4] *
+      Integrator1_CSTATE[2]) + Gain6_Gain[i + 6] *
+      Integrator1_CSTATE[3]);
   }
 
   // End of Sum: '<Root>/Sum2'
-  for (i = 0; i < 4; i++) {
+  for (int i = 0; i < 4; i++) {
     // Sum: '<Root>/Sum4' incorporates:
     //   Gain: '<Root>/Gain2'
     //   Gain: '<Root>/Gain3'
@@ -92,12 +83,12 @@ int inputToControlSystem(float position_m, float angle_r){
     //   Integrator: '<Root>/Integrator1'
     //   Sum: '<Root>/Sum3'
 
-    rtDW.Sum4[i] = ((rtConstP.Gain2_Gain[i + 4] * tmp[1] + rtConstP.Gain2_Gain[i]
-            * tmp[0]) + rtConstP.Gain3_Gain[i] * rtb_Saturation) +
-            (rtConstP.Gain4_Gain[i + 12] * rtX.Integrator1_CSTATE[3] +
-            (rtConstP.Gain4_Gain[i + 8] * rtX.Integrator1_CSTATE[2] +
-            (rtConstP.Gain4_Gain[i + 4] * rtX.Integrator1_CSTATE[1] +
-            rtConstP.Gain4_Gain[i] * rtX.Integrator1_CSTATE[0])));
+    Sum4[i] = ((Gain2_Gain[i + 4] * tmp[1] + Gain2_Gain[i]
+            * tmp[0]) + Gain3_Gain[i] * rtb_Saturation) +
+            (Gain4_Gain[i + 12] * Integrator1_CSTATE[3] +
+            (Gain4_Gain[i + 8] * Integrator1_CSTATE[2] +
+            (Gain4_Gain[i + 4] * Integrator1_CSTATE[1] +
+            Gain4_Gain[i] * Integrator1_CSTATE[0])));
   }
   /*
   Integrator1_CSTATE[0] = rtDW.Sum4[0];