From b57bd6ca27d72eba3be646ec526dbc4dd9b09921 Mon Sep 17 00:00:00 2001
From: Thefeli73 <thefeli73@hotmail.com>
Date: Sat, 24 Apr 2021 11:15:41 +0200
Subject: [PATCH] fixes, idk

---
 EENX15_LQR/EENX15_LQR.ino    | 7 ++++---
 EENX15_LQR/lqr_fullstate.ino | 3 +--
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/EENX15_LQR/EENX15_LQR.ino b/EENX15_LQR/EENX15_LQR.ino
index 7cd86ce..289a59d 100644
--- a/EENX15_LQR/EENX15_LQR.ino
+++ b/EENX15_LQR/EENX15_LQR.ino
@@ -3,7 +3,7 @@
 int lastCorrectionTime = 0;
 int lastPrintTime = 0;
 
-static int fastTimer = 80; //ms
+static int fastTimer = 10; //ms
 static int slowTimer = 1000; //ms
 
 //lqr stuff
@@ -171,7 +171,8 @@ void setSpeed(){
     float speed_ms = rps * 0.05;
     float angle_r = angle_pitch_output * 0.318;
     float angle_speed_rs = rps;
-    speed = lqr_fullstate(position_m, speed_ms, angle_r, angle_speed_rs)/ 0.022745; // (0.29/0.05)/255
+    //speed = lqr_fullstate(position_m, speed_ms, angle_r, angle_speed_rs);/// 0.019608; // (0.20*255)
+    speed = lqr_fullstate(0, 0, angle_r, 0);/// 0.019608; // (0.20*255)
     //speed = -22 * inputToControlSystem(0, 1);
     if(speed<0){
       digitalWrite(MotorPinB, CW);
@@ -184,7 +185,7 @@ void setSpeed(){
     else {
       speed = 0;
     }
-    speed = calc_speed(speed);
+    if(speed!=0){ speed = constrain(speed, -255, 255);speed = calc_speed(speed); }
     speed = abs(speed);
     speed = constrain(speed, 0, 255);
     analogWrite(MotorSpeedB, speed); //Wheel close to connections
diff --git a/EENX15_LQR/lqr_fullstate.ino b/EENX15_LQR/lqr_fullstate.ino
index e21dc72..34dd634 100644
--- a/EENX15_LQR/lqr_fullstate.ino
+++ b/EENX15_LQR/lqr_fullstate.ino
@@ -15,7 +15,6 @@ float lqr_fullstate(float position_m, float speed_ms, float angle_r, float angle
            matrix_K[1] * speed_ms +
            matrix_K[2] * angle_r +
            matrix_K[3] * angle_speed_rs;
-  result *= 0.05;
   /*
   if (result > 0.29) {
     result = 0.29;
@@ -24,7 +23,7 @@ float lqr_fullstate(float position_m, float speed_ms, float angle_r, float angle
   } else {
     result = result;
   }*/
-  Serial.print("K calculation: "); Serial.println(result); 
+  Serial.print("K calculation (force): "); Serial.println(result); 
   return result;
 }
 float calc_speed(float input) {