ME305 HW0x03 (Main Script)

Written by: Damond Li
Created: 11/23/2021
Updated: 11/25/2021
Description:
This script takes the equations developed in HW 0x02 and uncouples the equations before linearizing using Jacobian Matrices. The uncoupled and linearized equations are then solved using ode45 to simulate the response of the ball and platform system.
Required Files:
Table of Contents
ME305 HW0x03 (Main Script) Introduction Setup Workspace Define Symbolic Variables Establish Kinematic Equations Establish Kinetic Equations Open Loop Simulation Assignment 3a. Assignment 3b. Closed Loop Simulation: Assignment 4. ODE Functions for Simulation Ball Platform ODE with No Motor Input Ball Platform ODE with Motor Controller

Introduction

From HW0x02, we will use the equations derived to develop a simulation that shows the efficacy of our simplified dynamic model.

Setup Workspace

clear; clc;

Define Symbolic Variables

syms x(t) theta(t) g m_b m_p r_c r_b r_g I_b I_p T_x l_p r_m l_r r_p;

Establish Kinematic Equations

With the positional matrix, we can derive it once or twice to yield the velocity or acceleration matrix, respectively.
x_p = [ x + r_c*theta + r_b*theta;
-x*theta + r_c + r_b];
v_p = diff(x_p, t);
a_p = diff(x_p, t, t);
Replace the partial derivatives with their "dotted" symbols.
x_p = subs(x_p , [theta diff(theta,t), diff(theta,t,t), ...
x diff(x,t), diff(x,t,t)], ...
[sym('theta') sym('theta_dot') sym('theta_ddot'), ...
sym('x') sym('x_dot') sym('x_ddot')]);
v_p = subs(v_p , [theta diff(theta,t), diff(theta,t,t), ...
x diff(x,t), diff(x,t,t)], ...
[sym('theta') sym('theta_dot') sym('theta_ddot'), ...
sym('x') sym('x_dot') sym('x_ddot')]);
a_p = subs(a_p , [theta diff(theta,t), diff(theta,t,t), ...
x diff(x,t), diff(x,t,t)], ...
[sym('theta') sym('theta_dot') sym('theta_ddot'), ...
sym('x') sym('x_dot') sym('x_ddot')]);
Convert to symbolic variables rather than symbolic functions by evaluating at t = 0.
x_p = x_p(0);
v_p = v_p(0);
a_p = a_p(0);

Establish Kinetic Equations

Use the equations from HW0x02
M_eq_1 = -m_b*g*r_b*theta - m_b*r_b*x*diff(theta, t)^2 == (-m_b*r_b - I_b / r_b)*a_p(1);
M_eq_2 = m_p*r_g*g*theta + m_b*g*x_p(1) - T_x*l_p/r_m == (I_b/r_b + (r_c+r_b)*m_b)*a_p(1)...
+ 2*m_b*x*diff(theta, t)*diff(x, t) + (I_p + m_p*r_g^2)*diff(theta, t, t) + m_b*x^2*diff(theta, t, t);
Replace partial derivatives with their "dotted" form.
M_eq_1 = subs(M_eq_1 , [theta diff(theta,t), diff(theta,t,t), ...
x diff(x,t), diff(x,t,t)], ...
[sym('theta') sym('theta_dot') sym('theta_ddot'), ...
sym('x') sym('x_dot') sym('x_ddot')]);
M_eq_2 = subs(M_eq_2 , [theta diff(theta,t), diff(theta,t,t), ...
x diff(x,t), diff(x,t,t)], ...
[sym('theta') sym('theta_dot') sym('theta_ddot'), ...
sym('x') sym('x_dot') sym('x_ddot')]);
Create separate variables for x, x_dot, x_ddot, theta, theta_dot, and theta_ddot.
x = sym('x', 'real');
xd = sym('x_dot', 'real');
xdd = sym('x_ddot', 'real');
th = sym('theta', 'real');
thd = sym('theta_dot', 'real');
thdd = sym('theta_ddot', 'real');
Create M and f matrix using kinetic equations.
[M,f] = equationsToMatrix([M_eq_2;M_eq_1],[xdd;thdd])
M = 
f = 
The Matrix is in the form of (M * q_ddot = f) and we would like to isolate q_ddot.
Isolate q_ddot (linear and angular acceleration).
q_ddot = M\f;
We want a state vector that includes linear position, angular position, linear velocity, and angular velocity. The derivative of the state vector can be created with x_dot as the first term, theta_dot as the second term, and q_ddot as the third and fourth term (x_ddot and theta_ddot).
dx_dt = [xd
thd
q_ddot];
From the state vector derivative, we can linearize our equation by creating a Jacobian Matrix with the equations that we have.
Create Jacobian Matrix.
J(1, 1) = diff(dx_dt(1), x);
J(1, 2) = diff(dx_dt(1), th);
J(1, 3) = diff(dx_dt(1), xd);
J(1, 4) = diff(dx_dt(1), thd);
J(2, 1) = diff(dx_dt(2), x);
J(2, 2) = diff(dx_dt(2), th);
J(2, 3) = diff(dx_dt(2), xd);
J(2, 4) = diff(dx_dt(2), thd);
J(3, 1) = diff(dx_dt(3), x);
J(3, 2) = diff(dx_dt(3), th);
J(3, 3) = diff(dx_dt(3), xd);
J(3, 4) = diff(dx_dt(3), thd);
J(4, 1) = diff(dx_dt(4), x);
J(4, 2) = diff(dx_dt(4), th);
J(4, 3) = diff(dx_dt(4), xd);
J(4, 4) = diff(dx_dt(4), thd);
Create a similar matrix but with respect to the input motor torque.
U(1,1) = diff(dx_dt(1), T_x);
U(2,1) = diff(dx_dt(2), T_x);
U(3,1) = diff(dx_dt(3), T_x);
U(4,1) = diff(dx_dt(4), T_x);
Evaluate the Jacobian Matrices by plugging in the values for each variable with x, x_dot, theta, and theta_dot set to zero. We want to evaluate x, x_dot, theta, and theta_dot set to zero because the system is stable at that point. Intuitively, when the state vector is set to zero, the ball is at rest directly above the center of the platform. Ideally the ball will be balanced and remain at rest; therefore the system is stable.
Plug in values and evaluate.
A_ = vpa(subs(J, [x, xd, th, thd, g, r_m, l_r, r_b, r_g, l_p, r_p, r_c, m_b, m_p, I_p, I_b],...
[0, 0, 0, 0, 9.8, 0.06, 0.05, 0.0105, 0.042, 0.110, 0.0325, 0.05, 0.03, 0.4, 0.00188, 0.000001323]), 5)
A_ = 
B_ = vpa(subs(U, [x, xd, th, thd, g, r_m, l_r, r_b, r_g, l_p, r_p, r_c, m_b, m_p, I_p, I_b],...
[0, 0, 0, 0, 9.8, 0.06, 0.05, 0.0105, 0.042, 0.110, 0.0325, 0.05, 0.03, 0.4, 0.00188, 0.000001323]), 5)
B_ = 

Open Loop Simulation

Establish the initial values of the state vector.

Assignment 3a.

The ball is initially at rest on a level platform directly above the center of gravity of the platform and there is no torque input from the motor. Run this simulation for 1 second.
x = [0 % x
0 % theta
0 % x_dot
0]; % theta_dot
Establish the time period to run our simulation.
t = [0 1]; % Run for 1 second
Solve the ODE.
[t, X] = ode45('ballPlatform_noMotor', t, x);
Plot the response.
figure(1);
plot(t, X(:,1)*100);
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 1. \rm Linear Position Over Time for Ball at Rest on Center'});
ylabel('Linear Position, [cm]')
title({'3a. Linear Position vs. Time' ...
' '});
The system should be stable with the state vector set to zero. Intuitively, when the ball is at rest above the center of gravity of the platform, the ball should be perfectly balanced. This means the linear position, angular position, linear velocity, and angular velocity should remain at zero. The plot shows that this is true.
figure(2);
plot(t, X(:,2));
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 2. \rm Angular Position Over Time for Ball at Rest on Center'});
ylabel('Angular Position, [rad]')
title({'3a. Angular Position vs. Time' ...
' '});
The plot behaves as expected. Please refer to comment under Figure 1. for full observation.
figure(3);
plot(t, X(:,3)*100);
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 3. \rm Linear Velocity Over Time for Ball at Rest on Center'});
ylabel('Linear Velocity, [cm/s]')
title({'3a. Linear Velocity vs. Time' ...
' '});
The plot behaves as expected. Please refer to comment under Figure 1. for full observation.
figure(4);
plot(t, X(:,4));
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 4. \rm Angular Velocity Over Time for Ball at Rest on Center'});
ylabel('Angular Velocity, [rad/s]')
title({'3a. Angular Velocity vs. Time' ...
' '});
The plot behaves as expected. Please refer to comment under Figure 1. for full observation.

Assignment 3b.

The ball is initially at rest on a level platform offset horizontally from the center of the platform by 5 [cm] and there is no torque input from the motor. Run this simulation for 0.4 seconds.
x = [0.05 % x
0 % theta
0 % x_dot
0]; % theta_dot
Establish the time period to run our simulation.
t = [0 0.4];
Solve the ODE.
[t, X] = ode45('ballPlatform_noMotor', t, x);
Plot the response.
figure(5);
plot(t, X(:,1)*100);
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 5. \rm Linear Position Over Time for Ball Offset From Center'});
ylabel('Linear Position, [cm]')
title({'3b. Linear Position vs. Time' ...
' '});
According to the plot, when the ball starts at rest off center from the platforms center of gravity, the ball rolls slightly in the negative direction before accelerating in the positive direction. Intuitively, I imagine that when the platform starts rotating, the friction on the ball will act in the same direction as the platform is rolling. This friction force will create a moment about the balls center of mass which rotates the ball in the negative direction. Eventually, acceleration due to gravity will move the ball back in the positive direction.
figure(6);
plot(t, X(:,2));
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 6. \rm Angular Position Over Time for Ball Offset From Center'});
ylabel('Angular Position, [rad]')
title({'3b. Angular Position vs. Time' ...
' '});
With the ball offset from the center of gravity, I think the platform will fall in the direction the ball is offset towards. The graph plotting angular position over time is consistent with this hypothesis.
figure(7);
plot(t, X(:,3)*100);
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 7. \rm Linear Velocity Over Time for Ball Offset From Center'});
ylabel('Linear Velocity, [cm/s]')
title({'3b. Linear Velocity vs. Time' ...
' '});
As described under Figure 5., I would think that the moring platform first causes the ball to initially rotate in the negative direction before accelerating in the positive direction. The plot showing linear velocity over time is consistent with this hypothesis. The ball starts at rest indicated by zero velocity at time t = 0. The ball then accelerates in the negative direction for 0.175 seconds before accelerating in the positive direction.
figure(8);
plot(t, X(:,4));
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 5. \rm Angular Velocity Over Time for Ball Offset From Center'});
ylabel('Angular Velocity, [rad/s]')
title({'3b. Angular Velocity vs. Time' ...
' '});
This plot makes sense in that it starts with a velocity of zero and accelerates in the positive direction. Please refer to comments under Figure 5. and Figure 6. for a more complete description.

Closed Loop Simulation:

Assignment 4.

The ball is initially at rest on a level platform offset horizontally from the center of the platform by 5 [cm] and there is no torque input from the motor. Run this simulation for 0.4 seconds.
x = [0.05 % x
0 % theta
0 % x_dot
0]; % theta_dot
Establish the time period to run our simulation.
t = [0 20];
Solve the ODE.
[t, X] = ode45('ballPlatform_withMotor', t, x);
Plot the response.
figure(9);
plot(t, X(:,1)*100);
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 9. \rm Linear Position Over Time for Ball Offset Center with Controller'});
ylabel('Linear Position, [cm]')
title({'4. Linear Position vs. Time' ...
' '});
With the ball initially offset 5cm from the center, the platform moves the ball in the negative direction until it overshoots past the center. The ball then oscilates about the center for a few seconds until it eventually reaches steady state where the position of the ball is directly on the center of the platform (x = 0).
figure(10);
plot(t, X(:,2));
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 10. \rm Angular Position Over Time for Ball Offset Center with Controller'});
ylabel('Angular Position, [rad]')
title({'4. Angular Position vs. Time' ...
' '});
When the ball is offset in the positive direction, the platform will first rotate in the negative direction to accelerate the ball in the negative direction. When the ball overshoots past the center, the platform will rotate in the positive direction to get the ball to roll back. Eventually the platform will reach steady state where the angular position of the platform is zero. This will happen the position of the ball is directly above the platform's center of gravity (x = 0). According to the graph, the angular position of the platform peaks at about 0.06 radians which is roughly 3.5 degrees. This means that out small angle approximation assumption is valid for this case.
figure(11);
plot(t, X(:,3)*100);
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 11. \rm Linear Velocity Over Time for Ball Offset Center with Controller'});
ylabel('Linear Velocity, [cm/s]')
title({'4. Linear Velocity vs. Time' ...
' '});
This plot is consistent with our hypothesis regarding the bahavior of the system. Please refer to the note under Figure 9. and Figure 10. for the complete discription.
figure(12);
plot(t, X(:,4));
xlabel({'Time, [s]'...
' ' ...
'\bf Figure 12. \rm Angular Velocity Over Time for Ball Offset Center with Controller'});
ylabel('Angular Velocity, [rad/s]')
title({'4. Angular Velocity vs. Time' ...
' '});
This plot is consistent with our hypothesis regarding the bahavior of the system. Please refer to the note under Figure 9. and Figure 10. for the complete discription.

ODE Functions for Simulation

Ball Platform ODE with No Motor Input

ballPlatform_noMotor: Function for ODE45 to simulate the step response of the ball platform system with no input from the motor.
This function uses the a state vector of x, x_dot, theta, and theta_dot. The equation for the derivative of the state vector is determine by the Jacobian matrix evaluated with the state vector at zero.
function xdot = ballPlatform_noMotor(t, x)
xdot(1) = x(3);
xdot(2) = x(4);
xdot(3) = -88935/12928*x(1) + 3.0493*x(2);
xdot(4) = 91875/808 * x(1) + 65.3*x(2);
xdot = xdot';
end

Ball Platform ODE with Motor Controller

ballPlatform_noMotor: Function for ODE45 to simulate the step response of the ball platform system with motor input and controller.
This function uses the a state vector of x, x_dot, theta, and theta_dot. The equation for the derivative of the state vector is determine by the Jacobian matrix evaluated with the state vector at zero. The motor input is also determined through another Jacobian matrix relative to the motor torque.
function xdot = ballPlatform_withMotor(t, x)
T_m = x(1) * 0.3 + x(2) * 0.2 + x(3) * 0.05 + x(4) * 0.02;
xdot(1) = x(3);
xdot(2) = x(4);
xdot(3) = -88935/12928*x(1) + 3.0493*x(2) + T_m * 42.898;
xdot(4) = 91875/808 * x(1) + 65.3*x(2) - 709.06 * T_m;
xdot = xdot';
end