Listing 1. System Code for Simulation of Servo-Driven Carts

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;