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@13:51ef67cd4fd6, 2017-05-17 (annotated)

Committer:
cpul5338
Date:
Wed May 17 15:22:29 2017 +0000
Revision:
13:51ef67cd4fd6
Parent:
11:45a641da473d 
template

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"
cpul5338 11:45a641da473d 3 AnalogIn VariableResistor1_value(A0);
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 10:3b952ea7fad4 10 //float K_mphi[3]= {3.7165*0.03, 2.6747*0.03, -2.0436*0.03};
YCTung 8:79ca11e0129d 11 float K_mphi[3]= {6.3409*0.03, 4.1028*0.03, -5.0338*0.03};
YCTung 3:197b748a397a 12 float u = 0.0;
YCTung 3:197b748a397a 13 float phi_hat = 0.0;
YCTung 3:197b748a397a 14 float phi_hat_old = 0.0;
YCTung 3:197b748a397a 15 float phi_hat_aw = 0.0;
YCTung 3:197b748a397a 16 float dphi_hat= 0.0;
YCTung 3:197b748a397a 17 float roll_ref = 0.0;
YCTung 3:197b748a397a 18 float gamma_ref = 0.0;
YCTung 3:197b748a397a 19 float gamma_rad = 0.0;
YCTung 3:197b748a397a 20 float gamma_rad_old = 0.0;
YCTung 3:197b748a397a 21 int8_t gamma_degree = 0;
YCTung 3:197b748a397a 22
YCTung 3:197b748a397a 23 void calc_PD(float* K_in, float phi_hat_in)
YCTung 3:197b748a397a 24 {
YCTung 3:197b748a397a 25 u = -K_in[0]*(roll_angle - phi_hat_in - roll_ref) - K_in[1]*droll_angle;
YCTung 3:197b748a397a 26 }
YCTung 3:197b748a397a 27
YCTung 3:197b748a397a 28 void calc_Phi(float* K_phi_in)
YCTung 3:197b748a397a 29 {
YCTung 3:197b748a397a 30 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 31 phi_hat = phi_hat_old + (dphi_hat + phi_hat_aw)*sample_time;
YCTung 3:197b748a397a 32 }
YCTung 3:197b748a397a 33
YCTung 3:197b748a397a 34 void calc_Gamma(float u_in, float alpha, float multr)
YCTung 3:197b748a397a 35 {
YCTung 3:197b748a397a 36 gamma_rad = lpf(u_in,gamma_rad_old,alpha)/multr - GAMMA_OFFSET; ///38
YCTung 3:197b748a397a 37 gamma_rad_old = gamma_rad;
YCTung 3:197b748a397a 38 }
YCTung 3:197b748a397a 39
YCTung 3:197b748a397a 40 void anti_wdup(void)
YCTung 3:197b748a397a 41 {
YCTung 3:197b748a397a 42 if(phi_hat > PHI_HAT_UB)
YCTung 3:197b748a397a 43 {
YCTung 3:197b748a397a 44 phi_hat_aw = K_aw * (PHI_HAT_UB - phi_hat);
YCTung 3:197b748a397a 45 phi_hat = PHI_HAT_UB;
YCTung 3:197b748a397a 46 }
YCTung 3:197b748a397a 47 else if(phi_hat < PHI_HAT_LB)
YCTung 3:197b748a397a 48 {
YCTung 3:197b748a397a 49 phi_hat_aw = K_aw * (PHI_HAT_LB - phi_hat);
YCTung 3:197b748a397a 50 phi_hat = PHI_HAT_LB;
YCTung 3:197b748a397a 51 }
YCTung 3:197b748a397a 52 else
YCTung 3:197b748a397a 53 {
YCTung 3:197b748a397a 54 phi_hat_aw = 0.0;
YCTung 3:197b748a397a 55 }
YCTung 3:197b748a397a 56 phi_hat_old = phi_hat;
YCTung 3:197b748a397a 57 }