File content as of revision 0:bf9bf4b7625f:

#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