def __init__(self, K, tau, omega, Ts):
self.K = K # Motor gain
self.tau = tau # Time Constant
self.omega = omega # initial state
self.Ts = Ts
self.intg_omega = get_integrator(Ts, self.omega_dot)
self.intg_theta = get_integrator(Ts, self.theta_dot)
def __init__(self, J, d, b, omega, Ts):
self.J = J # Moment of Inertia
self.d = d # Distance between props and center of rotation
self.b = b # Damping coefficient
self.omega = omega # initial state
self.Ts = Ts
self.intg_omega = get_integrator(Ts, self.omega_dot)
self.intg_theta = get_integrator(Ts, self.theta_dot)
def __init__(self):
self.state = State()
self.state.timestep = ts_simulation
self.rigid_body = RigidBody()
self.gravity = Gravity(self.state)
self.wind = Wind(self.state)
self.aero = Aerodynamics(self.state)
self.thrust = Thrust(self.state)
self.intg = get_integrator(self.state.timestep, self.eom)
def __init__(self):
self.integ = get_integrator(P.Ts,model_DCMotor)
def f(t, x, u):
return np.array([x[1], -1 / P.tau * x[1] + P.K / P.tau * u])
# Initial Conditions
r0 = 1
theta0 = 0
omega0 = 0
t0 = 0
x0 = np.array([theta0, omega0])
C = PIDControl(P.kp, P.ki, P.kd, P.umax, P.sigma, P.Ts, flag=True)
plant = System(P.K, P.tau, omega0, P.Ts)
integrator_RK4 = intg.get_integrator(P.Ts, System.f)
# Simulate step response
t_history = [t0]
x_history = [x0]
u_history = [0]
r = r0
x = np.array([theta0, omega0])
t = t0
for i in range(P.nsteps):
u = C.PID(r, x[0])
t += P.Ts
x = integrator_RK4.step(t, x, u)
t_history.append(t)