Scribd
Upload
16 views

Code Rev 5

The document defines parameters and initial conditions for simulating a system of two coupled pendulums. It initializes arrays to store the state variables and control input over time. The simulation loop integrates the system dynamics using a sliding mode controller to track nominal trajectories for the pendulum angles. Plots of the nominal vs realized trajectories, control input, errors, and phase spaces are generated to analyze the simulation results.

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
16 views

Code Rev 5

The document defines parameters and initial conditions for simulating a system of two coupled pendulums. It initializes arrays to store the state variables and control input over time. The simulation loop integrates the system dynamics using a sliding mode controller to track nominal trajectories for the pendulum angles. Plots of the nominal vs realized trajectories, control input, errors, and phase spaces are generated to analyze the simulation results.

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, l+1) # Fix: Adjust the length of theta1_arr
theta2_arr = zeros(Float64, l+1) # Fix: Adjust the length of theta2_arr
theta1_dot_arr = zeros(Float64, l+1) # Fix: Adjust the length of theta1_dot_arr
theta2_dot_arr = zeros(Float64, l+1) # Fix: Adjust the length of theta2_dot_arr
u_arr = zeros(Float64, l+1) # Fix: Adjust the length of u_arr

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

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

# 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)

# update state variables


theta1_dot += dt * theta1_ddot
theta1 += dt * theta1_dot
theta2_dot += dt * theta2_ddot
theta2 += dt * theta2_dot

# store updated data


theta1_arr[i+1] = theta1
theta2_arr[i+1] = theta2
theta1_dot_arr[i+1] = theta1_dot
theta2_dot_arr[i+1] = theta2_dot
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")
plot(t, theta1_arr[1:l+1], 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")
plot(t, theta2_arr[1:l+1], 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+1], color="red")
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+1], color="red")
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+1], theta1_dot_arr[1:l+1], 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+1], theta2_dot_arr[1:l+1], color="red")
title("Phase Space for Second Pendulum")
xlabel("Theta2")
ylabel("Theta2_dot")

show()

You might also like