def main():
# import python's standard time module
import time
# import Controller and other blocks from modules
from pyctrl import Controller
from pyctrl.block import Printer
from pyctrl.block.clock import TimerClock
# initialize controller
hello = Controller()
# add the signal myclock
hello.add_signal('myclock')
# add a TimerClock as a source
hello.add_source('myclock', TimerClock(period=1), ['myclock'], enable=True)
# add a Printer as a sink
hello.add_sink('message',
Printer(message='Hello World!'), ['myclock'],
enable=True)
try:
# run the controller
with hello:
# do nothing for 5 seconds
time.sleep(5)
except KeyboardInterrupt:
pass
finally:
print('Done')
def test_local_timer(self):
from pyctrl.timer import Controller
controller = Controller(period=0.01)
self.run_test_basic(controller)
self.run_test_timer(controller)
self.run_test_set(controller)
self.run_test_sub_container(controller)
self.run_test_sub_container_timer(controller)
self.run_test_timer_sub_container(controller)
self.run_test_add_device(controller)
def test_local_timer():
from pyctrl.timer import Controller
controller = Controller()
_test_basic(controller)
_test_timer(controller)
_test_set(controller)
_test_sub_container(controller)
_test_sub_container_timer(controller)
_test_timer_sub_container(controller)
_test_add_device(controller)
def run_test_local(self):
from pyctrl import Controller
controller = Controller()
self.run_test_basic(controller)
self.run_test_timer(controller)
self.run_test_set(controller)
self.run_test_sub_container(controller)
self.run_test_sub_container_timer(controller)
self.run_test_timer_sub_container(controller)
self.run_test_add_device(controller)
def main():
# import python's standard time module
import time
# import Controller and other blocks from modules
from pyctrl import Controller
from pyctrl.block import Printer, Constant
# initialize controller
hello = Controller()
# add a Printer as a timer
hello.add_timer('message',
Printer(message='Hello World @ {:3.1f} s '), ['clock'],
None,
period=1,
repeat=True)
# Add a timer to stop the controller
hello.add_timer('stop',
Constant(value=0),
None, ['is_running'],
period=5,
repeat=False)
# add a Printer as a sink
hello.add_sink('message',
Printer(message='Current time {:5.3f} s', endln='\r'),
['clock'])
# print controller info
print(hello.info('all'))
try:
# run the controller
print('> Run the controller.')
with hello:
# wait for the controller to finish on its own
hello.join()
print('> Done with the controller.')
except KeyboardInterrupt:
pass
def test_clock():
from pyctrl import Controller
from pyctrl.block.clock import Clock, TimerClock
controller = Controller()
period = 0.01
controller.add_signal('clock')
clock = Clock()
controller.add_source('clock', clock, ['clock'])
K = 10
k = 0
while k < K:
(t, ) = controller.read_source('clock')
k += 1
assert clock.get('count') == 10
controller.set_source('clock', reset=True)
(t, ) = controller.read_source('clock')
assert t < 0.01
controller.remove_source('clock')
clock = TimerClock(period=period)
controller.add_source('clock', clock, ['clock'])
K = 10
k = 0
while k < K:
(t, ) = controller.read_source('clock')
k += 1
assert t > 0.9 * K * period
controller.set_source('clock', reset=True)
(t, ) = controller.read_source('clock')
assert t < 0.9 * 2 * period
clock.set_enabled(False)
def test_run():
from pyctrl import Controller
from pyctrl.block.clock import Clock, TimerClock
from pyctrl.block import Map
controller = Controller()
clock = Clock()
controller.add_source('clock', clock, ['clock'])
# start/stop with condition
controller.add_filter('condition', Map(function=lambda x: x < 1),
['clock'], ['is_running'])
controller.set_source('clock', reset=True)
controller.start()
is_running = controller.get_signal('is_running')
while is_running:
is_running = controller.get_signal('is_running')
controller.stop()
tk = controller.get_signal('clock')
assert tk > 1 and tk < 1.01
# run with condition
controller.set_source('clock', reset=True)
controller.run()
controller.stop()
tk = controller.get_signal('clock')
assert tk > 1 and tk < 1.01
controller.remove_source('clock')
def _test_mip_balance():
import numpy as np
from pyctrl import Controller
from pyctrl.block.container import Container, Input, Output
from pyctrl.block.system import System, Subtract, Differentiator, Sum, Gain
from pyctrl.block.nl import ControlledCombination, Product
from pyctrl.block import Fade, Printer
from pyctrl.system.ss import DTSS
from pyctrl.block.logic import CompareAbsWithHysterisis, SetFilter, State
GRN_LED = 61
PAUSE_BTN = 62
# create mip
mip = Controller()
# add signals
mip.add_signals('theta', 'theta_dot', 'encoder1', 'encoder2', 'pwm1',
'pwm2')
# phi is the average of the encoders
mip.add_signal('phi')
mip.add_filter('phi', Sum(gain=0.5), ['encoder1', 'encoder2'], ['phi'])
# phi dot
mip.add_signal('phi_dot')
mip.add_filter('phi_dot', Differentiator(), ['clock', 'phi'], ['phi_dot'])
# phi dot and steer reference
mip.add_signals('phi_dot_reference', 'phi_dot_reference_fade')
mip.add_signals('steer_reference', 'steer_reference_fade')
# add fade in filter
mip.add_filter('fade', Fade(target=[0, 0.5], period=5),
['clock', 'phi_dot_reference', 'steer_reference'],
['phi_dot_reference_fade', 'steer_reference_fade'])
# state-space matrices
A = np.array([[0.913134, 0.0363383], [-0.0692862, 0.994003]])
B = np.array([[0.00284353, -0.000539063], [0.00162443, -0.00128745]])
C = np.array([[-383.009, 303.07]])
D = np.array([[-1.22015, 0]])
B = 2 * np.pi * (100 / 7.4) * np.hstack((-B, B[:, 1:]))
D = 2 * np.pi * (100 / 7.4) * np.hstack((-D, D[:, 1:]))
ssctrl = DTSS(A, B, C, D)
# state-space controller
mip.add_signals('pwm')
mip.add_filter('controller', System(model=ssctrl),
['theta_dot', 'phi_dot', 'phi_dot_reference_fade'], ['pwm'])
# enable pwm only if about small_angle
mip.add_signals('small_angle', 'small_angle_pwm')
mip.add_filter('small_angle_pwm', Product(), ['small_angle', 'pwm'],
['small_angle_pwm'])
# steering biasing
mip.add_filter(
'steer', ControlledCombination(),
['steer_reference_fade', 'small_angle_pwm', 'small_angle_pwm'],
['pwm1', 'pwm2'])
# set references
mip.set_signal('phi_dot_reference', 0)
mip.set_signal('steer_reference', 0.5)
# add supervisor actions on a timer
# actions are inside a container so that they are executed all at once
mip.add_timer('supervisor',
Container(), ['theta'], ['small_angle', 'is_running'],
period=0.5,
repeat=True)
mip.add_signals('timer/supervisor/theta', 'timer/supervisor/small_angle',
'timer/supervisor/is_running')
mip.add_source('timer/supervisor/theta', Input(), ['theta'])
mip.add_sink('timer/supervisor/small_angle', Output(), ['small_angle'])
mip.add_sink('timer/supervisor/is_running', Output(), ['is_running'])
# add small angle sensor
mip.add_filter(
'timer/supervisor/is_angle_small',
CompareAbsWithHysterisis(threshold=0.11,
hysterisis=0.09,
offset=-0.07,
state=(State.LOW, )), ['theta'],
['small_angle'])
# reset controller and fade
mip.add_sink(
'timer/supervisor/reset_controller',
SetFilter(label=['/controller', '/fade'], on_rise={'reset': True}),
['small_angle'])
# add pause button on a timer
mip.add_source('timer/supervisor/pause_button',
('pyctrl.block', 'Constant'), ['is_running'],
kwargs={'value': 0},
enable=True)
from pyctrl.flask import JSONEncoder, JSONDecoder
json1 = JSONEncoder(sort_keys=True, indent=4).encode(mip)
obj = JSONDecoder().decode(json1)
json2 = JSONEncoder(sort_keys=True, indent=4).encode(obj)
assert json1 == json2
print('json = \n{}'.format(json1))
""" import Python's standard time module """
import time
# import Controller and other blocks from modules
from pyctrl import Controller
from pyctrl.block import Printer
from fg_source import FgSource
from fg_sink import FgSink
pilot = Controller()
pilot.add_signal('speed')
pilot.add_signal('heading')
pilot.add_signal('altitude')
pilot.add_signal('climb')
pilot.add_signal('pitch')
pilot.add_signal('bank')
pilot.add_signal('engine')
pilot.add_signal('latitude')
pilot.add_signal('longitude')
pilot.add_signal('northspeed')
pilot.add_signal('eastspeed')
pilot.add_signal('aileron')
pilot.add_signal('elevator')
pilot.add_signal('rudder')
pilot.add_signal('throttle')
pilot.add_source('fg_source',
FgSource(),
['speed', 'heading', 'altitude', 'climb', 'pitch', 'bank',
def testSetBlock(self):
from pyctrl import Controller
from pyctrl.block import Constant
controller = Controller()
controller.add_source('block',
Constant(),
['s1'])
self.assertTrue(controller.get_source('block', 'enabled'))
blk = logic.SetSource(parent=controller,
label='block',
on_rise_and_fall={'enabled': False})
self.assertTrue(blk.state is logic.State.LOW)
blk.write(1)
self.assertTrue(not controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.HIGH)
controller.set_source('block', enabled=True)
self.assertTrue(controller.get_source('block', 'enabled'))
blk.write(0.5)
self.assertTrue(controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.HIGH)
blk.write(0.1)
self.assertTrue(not controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.LOW)
controller.set_source('block', enabled=True)
self.assertTrue(controller.get_source('block', 'enabled'))
blk.write(0.5)
self.assertTrue(controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.LOW)
blk.write(0.9)
self.assertTrue(not controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.HIGH)
# OnRiseSet
blk = logic.SetSource(parent=controller,
label='block',
on_rise={'enabled': False})
self.assertTrue(blk.state is logic.State.LOW)
blk.write(1)
self.assertTrue(not controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.HIGH)
controller.set_source('block', enabled=True)
self.assertTrue(controller.get_source('block', 'enabled'))
blk.write(0.5)
self.assertTrue(controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.HIGH)
blk.write(0.1)
self.assertTrue(controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.LOW)
controller.set_source('block', enabled=True)
self.assertTrue(controller.get_source('block', 'enabled'))
blk.write(0.5)
self.assertTrue(controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.LOW)
blk.write(0.9)
self.assertTrue(not controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.HIGH)
# OnFallSet
blk = logic.SetSource(parent=controller,
label='block',
on_fall={'enabled': False})
controller.set_source('block', enabled=True)
self.assertTrue(blk.state is logic.State.LOW)
blk.write(1)
self.assertTrue(controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.HIGH)
self.assertTrue(controller.get_source('block', 'enabled'))
blk.write(0.5)
self.assertTrue(controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.HIGH)
blk.write(0.1)
self.assertTrue(not controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.LOW)
controller.set_source('block', enabled=True)
self.assertTrue(controller.get_source('block', 'enabled'))
blk.write(0.5)
self.assertTrue(controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.LOW)
blk.write(0.9)
self.assertTrue(controller.get_source('block', 'enabled'))
self.assertTrue(blk.state is logic.State.HIGH)
# try pickling
import pickle
pickle.dumps(blk)
def test1():
if not test_ode:
return
from pyctrl import Controller
controller = Controller()
Ts = 0.01
clock = TimerClock(period=Ts)
controller.add_source('clock', clock, ['clock'])
a = -1
b = 1
def f(t, x, u, a, b):
return a * x + b * u
t0 = 0
uk = 1
x0 = np.array([0])
sys = ODE((1, 1, 1), f, x0=x0, t0=t0, pars=(a, b))
controller.add_signals('input', 'output')
controller.add_filter('condition', Map(function=lambda x: x < 1),
['clock'], ['is_running'])
controller.add_filter('ode', TimeVaryingSystem(model=sys),
['clock', 'input'], ['output'])
controller.add_sink('logger', Logger(), ['clock', 'output'])
print(controller.info('all'))
controller.set_filter('ode', reset=True)
controller.set_source('clock', reset=True)
controller.set_sink('logger', reset=True)
controller.set_signal('input', uk)
controller.run()
xk = sys.state
log = controller.get_sink('logger', 'log')
t0 = log['clock'][0, 0]
tk = log['clock'][-1, 0]
yk = log['output'][-1, 0]
print('t0 = {}'.format(t0))
print('tk = {}'.format(tk))
print('yk = {}'.format(yk))
yyk = uk * (1 - math.exp(a * (tk - t0))) + x0[0] * math.exp(a * (tk - t0))
print(log)
print(t0, x0, tk, xk, yk, yyk)
assert np.abs(yk - yyk) < 1e-2
uk = 0
x0 = sys.state
controller.add_filter('condition', Map(function=lambda x: x < 2),
['clock'], ['is_running'])
print(controller.info('all'))
print('clock = {}'.format(controller.get_signal('clock')))
#controller.set_source('clock', reset = True)
controller.set_sink('logger', reset=True)
controller.set_signal('input', uk)
controller.run()
xk = sys.state
print('clock = {}'.format(controller.get_signal('clock')))
log = controller.get_sink('logger', 'log')
print('log = {}'.format(log))
t0 = log['clock'][0, 0]
tk = log['clock'][-1, 0]
yk = log['output'][-1, 0]
print('t0 = {}, x0 = {}, tk = {}, xk = {}, yk = {}'.format(
t0, x0, tk, xk, yk))
yyk = uk * (1 - math.exp(a * (tk - t0))) + x0 * math.exp(a * (tk - t0))
print(log)
print(t0, x0, tk, xk, yk, yyk)
assert np.abs(yk - np.array([yyk])) < 1e-2
uk = -1
x0 = sys.state
controller.add_filter('condition', Map(function=lambda x: x < 3),
['clock'], ['is_running'])
#controller.set_source('clock', reset = True)
controller.set_sink('logger', reset=True)
controller.set_signal('input', uk)
controller.run()
xk = sys.state
log = controller.get_sink('logger', 'log')
t0 = log['clock'][0, 0]
tk = log['clock'][-1, 0]
yk = log['output'][-1, 0]
yyk = uk * (1 - math.exp(a * (tk - t0))) + x0 * math.exp(a * (tk - t0))
print(t0, x0, tk, xk, yk, yyk)
assert np.abs(yk - np.array([yyk])) < 1e-2
clock.set_enabled(False)
def test2():
if not test_ode:
return
m1 = 30 / 1000
l1 = 7.6 / 100
r1 = (5 - (10 - 7.6) / 2) / 100
w1 = 10 / 100
d1 = 2.4 / 100
J1 = m1 * (w1**2 + d1**2) / 12
m2 = 44 / 1000
w2 = 25.4 / 100
d2 = 2.4 / 100
J2 = m2 * (w2**2 + d2**2) / 12
r2 = (25.4 / 2 - 1.25) / 100
Jm = 0.004106
km = 0.006039
bm = 0.091503
g = 9.8
bPhi = 0
bTheta = 0
def MK(x, u):
theta, phi, thetaDot, phiDot = x
return (
np.array([[J2 + m2 * r2**2, m2 * r2 * l1 * math.cos(theta - phi)],
[
m2 * r2 * l1 * math.cos(theta - phi),
J1 + Jm + m1 * r1**2 + m2 * l1**2
]]),
np.array([
bTheta * thetaDot + m2 * r2 *
(g * math.sin(theta) + l1 * math.sin(theta - phi) * phiDot**2),
g * (m1 * r1 + m2 * l1) * math.sin(phi) -
m2 * r2 * l1 * math.sin(theta - phi) * thetaDot**2 +
(bm + bPhi) * phiDot - km * u[0]
]))
def ff(t, x, u):
M, K = MK(x, u)
return np.hstack((x[2:4], -la.solve(M, K)))
theta0, phi0 = 0 + math.pi / 6, 0
t0, x0, u0 = 0, np.array([theta0, phi0, 0, 0]), [0]
M, K = MK(x0, u0)
print(M)
print(K)
print(ff(t0, x0, u0))
sys = ODE(shape=(1, 4, 4), t0=t0, x0=x0, f=ff)
tk = 5
uk = [0]
yk = sys.update(tk, uk)
print('1. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
from pyctrl import Controller
controller = Controller()
Ts = 0.01
controller.add_source('clock', Clock(), ['clock'])
condition = Map(function=lambda t: t < T)
controller.add_filter('condition', condition, ['clock'], ['is_running'])
controller.add_signals('tau', 'x')
controller.add_filter(
'ode',
TimeVaryingSystem(model=ODE(shape=(1, 4, 4), t0=t0, x0=x0, f=ff)),
['clock', 'tau'], ['x'])
controller.add_sink('logger', Logger(), ['clock', 'x'])
controller.set_source('clock', reset=True)
T = 5 + Ts
controller.run()
log = controller.get_sink('logger', 'log')
t0 = log['clock'][0, 0]
tk = log['clock'][0, -1]
yk = log['x'][0, -1]
# yk = log[-1,1:]
print('2. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
import control
fc = 7
wc = 2 * math.pi * fc
lpf = control.tf(wc, [1, wc])
ctr = -2 * 100
def gg(t, x, u):
return [x[0]]
Ts = 0.01
Ac, Bc, Cc, Dc = map(np.array, control.ssdata(control.ss(lpf * ctr)))
nc = Ac.shape[0]
def F(t, x, ref):
x, xc = x[0:4], x[4:4 + nc]
y = ref - gg(t, x, [0])
u = max(-100, min(100, Cc.dot(xc) + Dc.dot(y)))
#print(ff(t,x,u))
return np.hstack((ff(t, x, u), Ac.dot(xc) + Bc.dot(y)))
eta = 0
kappa = 0
ref = np.array([eta * math.pi])
theta0 = -20 * math.pi / 180
xx0 = [kappa * math.pi - theta0, eta * math.pi, 0, 0]
xc0 = np.zeros((nc, ))
x0 = np.hstack((xx0, xc0))
t0 = 0
print('F = {}'.format(F(t0, x0, ref)))
sys = ODE(shape=(1, 4, 4), t0=t0, x0=x0, f=F)
tk = 1
uk = np.array([0])
yk = sys.update(tk, uk)
print('1. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))
controller.reset()
Ts = 0.01
controller.add_source('clock', Clock(), ['clock'])
condition = Map(function=lambda t: t < T)
controller.add_filter('condition', condition, ['clock'], ['is_running'])
controller.add_signals('ref', 'x')
controller.add_filter(
'ode',
TimeVaryingSystem(model=ODE(shape=(1, 4, 4), t0=t0, x0=x0, f=F)),
['clock', 'ref'], ['x'])
controller.add_sink('logger', Logger(), ['clock', 'x'])
#print(controller.info('all'))
controller.set_source('clock', reset=True)
controller.set_signal('ref', ref)
T = 1 + Ts
controller.run()
log = controller.get_sink('logger', 'log')
t0 = log['clock'][0, 0]
tk = log['clock'][0, -1]
yk = log['x'][0, -1]
#yk = log[-1,1:]
print('2. [{:3.2f}, {:3.2f}] = {}'.format(t0, tk, yk))