class armController:
def __init__(self):
# Instantiates the PD object
self.thetaCtrl = PDControl(P.kp, P.kd, P0.tau_max, P.beta, P.Ts)
self.limit = P0.tau_max
def u(self, y_r, y):
# y_r is the referenced input
# y is the current state
theta_r = y_r[0]
theta = y[0]
# compute equilibrium torque tau_e
tau_e = P0.m * P0.g * (P0.ell / 2.0) * np.cos(theta)
# compute the linearized torque using PD
tau_tilde = self.thetaCtrl.PD(theta_r, theta, False)
# compute total torque
tau = tau_e + tau_tilde
tau = self.saturate(tau)
return [tau]
def saturate(self, u):
if abs(u) > self.limit:
u = self.limit*np.sign(u)
return u
class pendulumController:
'''
This class inherits other controllers in order to organize multiple controllers.
'''
def __init__(self):
# Instantiates the SS_ctrl object
# self.zCtrl = PDControl(P.kp_z, P.kd_z, P.theta_max, P.beta, P.Ts)
self.thetaCtrl = PDControl(P.kp_th, P.kd_th, P.F_max, P.beta, P.Ts)
def u(self, y_r, y):
# y_r is the referenced input
# y is the current state
theta_r = y_r[0]
# z = y[0]
theta = y[0]
# print('theta', theta)
# print('theta_r' , theta_r)
# print('control', self.thetaCtrl.PD(theta_r, theta, flag=False))
# the reference angle for theta comes from the outer loop PD control
# theta_r = self.zCtrl.PD(z_r, z, flag=False)
# the force applied to the cart comes from the inner loop PD control
F = -P.m * P.g * P.ell * np.sin(theta) / (
P.m * P.ell * P.ell) + self.thetaCtrl.PD(
theta_r, theta, flag=False)
return [F]
class satelliteController:
'''
This class inherits other controllers in order to organize multiple controllers.
'''
def __init__(self):
# Instantiates the SS_ctrl object
self.phiCtrl = PDControl(P.kp_phi, P.kd_phi, P.theta_max, P.beta, P.Ts)
self.thetaCtrl = PDControl(P.kp_th, P.kd_th, P.tau_max, P.beta, P.Ts)
def u(self, y_r, y):
# y_r is the referenced input
# y is the current state
phi_r = y_r[0]
theta = y[0]
phi = y[1]
# the reference angle for theta comes from the outer loop PD control
theta_r = self.phiCtrl.PD(phi_r, phi, flag=False)
# the torque applied to the base comes from the inner loop PD control
tau = self.thetaCtrl.PD(theta_r, theta, flag=False)
return [tau]
def __init__(self):
# Instantiates the SS_ctrl object
self.zCtrl = PDControl(P.kp_z, P.kd_z, P.theta_max, P.beta, P.Ts)
self.thetaCtrl = PDControl(P.kp_th, P.kd_th, P.F_max, P.beta, P.Ts)
def __init__(self):
# Instantiates the PD object
self.thetaCtrl = PDControl(P.kp, P.kd, P0.tau_max, P.beta, P.Ts)
self.limit = P0.tau_max