From 6784a07372b7487b17ca88306294fe1256b8a515 Mon Sep 17 00:00:00 2001
From: Thefeli73 <thefeli73@hotmail.com>
Date: Fri, 16 Apr 2021 15:38:21 +0200
Subject: [PATCH] add actual lqr (not compiling yet)

---
 EENX15_LQR/EENX15_LQR.ino |   9 ++-
 EENX15_LQR/LQR.ino        | 125 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 132 insertions(+), 2 deletions(-)

diff --git a/EENX15_LQR/EENX15_LQR.ino b/EENX15_LQR/EENX15_LQR.ino
index 28256a6..120c8d8 100644
--- a/EENX15_LQR/EENX15_LQR.ino
+++ b/EENX15_LQR/EENX15_LQR.ino
@@ -110,6 +110,7 @@ void loop() {
   
   if (m - lastCorrectionTime >= fastTimer) { //run this code ever 80ms (12.5hz)
     lastCorrectionTime = m;
+//    lqr();
     getSpeed();
     setSpeed();
   }
@@ -147,7 +148,11 @@ void printInfo(){
 
 void setSpeed(){
   if(abs(safe_angle)<50 ){
-    speed = 8*safe_angle;
+    //speed = 8*safe_angle;
+    float position_m = countA/174.76;
+    float angle_r = angle_pitch_output * 0.318;
+    speed = inputToControlSystem(position_m, angle_r);
+    speed *= 22;
     if(speed<0){
       digitalWrite(MotorPinB, CW);
       digitalWrite(MotorPinA, CCW);
@@ -162,7 +167,7 @@ void setSpeed(){
       speed = 0;
     }
     speed = abs(speed);
-    speed = constrain(speed, 0, 249);
+    speed = constrain(speed, 0, 250);
     analogWrite(MotorSpeedB, speed); //Wheel close to connections
     analogWrite(MotorSpeedA, speed); //First experiment wheel
   }
diff --git a/EENX15_LQR/LQR.ino b/EENX15_LQR/LQR.ino
index e69de29..6e8628d 100644
--- a/EENX15_LQR/LQR.ino
+++ b/EENX15_LQR/LQR.ino
@@ -0,0 +1,125 @@
+//LQR-stuff
+#include "Arduino_skal.h"
+
+// | ///////////////////////////////////
+// | //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 },
+
+  // Expression: C
+  //  Referenced by: '<Root>/Gain6'
+
+  { 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,
+    -115.34372132703447, -1.0534041975488044, -48.223441605702455,
+    117.16185100039935, 3490.0480780568214 },
+
+  // Expression: B
+  //  Referenced by: '<Root>/Gain3'
+
+  { 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;
+
+// | ///////////////////////////////////
+// | //Row 123-124 in Arduino_skal.cpp
+// v ///////////////////////////////////
+
+real_T tmp[2];
+int rtb_Saturation;
+
+// | ///////////////////////////////////
+// | //Row 140-143 in Arduino_skal.cpp
+// v ///////////////////////////////////
+
+// 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];
+  
+  if (0.0 - rtb_Saturation > 11.5) {
+    rtb_Saturation = 11.5;
+  } else if (0.0 - rtb_Saturation < -11.5) {
+    rtb_Saturation = -11.5;
+  } else {
+    rtb_Saturation = 0.0 - rtb_Saturation;
+  }
+  return rtb_Saturation;
+}
+// | ///////////////////////////////////
+// | //Row 165-188 in Arduino_skal.cpp
+// 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]);
+  }
+
+  // End of Sum: '<Root>/Sum2'
+  for (i = 0; i < 4; i++) {
+    // Sum: '<Root>/Sum4' incorporates:
+    //   Gain: '<Root>/Gain2'
+    //   Gain: '<Root>/Gain3'
+    //   Gain: '<Root>/Gain4'
+    //   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])));
+  }
+  /*
+  Integrator1_CSTATE[0] = rtDW.Sum4[0];
+  Integrator1_CSTATE[1] = rtDW.Sum4[1];
+  Integrator1_CSTATE[2] = rtDW.Sum4[2];
+  Integrator1_CSTATE[3] = rtDW.Sum4[3];
+  */
+  return saturatedSignalToMotors();
+}
+
+// | ///////////////////////////////////
+// | //Row 215-225 in Arduino_skal.cpp
+// v ///////////////////////////////////
+
+void Arduino_skalModelClass::Arduino_skal_derivatives()
+{
+  Arduino_skalModelClass::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];
+}