Diff: Controller.cpp

Revision:

0:bf9bf4b7625f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,72 @@
+#include "Controller.h"
+#include "SystemConstant.h"
+#include "SensorFusion.h"
+
+bool test1 = 0;
+float sigma = 0.0;
+float alpha_1 = 0.0;
+float alpha_2 = 0.0;
+float K_1[3] = {1175.5, 302.2, -27.7};
+float K_2[3] = {193.573, 50.0229, -2.8192};
+float K_LQR01[3] = {39.9682, 9.2497, -0.0712};          // R = 1
+float K_LQR11[3] = {107.3029, 26.5432, -2.1435};        // R = 0.01
+float K_LQR21[3] = {285.88, 72.6362, -7.5105};          // R = 0.001
+float K_LQR31[3] = {444.0651, 106.7309, -12.7130};       // R = 0.0001
+float K_LQR75[3] = {789.0790, 190.093 -23.4674};        // R = 0.0003
+float K_LQR755[3]= {981.915, 252.3768, -28.3543};       // R = 0.000075
+float K_LQR655[3]= {1053.0382, 270.7444, -30.4835};     // R = 0.00065
+float K_LQR55[3] = {1197.4, 308.0321, -34.8059};        // R = 0.00005
+float K_LQR65[3] = {1095.0974, 281.6061, -31.7426};     // R = 0.00006
+float K_LQR35[3] = {1539.18735, 396.2928, -45.0369};    // R = 0.00006
+float K_LQR15[3] = {2649.2, 682.9531, -78.2644};        // R = 0.00001
+float K_LQR85[3] = {951.4667, 244.5136, -27.4427};      // R = 0.00008
+float K_LQR95[3] = {898.3697, 230.8014, -25.8531};      // R = 0.00009
+float u_1 = 0.0;
+float u_2 = 0.0;
+float u_3 = 0.0;
+float u_d = 0.0;
+float u = 0.0;
+float roll_ref = 0.0;
+float roll_err = 0.0;
+float steer_ref = 0.0;
+float steer_ref_old = 0.0;
+float steer_rad = 0.0;
+float steer_rad_old = 0.0;
+float steer_degree = 0.0;
+float steering_angle = 0.0f;
+
+
+//*************************************************************************************************************************************************************************************************************************
+//                                                                              Controller
+void controller(float velocity)
+{
+    roll_err = roll_angle - roll_ref;
+    
+    // Controller from Linear Quadratic Regulator
+//    u_1 = - K_LQR75[0]*roll_err;
+//    u_2 = - K_LQR655[1]*droll_angle;
+//    u_3 = - K_LQR75[2]*(steer_rad-steer_ref);
+//    u   = u_1 + u_2 + u_3 - 18.8803f*roll_ref;
+    
+    
+    // Controller from Linear Matrice Inequality
+    sigma = (velocity - Vmin) / (Vmax - Vmin);
+    alpha_1 = (1 - sigma);
+    alpha_2 = sigma;
+    u_1 = -K_1[0]*roll_err*alpha_1 - K_2[0]*roll_err*alpha_2;
+    u_2 = -K_1[1]*droll_angle*alpha_1 - K_2[1]*droll_angle*alpha_2;
+    u_3 = -K_1[2]*(steer_rad-steer_ref)*alpha_1 - K_2[2]*(steer_rad-steer_ref)*alpha_2;
+    u = u_1 + u_2 + u_3  - 18.8803f*roll_ref;
+    
+}// controller
+
+//*************************************************************************************************************************************************************************************************************************
+//                                                                              steer_angle
+void steer_angle(float u_in, float velocity)
+{
+    steer_rad = lpf(u_in, steer_rad_old, u2steer_lpf_freq * velocity) * u2steer_gain / velocity; 
+    steer_rad_old = steer_rad;
+    steering_angle = steer_rad/3.14f*180.0f;
+    if(steering_angle > 60) steering_angle =  60;
+    if(steering_angle <-60) steering_angle = -60;
+}// steer_angle
\ No newline at end of file