Scribd
Upload
20 views

Code Rev 2

The document contains code to simulate the dynamics of a double pendulum system using a sliding mode controller. It defines parameters for the system, controller, and simulation. State variables are initialized and stored in arrays as the system is simulated over time. Plots are generated to show the nominal vs realized trajectories, control signal, tracking errors, and phase spaces for each pendulum.

Uploaded by

Sheikh Robin
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
20 views

Code Rev 2

The document contains code to simulate the dynamics of a double pendulum system using a sliding mode controller. It defines parameters for the system, controller, and simulation. State variables are initialized and stored in arrays as the system is simulated over time. Plots are generated to show the nominal vs realized trajectories, control signal, tracking errors, and phase spaces for each pendulum.

Uploaded by

Sheikh Robin
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 3

using PyPlot

# Control Parameters
K = 500
w = 1
Lambda = 3

# Simulation properties
dt = 1e-3
LONG = 100000
l = Int(LONG-1)

# System Parameters
m1 = 1 # mass of the first pendulum
m2 = 1 # mass of the second pendulum
L1 = 1 # length of the first pendulum
L2 = 1 # length of the second pendulum
g = 9.81 # acceleration due to gravity

# Initial conditions
theta1 = 0.1 # initial angle of the first pendulum
theta2 = 0.1 # initial angle of the second pendulum
theta1_dot = 0 # initial angular velocity of the first pendulum
theta2_dot = 0 # initial angular velocity of the second pendulum

# Nominal Trajectories
theta1_nom = pi * ones(l+1) # Fix 1: Create an array of nominal angles
theta2_nom = zeros(l+1) # Fix 1: Create an array of nominal angles

# Arrays to store data


theta1_arr = zeros(Float64, LONG)
theta2_arr = zeros(Float64, LONG)
theta1_dot_arr = zeros(Float64, LONG)
theta2_dot_arr = zeros(Float64, LONG)
u_arr = zeros(Float64, LONG)

# Initialization
theta1 = theta1_arr[1]
theta2 = theta2_arr[1]
theta1_dot = theta1_dot_arr[1]
theta2_dot = theta2_dot_arr[1]

# Simulation
for i in 1:l
# store data
theta1_arr[i] = theta1
theta2_arr[i] = theta2
theta1_dot_arr[i] = theta1_dot
theta2_dot_arr[i] = theta2_dot

# error and its derivative


e1 = theta1_nom[i] - theta1
e2 = theta2_nom[i] - theta2
e1_dot = -theta1_dot
e2_dot = -theta2_dot

# sliding surface
S1 = Lambda * e1 + 2 * Lambda * e1_dot # Fix 3: Correct the sliding surface
calculation
S2 = Lambda * e2 + 2 * Lambda * e2_dot # Fix 3: Correct the sliding surface
calculation

# control input (torque at the joint between the two pendulums)


u = m2 * L2 * (K * tanh(S1 / w) + K * tanh(S2 / w))
u_arr[i] = u

# system dynamics
theta1_ddot = (m2 * L2 * theta2_dot^2 * sin(theta2-theta1) + (m1+m2) * g *
sin(theta1)) / ((m1 + m2) * L1)
theta2_ddot = (u - m2 * L1 * theta1_dot^2 * sin(theta2-theta1) - m2 * g *
sin(theta2)) / (m2 * L2) # Fix 6: Correct the dynamics equation

# update state variables


theta1_dot += dt * theta1_ddot # Fix 6: Use compound assignment operator +=
theta1 += dt * theta1_dot # Fix 6: Use compound assignment operator +=
theta2_dot += dt * theta2_ddot # Fix 6: Use compound assignment operator +=
theta2 += dt * theta2_dot # Fix 6: Use compound assignment operator +=
end

# Plotting
t = dt:dt:l*dt

# Plotting Nominal and Realized Trajectory for First Pendulum


figure()
subplot(2, 3, 1)
plot(t, theta1_nom[1:l+1], label="Nominal", color="black") # Fix 1: Use the
nominal trajectory array
plot(t, theta1_arr[1:l], label="Realized", color="red")
title("Nominal and Realized Trajectory for First Pendulum")
xlabel("Time")
ylabel("Angle")
legend()

# Plotting Nominal and Realized Trajectory for Second Pendulum


subplot(2, 3, 2)
plot(t, theta2_nom[1:l+1], label="Nominal", color="black") # Fix 1: Use the
nominal trajectory array
plot(t, theta2_arr[1:l], label="Realized", color="red")
title("Nominal and Realized Trajectory for Second Pendulum")
xlabel("Time")
ylabel("Angle")
legend()

# Plotting Control Signal


subplot(2, 3, 3)
plot(t, u_arr[1:l], color="red")
title("Control Signal")
xlabel("Time")
ylabel("Control Input")

# Plotting Tracking Error for First Pendulum


subplot(2, 3, 4)
plot(t, theta1_nom[1:l+1] - theta1_arr[1:l], color="red") # Fix 1: Use the nominal
trajectory array
title("Tracking Error for First Pendulum")
xlabel("Time")
ylabel("Error")
# Plotting Tracking Error for Second Pendulum
subplot(2, 3, 5)
plot(t, theta2_nom[1:l+1] - theta2_arr[1:l], color="red") # Fix 1: Use the nominal
trajectory array
title("Tracking Error for Second Pendulum")
xlabel("Time")
ylabel("Error")

# Plotting Phase Space for First Pendulum


subplot(2, 3, 6)
plot(theta1_arr[1:l], theta1_dot_arr[1:l], color="red")
title("Phase Space for First Pendulum")
xlabel("Theta1")
ylabel("Theta1_dot")

tight_layout()

# Plotting Phase Space for Second Pendulum


figure()
plot(theta2_arr[1:l], theta2_dot_arr[1:l], color="red")
title("Phase Space for Second Pendulum")
xlabel("Theta2")
ylabel("Theta2_dot")

show()

You might also like