From 5f559bb89539fd353f6d8f62f753ac92578b9d6e Mon Sep 17 00:00:00 2001
From: Thefeli73 <thefeli73@hotmail.com>
Date: Sat, 17 Apr 2021 14:05:54 +0200
Subject: [PATCH] try to fix final function

---
 EENX15_LQR/LQR.ino | 20 ++++++++------------
 1 file changed, 8 insertions(+), 12 deletions(-)

diff --git a/EENX15_LQR/LQR.ino b/EENX15_LQR/LQR.ino
index f97f021..85a7dbd 100644
--- a/EENX15_LQR/LQR.ino
+++ b/EENX15_LQR/LQR.ino
@@ -90,27 +90,23 @@ int inputToControlSystem(float position_m, float angle_r){
             (Gain4_Gain[i + 4] * Integrator1_CSTATE[1] +
             Gain4_Gain[i] * Integrator1_CSTATE[0])));
   }
-  /*
-  Integrator1_CSTATE[0] = rtDW.Sum4[0];
-  Integrator1_CSTATE[1] = rtDW.Sum4[1];
-  Integrator1_CSTATE[2] = rtDW.Sum4[2];
-  Integrator1_CSTATE[3] = rtDW.Sum4[3];
-  */
+  
+  Arduino_skal_derivatives();
   return saturatedSignalToMotors();
 }
 
 // | ///////////////////////////////////
 // | //Row 215-225 in Arduino_skal.cpp
 // v ///////////////////////////////////
-/*
-void Arduino_skalModelClass::Arduino_skal_derivatives()
-{
-  Arduino_skalModelClass::XDot *_rtXdot;
+
+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[3] = rtDW.Sum4[3];*/
+}