Chris LU / Mbed 2 deprecated Nucleo_ZTC_20160607_oldest

code of robot bike

Dependencies:   SDFileSystem mbed

Fork of Robot_Bicycle by Chris LU

ZTC.cpp@3:197b748a397a, 2016-06-22 (annotated)

Committer:
YCTung
Date:
Wed Jun 22 04:35:18 2016 +0000
Revision:
3:197b748a397a
Child:
8:79ca11e0129d
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YCTung 3:197b748a397a 1 #include "ZTC.h"
YCTung 3:197b748a397a 2 #include "SensorFusion.h"
YCTung 3:197b748a397a 3
YCTung 3:197b748a397a 4 ///ZTC
YCTung 3:197b748a397a 5 float K_h[3] = {1636.8, 80.0, 0.0};///{1261.8, 70, 0};///{2088, 28.65, 0};///
YCTung 3:197b748a397a 6 float K_m[3] = {3458.8, 117.0, 0.0};///{1643.7, 80.2, 0};///{2536.8, 100, 0};///
YCTung 3:197b748a397a 7 float K_l[3] = {2143.8, 0.0, 0.0};
YCTung 3:197b748a397a 8 float K_s[3] = {0,0,0};
YCTung 3:197b748a397a 9 float K_hphi[3] = {0.100359*3.0, 0.034599*3.0, -0.05653*3.0};///{10.0359, 3.4599, -5.653};///
YCTung 3:197b748a397a 10 float K_mphi[3]= {3.7165*0.03, 2.6747*0.03, -2.0436*0.03};///{0.037165*6.0, 0.026747*6.0, -0.020436*6.0};///
YCTung 3:197b748a397a 11 float u = 0.0;
YCTung 3:197b748a397a 12 float phi_hat = 0.0;
YCTung 3:197b748a397a 13 float phi_hat_old = 0.0;
YCTung 3:197b748a397a 14 float phi_hat_aw = 0.0;
YCTung 3:197b748a397a 15 float dphi_hat= 0.0;
YCTung 3:197b748a397a 16 float roll_ref = 0.0;
YCTung 3:197b748a397a 17 float gamma_ref = 0.0;
YCTung 3:197b748a397a 18 float gamma_rad = 0.0;
YCTung 3:197b748a397a 19 float gamma_rad_old = 0.0;
YCTung 3:197b748a397a 20 int8_t gamma_degree = 0;
YCTung 3:197b748a397a 21
YCTung 3:197b748a397a 22 void calc_PD(float* K_in, float phi_hat_in)
YCTung 3:197b748a397a 23 {
YCTung 3:197b748a397a 24 u = -K_in[0]*(roll_angle - phi_hat_in - roll_ref) - K_in[1]*droll_angle;
YCTung 3:197b748a397a 25 }
YCTung 3:197b748a397a 26
YCTung 3:197b748a397a 27 void calc_Phi(float* K_phi_in)
YCTung 3:197b748a397a 28 {
YCTung 3:197b748a397a 29 dphi_hat = -K_phi_in[0]*(roll_angle - phi_hat - roll_ref) - K_phi_in[1]*droll_angle - K_phi_in[2]*(gamma_rad-gamma_ref);
YCTung 3:197b748a397a 30 phi_hat = phi_hat_old + (dphi_hat + phi_hat_aw)*sample_time;
YCTung 3:197b748a397a 31 }
YCTung 3:197b748a397a 32
YCTung 3:197b748a397a 33 void calc_Gamma(float u_in, float alpha, float multr)
YCTung 3:197b748a397a 34 {
YCTung 3:197b748a397a 35 gamma_rad = lpf(u_in,gamma_rad_old,alpha)/multr - GAMMA_OFFSET; ///38
YCTung 3:197b748a397a 36 gamma_rad_old = gamma_rad;
YCTung 3:197b748a397a 37 }
YCTung 3:197b748a397a 38
YCTung 3:197b748a397a 39 void anti_wdup(void)
YCTung 3:197b748a397a 40 {
YCTung 3:197b748a397a 41 if(phi_hat > PHI_HAT_UB)
YCTung 3:197b748a397a 42 {
YCTung 3:197b748a397a 43 phi_hat_aw = K_aw * (PHI_HAT_UB - phi_hat);
YCTung 3:197b748a397a 44 phi_hat = PHI_HAT_UB;
YCTung 3:197b748a397a 45 }
YCTung 3:197b748a397a 46 else if(phi_hat < PHI_HAT_LB)
YCTung 3:197b748a397a 47 {
YCTung 3:197b748a397a 48 phi_hat_aw = K_aw * (PHI_HAT_LB - phi_hat);
YCTung 3:197b748a397a 49 phi_hat = PHI_HAT_LB;
YCTung 3:197b748a397a 50 }
YCTung 3:197b748a397a 51 else
YCTung 3:197b748a397a 52 {
YCTung 3:197b748a397a 53 phi_hat_aw = 0.0;
YCTung 3:197b748a397a 54 }
YCTung 3:197b748a397a 55 phi_hat_old = phi_hat;
YCTung 3:197b748a397a 56 }