Skip to content

Instantly share code, notes, and snippets.

View Rhyssmcm's full-sized avatar

Rhyss McMullan Rhyssmcm

2 followers · 1 following
View GitHub Profile
@Rhyssmcm
Rhyssmcm / PendulumQuestion5.py
Last active November 19, 2020 20:01
(Q5) Suppose that the inverted pendulum is initially at equilibrium. Its parameters are given in (5.3). A unit impulse is applied to the system at t = 0. Choose the parameters of a PID controller so that the angle θ remains with [−5 ◦ , 5 ◦ ] and θ is below 0.1 ◦ after 0.4 s.
import sympy as sym
import control as ctrl
import numpy as np
import matplotlib.pyplot as plt
m, ell, x3, x4, M, g, F, m = sym.symbols('m, ell, x3, x4, M, g, F, m')
# φ(F, x3, x4)
phi = 4*m*ell*x4**2*sym.sin(x3) + 4*F - 3*m*g*sym.sin(x3)*sym.cos(x3)
phi /= 4*(M+m) - 3*m*sym.cos(x3)**2
dphi_x3 = phi.diff(x3)
dphi_x4 = phi.diff(x4)
@Rhyssmcm
Rhyssmcm / PendulumQuestion4.py
Last active November 19, 2020 20:03
(Q4)The following parameters are given: M = 0.3 kg, (5.3a) m = 0.1 kg, (5.3b) ` = 35 cm, (5.3c) g = 9.81 m/s 2 . (5.3d) Suppose that the inverted pendulum is initially at rest at the upright position. Simulate the trajectories of θ(t) and x(t) upon the action of the force F(t) = sin(100t 2 ). (5.4) Simulate over t ∈ [0, 0.2]s.
import sympy as sym
import numpy as np
import control as ctrl
import matplotlib.pyplot as p
m, ell, x3, x4, M, g, F, m = sym.symbols('m, ell, x3, x4, M, g, F, m')
phi = 4*m*ell*x4**2*sym.sin(x3) + 4*F - 3*m*g*sym.sin(x3)*sym.cos(x3)
phi /= 4*(M+m) - 3*m*sym.cos(x3)**2
dphi_x3 = phi.diff(x3)
dphi_x4 = phi.diff(x4)
@Rhyssmcm
Rhyssmcm / PendulumQuestion 3.py
Last active November 19, 2020 20:04
(Q3)Use sympy to determine closed-form (symbolic) expressions for the impulse, step and frequency response of Gθ and Gx
import sympy as sym
m, ell, x1, x2, x3, x4, M, g, F = sym.symbols('m, ell, x1, x2, x3, x4, M, g, F')
# φ(F, x3, x4)
phi = 4*m*ell*x4**2*sym.sin(x3) + 4*F - 3*m*g*sym.sin(x3)*sym.cos(x3)
phi /= 4*(M+m) - 3*m*sym.cos(x3)**2
dphi_x3 = phi.diff(x3)
dphi_x4 = phi.diff(x4)
dphi_F = phi.diff(F)
@Rhyssmcm
Rhyssmcm / PendulumQuestion2.py
Created November 16, 2020 17:21
(Q2)Derive the transfer function Gθ(s) using the force, F(s), as an input variable and the angle, X3(s), as the output variable. Derive the transfer function Gx(s) using the force, F(s), as an input variable and the horizontal position, X1(s).
import sympy as sym
m, ell, x1, x2, x3, x4, M, g, F, m = sym.symbols('m, ell, x1, x2, x3, x4, M, g, F, m')
# φ(F, x3, x4)
phi = 4*m*ell*x4**2*sym.sin(x3) + 4*F - 3*m*g*sym.sin(x3)*sym.cos(x3)
phi /= 4*(M+m) - 3*m*sym.cos(x3)**2
dphi_x3 = phi.diff(x3)
dphi_x4 = phi.diff(x4)
dphi_F = phi.diff(F)
psi = -3*(m*ell*x4**2*sym.sin(x3)*sym.cos(x3) + F*sym.cos(x3) - (M+m)*g*sym.sin(x3))
@Rhyssmcm
Rhyssmcm / PedulumQuestion1.py
Created November 16, 2020 17:19
(Q1)Use Python to show that the partial derivatives of φ and ψ defined in (3.2)
import sympy as sym
m, ell, x3, x4, M, g, F, m = sym.symbols('m, ell, x3, x4, M, g, F, m')
# φ(F, x3, x4)
phi = 4*m*ell*x4**2*sym.sin(x3) + 4*F - 3*m*g*sym.sin(x3)*sym.cos(x3)
phi /= 4*(M+m) - 3*m*sym.cos(x3)**2
dphi_x3 = phi.diff(x3)
dphi_x4 = phi.diff(x4)
dphi_F = phi.diff(F)
psi = -3*(m*ell*x4**2*sym.sin(x3)*sym.cos(x3) + F*sym.cos(x3) - (M+m)*g*sym.sin(x3))
@Rhyssmcm
Rhyssmcm / lanekeepingQ2part4.py
Created October 28, 2020 13:51
(Q2.4) How may the sampling time, Ts, affect the closed-loop behaviour?
import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
class Car:
def __init__(self, length=2.3, velocity=5., disturbance=0, x=0., y=0., pose=0.):
"""
:param length: The length between the two axles of the car
@Rhyssmcm
Rhyssmcm / lanekeepingQ2part3.py
Created October 28, 2020 13:46
(Q2.3) Determine a triple of PID parameters so that the closed-loop system behaves “nicely,” i.e., it converges fast to the set point without a constant bias and does not exhibit oscillations. Using the triple of Kp, Kd and Ki you chose, produce two plots: (i) u(t) vs time, (ii) the (x, y) trajectory of the system, for t ∈ [0, 50]. Discuss brief…
import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
class Car:
def __init__(self, length=2.3, velocity=5., disturbance=0, x=0., y=0., pose=0.):
"""
:param length: The length between the two axles of the car
@Rhyssmcm
Rhyssmcm / lanekeepingQ2part2.py
Last active October 28, 2020 13:49
(Q2.2) Suppose that the system is controlled with a PD controller. Fix the value of Kp and plot in the same axes the (x, y) trajectory of the system for t ∈ [0, 50] for different values of Kd. Comment briefly on how Kd affects the closed-loop dynamics
import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
class Car:
def __init__(self, length=2.3, velocity=1, x=0, y=0, pose_init=0, disturbance= 0):
self.__length = length
@Rhyssmcm
Rhyssmcm / lanekeepingQ2part1.py
Created October 28, 2020 13:40
(Q2.1) Suppose that the system is controlled with a P controller. Plot in the same axes the (x, y) trajectories of the system for t ∈ [0, 50] for different values of Kp (starting from some initial point). Comment on how Kp affects the closed-loop behaviour
import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
class Car:
def __init__(self, length=2.3, velocity=1, x=0, y=0, pose_init=0, disturbance= 0):
self.__length = length
@Rhyssmcm
Rhyssmcm / LanekeepingQ1part2.py
Last active October 28, 2020 14:51
(Q1.1) Plot the trajectory of the vehicle on the X-Y plane
import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
class PidController:
"""PidController
Documentation goes here
"""