Controller.cpp

00001 #include "Controller.h"
00002 #include "SystemConstant.h"
00003 #include "SensorFusion.h"
00004 
00005 bool test1 = 0;
00006 float sigma = 0.0;
00007 float alpha_1 = 0.0;
00008 float alpha_2 = 0.0;
00009 float K_1[3] = {1175.5, 302.2, -27.7};
00010 float K_2[3] = {193.573, 50.0229, -2.8192};
00011 float K_LQR01[3] = {39.9682, 9.2497, -0.0712};          // R = 1
00012 float K_LQR11[3] = {107.3029, 26.5432, -2.1435};        // R = 0.01
00013 float K_LQR21[3] = {285.88, 72.6362, -7.5105};          // R = 0.001
00014 float K_LQR31[3] = {444.0651, 106.7309, -12.7130};       // R = 0.0001
00015 float K_LQR75[3] = {789.0790, 190.093 -23.4674};        // R = 0.0003
00016 float K_LQR755[3]= {981.915, 252.3768, -28.3543};       // R = 0.000075
00017 float K_LQR655[3]= {1053.0382, 270.7444, -30.4835};     // R = 0.00065
00018 float K_LQR55[3] = {1197.4, 308.0321, -34.8059};        // R = 0.00005
00019 float K_LQR65[3] = {1095.0974, 281.6061, -31.7426};     // R = 0.00006
00020 float K_LQR35[3] = {1539.18735, 396.2928, -45.0369};    // R = 0.00006
00021 float K_LQR15[3] = {2649.2, 682.9531, -78.2644};        // R = 0.00001
00022 float K_LQR85[3] = {951.4667, 244.5136, -27.4427};      // R = 0.00008
00023 float K_LQR95[3] = {898.3697, 230.8014, -25.8531};      // R = 0.00009
00024 float u_1 = 0.0;
00025 float u_2 = 0.0;
00026 float u_3 = 0.0;
00027 float u_d = 0.0;
00028 float u = 0.0;
00029 float roll_ref = 0.0;
00030 float roll_err = 0.0;
00031 float steer_ref = 0.0;
00032 float steer_ref_old = 0.0;
00033 float steer_rad = 0.0;
00034 float steer_rad_old = 0.0;
00035 float steer_degree = 0.0;
00036 float steering_angle = 0.0f;
00037 
00038 
00039 //*************************************************************************************************************************************************************************************************************************
00040 //                                                                              Controller
00041 void controller(float velocity)
00042 {
00043     roll_err = roll_angle - roll_ref;
00044     
00045     // Controller from Linear Quadratic Regulator
00046 //    u_1 = - K_LQR75[0]*roll_err;
00047 //    u_2 = - K_LQR655[1]*droll_angle;
00048 //    u_3 = - K_LQR75[2]*(steer_rad-steer_ref);
00049 //    u   = u_1 + u_2 + u_3 - 18.8803f*roll_ref;
00050     
00051     
00052     // Controller from Linear Matrice Inequality
00053     sigma = (velocity - Vmin) / (Vmax - Vmin);
00054     alpha_1 = (1 - sigma);
00055     alpha_2 = sigma;
00056     u_1 = -K_1[0]*roll_err*alpha_1 - K_2[0]*roll_err*alpha_2;
00057     u_2 = -K_1[1]*droll_angle*alpha_1 - K_2[1]*droll_angle*alpha_2;
00058     u_3 = -K_1[2]*(steer_rad-steer_ref)*alpha_1 - K_2[2]*(steer_rad-steer_ref)*alpha_2;
00059     u = u_1 + u_2 + u_3  - 18.8803f*roll_ref;
00060     
00061 }// controller
00062 
00063 //*************************************************************************************************************************************************************************************************************************
00064 //                                                                              steer_angle
00065 void steer_angle(float u_in, float velocity)
00066 {
00067     steer_rad = lpf(u_in, steer_rad_old, u2steer_lpf_freq * velocity) * u2steer_gain / velocity; 
00068     steer_rad_old = steer_rad;
00069     steering_angle = steer_rad/3.14f*180.0f;
00070     if(steering_angle > 60) steering_angle =  60;
00071     if(steering_angle <-60) steering_angle = -60;
00072 }// steer_angle