![](/media/cache/profiles/a68af63859af253903d758699756efd3.jpg.50x50_q85.jpg)
2018/06/08
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