/* Disturbance compensation PD controller */ theta_motor_ref = ref_dat[j]; thdot_motor = ( theta_motor - theta_motor_old ) / STIME; theta_motor_old = theta_motor; thdot_motor_ref = ( theta_motor_ref - theta_motor_ref_old ) / STIME; theta_motor_ref_old = theta_motor_ref; /*the observer*/ thdot_motor_obs_r = obs_22_r * thdot_motor_obs_r + obs_b2_r * control_u/U_gain + obs_b2_r * sw -obs_b2_r*k3*theta_motor; /*calculatin of error and manifold */ theta_motor_err = theta_motor_ref - theta_motor; thdot_motor_err = thdot_motor_ref - thdot_motor; s_manifold_r = thdot_motor - thdot_motor_obs_r; if (s_manifold_r > 0) { sw =z_1 + obs_gain; }else{ sw =z_1 - obs_gain; }; ztmp_1 = zd11* z_1 +zd12* z_2 +zd13* z_3 + bzd1*sw; ztmp_2 = zd21* z_1 +zd22* z_2 +zd23* z_3 + bzd2*sw; z_3 = zd31* z_1 +zd32* z_2 +zd33* z_3 + bzd3*sw; z_1 = ztmp_1; z_2 = ztmp_2; /*if (time>3.2) z_1 = -10;*/ control_u = (k1 * theta_motor_err - k2 * thdot_motor - z_1 + k3* theta_motor)*U_gain;