(1) % MotionGenesis file:  MGRocketSledOnInclinedPlaneFeedForwardControlX.txt
   (2) % Copyright (c) 2009-2016 Motion Genesis LLC.  All rights reserved.
   (3) %-------------------------------------------------------------
   (4) NewtonianFrame  N                      % Nx> is directed upward along inclined plane.
   (5) Particle        Q                      % Rocket-sled (modeled as a particle).
   (6) %-------------------------------------------------------------
   (7) Variable   x''                         % Upward position along inclined plane.
   (8) Specified  xDesired''                  % Desired behavior for x.
   (9) Specified  Fc                          % Control force (to be designed).
   (10) Specified  Funmodeled = 40*sin(t)^2    % Unmodeled disturbance force.
-> (11) Funmodeled = 40*sin(t)^2

   (12) Constant   theta = 30 degs             % Angle of inclined plane.
   (13) Constant   g = 9.8 m/s^2               % Earth's gravitational acceleration.
   (14) Constant   b = 0.5 kg/m                % Air-resistance coefficient.
   (15) Q.SetMass( m = 50 kg )
   (16) %-------------------------------------------------------------
   (17) %     Translational kinematics.
   (18) Q.Translate(  No,  x*Nx>  )
-> (19) p_No_Q> = x*Nx>
-> (20) v_Q_N> = x'*Nx>
-> (21) a_Q_N> = x''*Nx>

   (22) %-------------------------------------------------------------
   (23) %     Relevant forces.
   (24) GravityDirection> = -sin(theta)*Nx> - cos(theta)*Ny>
-> (25) GravityDirection> = -sin(theta)*Nx> - cos(theta)*Ny>

   (26) AirResistance> = -b*abs(x')*x'*Nx>
-> (27) AirResistance> = -b*x'*abs(x')*Nx>

   (28) Q.AddForce(  m*g*GravityDirection> + AirResistance> + Fc*Nx> - Funmodeled*Nx>  )
-> (29) Force_Q> = (Fc-m*g*sin(theta)-Funmodeled-b*x'*abs(x'))*Nx> - m*g*cos(
        theta)*Ny>

   (30) %-------------------------------------------------------------
   (31) %     Equation of motion
   (32) Dynamics = Dot( Nx>,  Q.GetDynamics() )
-> (33) Dynamics = m*g*sin(theta) + Funmodeled + b*x'*abs(x') + m*x'' - Fc

   (34) %-------------------------------------------------------------
   (35) %     Integration parameters and initial values for x and x'.
   (36) Input  tFinal = 12 sec,  tStep = 0.05 sec,  absError = 1.0E-7,  x = 0 m,  x' = 0 m/s
   (37) %-------------------------------------------------------------
   (38) %     List output parameters to be plotted.
   (39) OutputPlot  t sec,  xDesired meters,  x meters
   (40) OutputPlot  t sec,  Fc Newtons
   (41) %*************************************************************
   (42) %****** Build feed-forward control for x (append method) *****
   (43) %*************************************************************
   (44) %     Step 1:  Pick whatever you want for xDesired.
   (45) SetDt( xDesired = 2 + sin(pi/4*t) )
-> (46) xDesired = 2 + sin(0.7853982*t)
-> (47) xDesired' = 0.7853982*cos(0.7853982*t)
-> (48) xDesired'' = -0.6168503*sin(0.7853982*t)

   (49) %-------------------------------------------------------------
   (50) %     Step 2:  Construct feed-forward ControlEqn governing Fc.
   (51) Constant  wn = 1.5 rad/sec;   kd = 2*wn;   kp = wn^2
-> (52) kd = 2*wn
-> (53) kp = wn^2

   (54) Ax = xDesired''  +  kd * (xDesired' - x')  +  kp * (xDesired - x)
-> (55) Ax = xDesired'' + kp*(xDesired-x) + kd*(xDesired'-x')

   (56) ControlEqn = Evaluate( Dynamics,  x'' = Ax,  Funmodeled = 0 )
-> (57) ControlEqn = m*g*sin(theta) + b*x'*abs(x') + m*Ax - Fc

   (58) %-------------------------------------------------------------
   (59) %     Step 3:  Append ControlEqn to Dynamics and simulate actual x'' (with non-zero Funmodeled).
   (60) ODE( [Dynamics; ControlEqn],  x'', Fc ) MGRocketSledOnInclinedPlaneFeedForwardControlX

   (61) %-------------------------------------------------------------