From c2061717e63172e6e13da9e54240ae5a199caf86 Mon Sep 17 00:00:00 2001 From: Thefeli73 Date: Tue, 20 Apr 2021 13:27:26 +0200 Subject: [PATCH] try new K matrix, also scale output --- EENX15_LQR/lqr_fullstate.ino | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/EENX15_LQR/lqr_fullstate.ino b/EENX15_LQR/lqr_fullstate.ino index 929b0ef..3b694b3 100644 --- a/EENX15_LQR/lqr_fullstate.ino +++ b/EENX15_LQR/lqr_fullstate.ino @@ -6,7 +6,8 @@ float lqr_fullstate(float position_m, float speed_ms, float angle_r, float angle // const float matrix_K [4] = {-0.1054, -0.6273, 23.5822, 3.1936}; // const float matrix_K [4] = {-0.1054, -0.5273, 23.5546, 3.1876}; // const float matrix_K [4] = {-0.1054, -0.5273, 23.5546, 3.1876}; - const float matrix_K [4] = {-0.7071, -1.5720, 26.0726, 3.7050}; +// const float matrix_K [4] = {-0.7071, -1.5720, 26.0726, 3.7050}; + const float matrix_K [4] = {-0.7071, -1.5980, 26.6081, 4.3220}; float result; result = matrix_K[0] * position_m + @@ -32,10 +33,11 @@ float calc_speed(float input) { float c = 90.75; float radps = a * pow(speed, b) + c; ////// the response graph */ - float scale = 8; - input = abs(input)*0.30796*scale; // scale down to rad/s (78,53/255) + float scale = 1.5; + input = abs(input)*0.30796; // scale down to rad/s (78,53/255) + Serial.print("input: "); Serial.println(input); float result = pow((3145.84/(90.75 - input)),1.00715); // break out x from response graph - + result *= scale; Serial.print("calcspeed: "); Serial.println(result); return result;