begin constant defs Omega_sq omega_sq b c d % driving frequency f end % begin constant code % The "true constants" defaults Omega_sq = (280.2 / 1.4336); omega_sq = (.667 / 0.394); b = (.667 / 1.436); c = (5.682 / 1.4336); d = (1.371 / 0.394); % make the constants adjustable % by up to a factor of 2 limit_b[LOW] = 0.5 * b; limit_b[HIGH] = 2.0 * b; limit_c[LOW] = 0.5 * c; limit_c[HIGH] = 2.0 * c; limit_d[LOW] = 0.5 * d; limit_d[HIGH] = 2.0 * d; limit_Omega_sq[LOW] = 0.5 * Omega_sq; limit_Omega_sq[HIGH] = 2.0 * Omega_sq; limit_omega_sq[LOW] = 0.5 * omega_sq; limit_omega_sq[HIGH] = 2.0 * omega_sq; limit_f_[LOW] = 0.0; limit_f[HIGH] = 99.0; end % begin states defs % simulated system states x x_dot y y_dot end % begin control defs % only a single motor voltage u end % begin control algorithm u = 24.0 * cos(2.0 * 3.141526 * f * t); end % begin state dynamics % differential equations of the system: CONTROL(); /* evaluate u */ d_dt_x = x_dot; d_dt_x_dot = - Omega_sq * (x - y) - c * x_dot + b * u; d_dt_y = y_dot; d_dt_y_dot = - omega_sq * (y - x) - d * y_dot; end % begin init code % initialize the simulated system x = 0.0; x_dot = 0.0; y = 0.0; y_dot = 0.0; % and finally set the usual runtine parameters: t_INIT = 0.0; T_FINAL = 5.0; H_MAX = .001; N_STEPS = 5000;