%% *Model Overview*
% In this tutorial, you model a simple link with two end frames that you can later connect to joints. Rigid Transform blocks provide the end frames,
% while a Solid block provides geometry, inertia, and color. For simplicity, the model assumes the link has a brick shape.
% Building a Simple pendulum involves building a model of a simple link and
% using the link to create the model of a simple pendulum. You will then
% perfrom analysis on the pendulum. The analysis involves plotting the
% angle and angular velocity of the pendulum in damped and undamped cases.
% You can also drive the pendulum with an external force and plot its
% response but we will not be doing it today. You can obtain more
% information from the following link.
%%
% <http://uk.mathworks.com/help/physmod/sm/gs/analyze-simple-pendulum.html>

%% Build model of a Simple Link
% 
% * At the MATLAB® command line, enter |smnew|. The SimMechanics™ block library and a model template with commonly used blocks open up.
% * Make a copy of the Rigid Transform block and paste it in the model. The Rigid Transform blocks enable you to create new frames to which you can connect joints during multibody assembly.
% * Delete the blocks Simulink-PS Converter, PS-Simulink Converter, and Scope. You do not need these blocks in this tutorial.
% * Connect the remaining blocks as shown in the figure. Ensure that the base frame ports (B) of the Rigid Transform blocks both face the Solid block frame port. 
% Since each Rigid Transform block applies a spatial transformation with respect to its base frame, switching port connections generally changes the spatial relationship between the two frames.
%
%
% <<simpleLink.png>>
% 
% * In the Solid block dialog box, specify the following parameters. Later, you define the MATLAB variables shown using a Subsystem block that contains the Solid and Rigid Transform blocks. Among its advantages, 
% this approach enables you to update variables used in multiple blocks from a single place—the Subsystem block dialog box
%
%
% 
% <<table1.png>>
%
% * In the dialog boxes of the Rigid Transform and Rigid Transform1 blocks, specify the following parameters. 
% These parameters encode the offset between the base and follower port frames of the Rigid Transform blocks, located at the link ends, with respect to the Solid reference port frame.
%
%
%
% <<table2.png>>
%
%
%% *Generate Subsystem (Optional)*
% If you do not want to create a subsystem, please enter the values of the
% parameters directly into the properties of the solid.
% * Select the Solid block and the two Rigid Transform blocks.
% * Right-click the highlighted region and select Create Subsystem from Selection. 
% Simulink® adds a new Subsystem block containing the Solid and Rigid Transform blocks. At the end of the tutorial, this will be a custom block representing the simple link rigid body.
%
%
%
% <<subsytem.png>>
%
%
% * Right-click the Subsystem block, and select *Mask > Create Mask*. 
% A mask editor opens up, enabling you to specify the numerical values of the MATLAB variables you entered in the Solid and Rigid Transform block dialog boxes.
% * In the *Parameters & Dialog* tab of the Mask Editor window, add five edit fields to the *Parameters* folder. You can find this folder in the Dialog box pane. 
% In the edit fields, specify the following parameters and click *OK*. 
% Prompt is the desired text for each parameter in the Subsystem block dialog box. *Name* is the MATLAB variable associated with each Subsystem block parameter.
%
%
%
% <<prompttable.png>>
%
%
% * Double-click the Subsystem block dialog box and enter the following numerical values.
% These are the values of the MATLAB variables that you entered in the Solid and Rigid Transform block dialog boxes.
%
%
%
% <<values.png>>
%
%
%% Visualize Model
% Update the block diagram. You can do this by selecting, in the Simulink menu bar, *Simulation > Update Diagram*. 
% Mechanics Explorer opens with a front view of the simple link model. In the Mechanics Explorer toolstrip, select the isometric view button
% to obtain the 3-D view shown below. To view the frames present in the model—including those you created using the Rigid Transform blocks—select *View > Show Frames* in the Mechanics
% Explorer menu bar
%
%
%
% <<bar.png>>
%
%

%% Model Simple Pendulum
% The pendulum is the simplest mechanical system you can model. This system contains two rigid bodies, a link and a fixed pivot, connected by a revolute joint. In this part of the tutorial, 
% you model and simulate a pendulum using the custom link block you created in Model Simple Link. 
% A Revolute Joint block provides the rotational degree of freedom between the link and the world frame.


%% Build Model
%
% * Use the  Simple Link custom block you created in the previous part of
% the tutorial to "Model Simple Link"
% * Drag a Revolute Joint block into the model. You can find this block in the *SimMechanics Second Generation > Joints library*. 
% This block provides one rotational degree of freedom between its port frames.
% Connect the blocks as shown in the figure. The port orientation of the Revolute Joint block becomes important when you 
% specify joint state targets, prescribe joint actuation inputs, or sense joint dynamic variables. 
% The Revolute Joint block interprets each quantity as that applied to the follower frame with respect to the base frame, 
% so switching the port connections can affect model assembly and simulation.
%
%
%
% <<simplePend.png>>
%
%
% * In the Solid block dialog box, specify the following parameters. 
% This block connects rigidly to the World frame and therefore has no effect on model dynamics. You can leave the inertia parameters in their default values.
%
%
%
% <<tablePend.png>>
%
%
%
%% Specify Gravity
% The Revolute Joint block uses the common Z axis of the base and follower frames as the joint rotation axis. To ensure the pendulum oscillates under the effect of gravity, change 
% the gravity vector so it no longer aligns with the Z axis. To do this, in the Mechanism Configuration block dialog box, set the *Uniform Gravity > Gravity* parameter to [0 -9.81 0].

%% Set Pendulum Starting Position
% You can specify the desired joint angle using the *State Targets* menu in the Revolute Joint block dialog box. 
% To do this, select *State Targets > Position* and enter the desired joint angle. For this tutorial, 
% you can leave the angle in its default value, which corresponds to a horizontal pendulum starting position.

%% Configure Solver
% * In the Simulink® Editor menu bar, select *Simulation > Model Configuration* Parameters.
% * In the *Solver* tab, set the *Solver* parameter to ode15s (stiff/NDF). This solver is the recommended choice for physical models.
% Set *Max step size* to 0.01 and click *OK*. The small step size increases the simulation accuracy and produces a smoother animation in Mechanics Explorer. 
% Small step sizes can have a detrimental effect on simulation speed but, in such a simple model, a value of 0.01 provides a good balance between simulation speed and accuracy.
%% Assemble Model
% Update the block diagram. You can do this in the Simulink Editor menu bar, by selecting Simulation > Update diagram. Mechanics Explorer opens with a 3-D view of the model in its initial configuration.
% In the Mechanics Explorer toolstrip, check that the *View convention* parameter is set to Y up (XY Front).
% This view convention ensures that gravity is vertically aligned on your screen.
% Select a standard view button to refresh the Mechanics Explorer display. 
% The figure shows a front view of the model. Save the visualization
% settings by clicking the Save explorer configuration to model button.
%
%
%
% <<finalPend.png>>
%
%
%
%% Simulate Model
% Run the simulation. You can do this through the Simulink Editor menu bar, by selecting *Simulation > Run*. Mechanics Explorer plays a physics-based animation of the pendulum model.

%% Analyse simple Pendulum
% In this part of the tutorial, you explore the various forces and torques that you can add to a model. 
% Then, using blocks with motion sensing capability, you analyze the resulting dynamic response of the model. 
% The end result is a set of time-domain and phase plots, one for each combination of forces and torques. 
% You create these plots using MATLAB® commands with SimMechanics™ motion outputs as arguments.
% 
%% Sense Pendulum motion.
% * In the Sensing menu of the Revolute Joint block dialog box, select the following variables:
% # Position
% # Velocity
% The block exposes two additional physical signal ports, labeled q and w, that output the angular position and velocity of the pendulum with respect to the world frame.
% * Drag 2 *PS-Simulink Converter* blocks from *Simscape > Utilities* in
% Simulink Library
% * Drag 2 *To Workspace* blocks from *Simulink > Sinks* in
% Simulink Library
% * Change the Variable name parameters in the To Workspace block dialog boxes to |q| and |w|. These variables make it easy 
% to identify the joint variables that the To Workspace blocks output during simulation—position, through the Revolute Joint block port q, 
% and velocity, through the Revolute Joint block port w.
% * Connect the blocks as shown in the figure. Ensure that the To Workspace block with variable 
% name q connects, through the PS-Simulink Converter block, to the Revolute Joint block port q, and that the 
% To Workspace block with variable name w connects to the Revolute Joint block port w.
%
%
%
% <<simplePendAna.png>>
%
%
%
% * Save the mdoel under a different name, e.g, |simple_pendulum_analysis|.
%% Automate the analysis process
% To automate the analysis process, open the Simple Pendulum model either by clicking on it in the folder
% browser or by using the following command in your command window. You can then copy these commands into a file and save it. 
% Run the simulation. You can do this through the Simulink Editor menu bar,
% by selecting *Simulation > Run*.

 open_system('SimplePendulum');
% 
 dampCoeff = 0; % Set the damping coefficient to zero for undamped pendulum oscillations
 sim('SimplePendulum');
% 
% 
%% Plot the position and angular velocity of the pendulum 
figure; % Open a new figure
hold on;
plot(q); % Plot the pendulum angle
plot(w); % Plot the pendulum angular velocity
title('Angle and angular velocity of the pendulum'); %title
xlabel('Time (s)'); % xlabel
ylabel('Angle (deg), Angular velocity(deg/s)'); % ylabel
% 
%% Phase plot of the pendulum
figure;
plot(q.data, w.data);

%% Try simulating the model using different starting angles.
% You can change the starting angle in the State Targets > Position menu of the 
% Revolute Joint block dialog box. 
% The figure shows a compound phase plot for starting angles of -80, -40, 0, 40, and 80 degrees.

%% Damped Pendulum
% In the Revolute Joint block dialog box, set *Internal Mechanics > Damping* to 8e-5 (N*m)/(deg/s) or give it a variable name that you can
% from MATLAB command line. In this example, this variable is called |dampCoeff|.
% The damping coefficient causes energy dissipation during motion,
% resulting in a gradual decay of the pendulum oscillation amplitude. 
% Ensure that State Targets > Position > Value is set to 0 deg and run the
% simulation
dampCoeff = 8e-5;
sim('SimplePendulum');

%% Plot damped oscillations
figure; % Open a new figure
hold on;
plot(q); % Plot the pendulum angle
plot(w); % Plot the pendulum angular velocity
title('Angle and angular velocity of a damped pendulum'); %title
xlabel('Time (s)'); % xlabel
ylabel('Angle (deg), Angular velocity(deg/s)'); % ylabel

% Phase plot damped oscillations
figure;
plot(q.data, w.data);

%% Try simulating the model using different starting angles. 
% You can change the starting angle in the *State Targets > Position* menu of the Revolute Joint block dialog box. 
% The figure shows a compound phase plot for starting angles of -240, -180, -120, -60, 0, and 60 degrees.