class DCMotorParams():
def __init__(self, J=0.01, b=0.1, K=0.01, R=1, L=0.5):
# (J) moment of inertia of the rotor 0.01 kg.m^2
# (b) motor viscous friction constant 0.1 N.m.s
# (Ke) electromotive force constant 0.01 V/rad/sec
# (Kt) motor torque constant 0.01 N.m/Amp
# (R) electric resistance 1 Ohm
# (L) electric inductance 0.5 H
# Note that in SI units, Ke = Kt = K
self.J = J
self.b = b
self.K = K
self.R = R
self.L = L
class DCMotor():
def __init__(self, x0, params):
self._params = params
self._x0 = x0
self._x = x0
self._J_load = 0
self.update_motor_matrix()
def update_motor_matrix(self):
# state variables are: rotation speed (w, or theta_dot) and current (i)
self._A = np.array([[-self._params.b/(self._params.J+self._J_load), self._params.K/(self._params.J+self._J_load)],
[-self._params.K/self._params.L, -self._params.R/self._params.L]])
self._B = np.array([[0],[1/self._params.L]])
self._C = np.array([1, 0])
self._D = 0;
def reset(self):
self._x = self._x0
def set_load(self, J_load):
self._params.J += J_load
self.update_motor_matrix()
def step(self, dt, u):
self._x = self._x + dt*(self._A@self._x + self._B*u)
def measure_speed(self):
self._y = self._C@self._x
return self._y
motor = DCMotor(np.array([[0], [0]]), DCMotorParams())
y = []
u = 1 # input
time_vector = np.arange(0, 10, 0.01)
for t in time_vector:
motor.step(0.01, u)
y.append(motor.measure_speed())
fig = plt.figure(figsize=(10,5))
plt.plot(time_vector, y, linewidth=3)
plt.xlabel('time (s)')
plt.ylabel('speed (w, rad/s)');
Now we can add a load on the motor to verify how it changes the response
motor.reset()
motor.set_load(0.1)
y = []
u = 1 # input (Volts)
time_vector = np.arange(0, 10, 0.01)
for t in time_vector:
motor.step(0.01, u)
y.append(motor.measure_speed())
fig = plt.figure(figsize=(10,5))
plt.plot(time_vector, y)
plt.xlabel('time (s)')
plt.ylabel('speed (w, rad/s)');
As expected, given than now we have a higher load the motor takes more time to get to its steady state.
The DCMotor defined in the previous class does not include the rotor's position in its state. We are going to slighlty modify the equations to add one state to explicit the rotor's position.
This is the class that we export in our library.
We also define a wrapping function to make sure the rotor position stays within $[-\pi,\pi]$.
motor = DCMotor(x0=np.array([[0], [0], [0]]), params=DCMotorParams())
y = np.array([[0],[0]]) # initialise to 0. This is only here to have the correct size. Alternative: y = np.empty((2,1))
u = 1 # input
time_vector = np.arange(0, 10, 0.01)
for t in time_vector:
motor.step(0.01, u)
y = np.append(y, motor.measure(), axis=1)
# remove the first item which was there only to have the correct size
y = np.delete(y, 0, axis=1)
position = y[0,:]
velocity = y[1,:]
fig, axs = plt.subplots(2, 1, figsize=(10,5))
axs[0].plot(time_vector, velocity, linewidth=3)
axs[0].set_xlabel('time (s)')
axs[0].set_ylabel('speed (w, rad/s)')
axs[1].plot(time_vector, position, linewidth=3)
axs[1].set_xlabel('time (s)')
axs[1].set_ylabel('position (rad)');
plt.tight_layout()
Or if we want to express it as a transfer function: $$ P(s) = \frac{\dot\Theta(s)}{V(s)} = \frac{K}{(Js+b)(Ls+R) + K^2} $$
and if we want to measure the position
$$ \tilde{P}(s) = \frac{\Theta(s)}{V(s)} = \frac{K}{s(Js+b)(Ls+R) + K^2} $$The next step is to have a motor controller so that we can use it in our workspace.
pendulum = Pendulum(theta_0=np.radians(0),
theta_dot_0=0,
params=PendulumParameters(b=0.2))
motor = DCMotor(x0=np.array([[0], [0], [0]]),
params=DCMotorParams())
pid = PID(Kp=1000,
Kd=500,
Ki=1000)
# connect the motor to the pendulum
motor.connect_to(pendulum)
t0, tf, dt = 0, 20, 0.01
time_vector = np.arange(t0, tf, dt)
angle_des = 5 # desired final value
pendulum_angles = []
for t in time_vector:
u = pid.control(dt, y_des=np.radians(angle_des),
y=pendulum.position())
torque = motor.step(dt, u)
pendulum.step(dt, u=torque)
pendulum_angles.append(np.degrees(pendulum.position()))
print('Reference position: {:.4f} rad/{:.1f} deg'.format(np.radians(angle_des), angle_des))
print('Pendulum position (tf): {:.4f} rad/{:.1f} deg'.format(np.radians(pendulum_angles[-1]), pendulum_angles[-1]))
plt.plot(time_vector, pendulum_angles)
plt.plot(time_vector, angle_des*np.ones((len(pendulum_angles))));
plt.xlabel('time (s)')
plt.ylabel('pendulum position (deg)');
plt.ylim(0, angle_des+1);
plt.grid()
pendulum = Pendulum(theta_0=np.radians(0),
theta_dot_0=0,
params=PendulumParameters(b=0.2))
motor = DCMotor(x0=np.array([[0], [0], [0]]),
params=DCMotorParams())
pid = PID(Kp=1000,
Kd=500,
Ki=1000)
# connect the motor to the pendulum
motor.connect_to(pendulum)
t0, tf, dt = 0, 20, 0.01
sim = Simulator(pendulum, motor, pid)
sim.y_des(10) # desired final value in degrees
results = sim.run(t0, tf, dt)
fig, axs = plt.subplots(len(results.keys())-1, 1, figsize=(10,10))
for index, key in enumerate(results.keys()):
if key == 'time (s)': continue
if key == 'position (rad)':
axs[index].plot(results['time (s)'], np.degrees(results[key]), linewidth=3)
axs[index].plot(results['time (s)'], sim.y_des*np.ones(results['time (s)'].shape), color='r', linewidth=3)
else:
axs[index].plot(results['time (s)'], results[key], linewidth=3)
axs[index].set_xlabel('time (s)')
axs[index].set_ylabel(key)
axs[index].grid()
plt.tight_layout()
pendulum = Pendulum(theta_0=np.radians(0),
theta_dot_0=0,
params=PendulumParameters(b=0.2))
motor = DCMotor(x0=np.array([[0], [0], [0]]),
params=DCMotorParams())
pid = PID(Kp=1000,
Kd=500,
Ki=1000)
# connect the motor to the pendulum
motor.connect_to(pendulum)
t0, tf, dt = 0, 10, 1./30 # time
sim = Simulator(pendulum, motor, pid)
sim.y_des(10) # desired final value in degrees
results = sim.run(t0, tf, dt)
ap = AnimateControlledPendulum(sim)
ap.start_animation(t0, tf, dt)
