#!/usr/bin/env python
# coding: utf-8
# # Steering Control
#
# ## Preliminaries
#
# The next lines setup some things and import the various libraries required to run this notebook.
#
# Don't forget to initialize the server with:
#
# sudo pyctrl_start_server -m pyctrl.rc.drive
#
# ## Preliminaries
#
# The next lines setup some things and import the various libraries required to run this notebook.
# In[ ]:
get_ipython().run_line_magic('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'))
# ## Closed-loop Velocity Control
#
# 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.
# ## Closed-loop Position Control
#
# 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:
#
#
# ## Steering control
#
# 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:
#
# $$
# \begin{aligned}
# \omega &= \frac{r}{d} (\omega_r - \omega_l), \\
# v &= \frac{r}{2} (\omega_r + \omega_l)
# \end{aligned}
# $$
#
# 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`.
# In[ ]:
mip.reset()
# In[ ]:
T = 5
K = 10
# In[ ]:
# 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'))
# In[ ]:
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:
# In[ ]:
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:
# In[ ]:
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*:
# In[ ]:
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*:
# In[ ]:
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()
# #### Reference signal #1
# In[ ]:
# Interpolated input signals
T = 5
ts = [0, T]
yaws = 2*np.pi*np.array([0, 2])
# In[ ]:
plt.figure()
plt.plot(ts, yaws)
plt.ylabel('position (cycles)')
plt.xlabel('time (s)')
plt.grid()
# #### Reference signal #2
# In[ ]:
# 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])
# In[ ]:
plt.figure()
plt.plot(ts, yaws)
plt.ylabel('position (cycles)')
plt.xlabel('time (s)')
plt.grid()
# #### Reference signal #3
# In[ ]:
# 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])
# In[ ]:
plt.figure()
plt.plot(ts, yaws)
plt.ylabel('position (cycles)')
plt.xlabel('time (s)')
plt.grid()
# Run the controller:
# In[ ]:
K=10
# In[ ]:
# 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'))
# In[ ]:
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:
# In[ ]:
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*:
# In[ ]:
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*:
# In[ ]:
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()
# In[ ]: