The next lines setup some things and import the various libraries required to run this notebook.
%matplotlib notebook
import numpy as np
import matplotlib.pyplot as plt
import math
import time
from pyctrl.block import ShortCircuit, Wrap
from pyctrl.block.system import Differentiator, Gain, Feedback, System, Sum, Subtract
from pyctrl.block import Interp, Logger, Constant
from pyctrl.system.tf import PID
from pyctrl.client import Controller
HOST, PORT = "192.168.8.1", 9999
mip = Controller(host = HOST, port = PORT)
mip.reset()
print(mip.info('all'))
In closed-loop control, the control input, in this case the voltage controlled by the pwm
signal, is also produced by an algorithm, the controller. However, in addition to the reference signal, in this case a velocity, the controller also responds to a measurement, in this case the velocity, which is fedback into the signal producing the measurement as shown in the following block-diagram:
With the purpose of analyzing the resulting closed-loop system assume that the motor and the controller are both well represented by the linear models:
$$ \begin{aligned} v(x) &= g \, x, & x(e) &= k \, e \end{aligned} $$where $e$ represents the error signal in the above diagram.
Feedback can be used to control any quantity which can be measured. The underlying algorithm is often based on the exact same diagram you studied before. For example, if one would like to control the position of the MIP rather than its velocity, then one could use position feedback as in the following diagram:
As you studied before, the relation between the pwm
inputs, the forward velocity, $v$, and the angular velocity, $\omega$, can be represented by the relations:
where $r$ is the radius of the wheels and $d$ is the distance between the wheels. The above relationships can be represented by the block diagram:
Because the angular velocity, $\omega$, is the derivative of the heading angle, $\theta$, steering control can be performed based on the following diagram:
where $\bar{\theta}$ is the reference angle and $\overline{\mathrm{pwm}}$ is a open-loop control input for the forward velocity. Of course one could also close the loop on the forward velocity as well.
The signal corresponding to the heading angle $\theta$ in the MIP is the signal yaw
.
mip.reset()
T = 5
K = 10
# Add reference signal
mip.add_signals('ref_yaw')
# add differentiators
mip.add_signals('velocity1','velocity2')
mip.add_filter('velocity1', Differentiator(), ['clock', 'encoder1'],['velocity1'])
mip.add_filter('velocity2', Differentiator(), ['clock', 'encoder2'],['velocity2'])
# Closed-loop controllers
mip.add_filter('Controller1',
Feedback(block = Gain(gain = K)),
['yaw','ref_yaw'],
['pwm_yaw'])
mip.add_filter('Sum1',
Subtract(),
['pwm_yaw','ref_pwm1'],
['pwm1'])
mip.add_filter('Sum2',
Sum(),
['pwm_yaw','ref_pwm2'],
['pwm2'])
# add logger
mip.add_sink('logger', Logger(auto_reset = True),
['clock',
'encoder1', 'encoder2',
'velocity1','velocity2',
'pwm1','pwm2',
'yaw'])
# add a timer to stop the controller
mip.add_timer('stop',
Constant(value = 0),
None, ['is_running'],
period = T, repeat = False)
print(mip.info('all'))
ref_pwm1 = 30
ref_pwm2 = 30
ref_yaw = 0.5
mip.set_signal('ref_pwm1', ref_pwm1)
mip.set_signal('ref_pwm2', ref_pwm2)
mip.set_signal('ref_yaw', ref_yaw)
with mip:
mip.join()
log = mip.get_sink('logger', 'log')
Calculate the average heading angle and its standard deviation:
clock = log['clock']
mean_yaw = log['yaw'][clock > clock[0] + 1].mean()
std_yaw = log['yaw'][clock > clock[0] + 1].std()
print('yaw: mean = {:5.3f}, std = {:5.3f}%'.format(mean_yaw,100*std_yaw/mean_yaw))
Plot the heading angle:
plt.figure()
plt.plot(log['clock'], log['yaw'], 'b',
[clock[0],clock[-1]], [ref_yaw,ref_yaw], 'b--',
[clock[0],clock[-1]], [mean_yaw,mean_yaw], 'b')
plt.xlabel('time (s)')
plt.ylabel('yaw (rad)')
plt.title('yaw x time')
plt.grid()
Plot pwm versus time:
plt.figure()
plt.plot(clock, log['pwm1'],
clock, log['pwm2'])
plt.ylabel('pwm (%)')
plt.xlabel('time (s)')
plt.title('pwm x time')
plt.grid()
Plot velocities versus time:
plt.figure()
plt.plot(log['clock'], log['velocity1'], 'b',
log['clock'], log['velocity2'], 'r')
plt.xlabel('time (s)')
plt.ylabel('velocity (Hz)')
plt.title('velocity x time')
plt.grid()
# Interpolated input signals
T = 5
ts = [0, T]
yaws = 2*np.pi*np.array([0, 2])
plt.figure()
plt.plot(ts, yaws)
plt.ylabel('position (cycles)')
plt.xlabel('time (s)')
plt.grid()
# Interpolated input signals
T = 10
ts = [0, T/4, T/2, 3*T/4, T]
yaws = 2*np.pi*np.array([0, 1, 0, 1, 0])
plt.figure()
plt.plot(ts, yaws)
plt.ylabel('position (cycles)')
plt.xlabel('time (s)')
plt.grid()
# Interpolated input signals
T = 10
ts = [0, T/4, T/4, 2*T/4, 2*T/4, 3*T/4, 3*T/4, T, T]
yaws = 2*np.pi*np.array([0, 0, 1/4, 1/4, 1/2, 1/2, 3/4, 3/4, 1])
plt.figure()
plt.plot(ts, yaws)
plt.ylabel('position (cycles)')
plt.xlabel('time (s)')
plt.grid()
Run the controller:
K=10
# Add reference signal
mip.add_signals('ref_yaw')
# add filters to interpolate data
mip.add_filter('ref_yaw',
Interp(fp = ts, xp = yaws, period = T),
['clock'],
['ref_yaw'])
# add differentiators
mip.add_signals('velocity1','velocity2')
mip.add_filter('velocity1', Differentiator(), ['clock', 'encoder1'],['velocity1'])
mip.add_filter('velocity2', Differentiator(), ['clock', 'encoder2'],['velocity2'])
# Closed-loop controllers
mip.add_filter('wrap', Wrap(), ['yaw'], ['continuous_yaw'])
mip.add_filter('Controller1',
Feedback(block = Gain(gain = K)),
['continuous_yaw','ref_yaw'],
['pwm_yaw'])
mip.add_filter('Sum1',
Subtract(),
['pwm_yaw','ref_pwm1'],
['pwm1'])
mip.add_filter('Sum2',
Sum(),
['pwm_yaw','ref_pwm2'],
['pwm2'])
# add logger
mip.add_sink('logger', Logger(auto_reset = True),
['clock',
'encoder1', 'encoder2',
'velocity1','velocity2',
'pwm1','pwm2',
'continuous_yaw', 'ref_yaw'])
# add a timer to stop the controller
mip.add_timer('stop',
Constant(value = 0),
None, ['is_running'],
period = T, repeat = False)
print(mip.info('all'))
ref_pwm1 = 30
ref_pwm2 = 30
mip.set_signal('ref_pwm1', ref_pwm1)
mip.set_signal('ref_pwm2', ref_pwm2)
mip.set_source('clock', reset=True)
mip.set_filter('wrap', reset=True)
mip.set_filter('ref_yaw', reset=True)
with mip:
mip.join()
log = mip.get_sink('logger', 'log')
Plot the heading angle:
plt.figure()
plt.plot(log['clock'], log['continuous_yaw'], 'b',
log['clock'], log['ref_yaw'], 'b--')
plt.xlabel('time (s)')
plt.ylabel('yaw (rad)')
plt.title('yaw x time')
plt.grid()
Plot pwm versus time:
plt.figure()
plt.plot(log['clock'], log['pwm1'],
log['clock'], log['pwm2'])
plt.ylabel('pwm (%)')
plt.xlabel('time (s)')
plt.title('pwm x time')
plt.grid()
Plot velocities versus time:
plt.figure()
plt.plot(log['clock'], log['velocity1'], 'b',
log['clock'], log['velocity2'], 'r')
plt.xlabel('time (s)')
plt.ylabel('velocity (Hz)')
plt.title('velocity x time')
plt.grid()