def main():
# import blocks and controller
from pyctrl.rc.mip 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
from rcpy.gpio import GRN_LED, PAUSE_BTN
from rcpy.led import red
# export json?
export_json = True
# create mip
mip = Controller()
# 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')
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 green led
mip.add_sink('timer/supervisor/green_led', ('pyctrl.rc.led', 'LED'),
['small_angle'],
kwargs={'pin': GRN_LED},
enable=True)
# add pause button on a timer
mip.add_source('timer/supervisor/pause_button',
('pyctrl.rc.button', 'Button'), ['is_running'],
kwargs={
'pin': PAUSE_BTN,
'invert': True
},
enable=True)
# print controller
print(mip.info('all'))
# export json?
if export_json:
from pyctrl.flask import JSONEncoder
# export controller as json
json = JSONEncoder(sort_keys=True, indent=4).encode(mip)
with open('rc_mip_balance.json', 'w') as f:
f.write(json)
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
print("""
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* M I P B A L A N C E *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
""")
print("""
Hold your MIP upright to start balancing
Use your keyboard to control the mip:
* UP and DOWN arrows move forward and back
* LEFT and RIGHT arrows steer
* / stops forward motion
* . stops steering
* SPACE resets forward motion and steering
""")
# reset everything
mip.set_source('clock', reset=True)
mip.set_source('encoder1', reset=True)
mip.set_source('encoder2', reset=True)
mip.set_filter('controller', reset=True)
mip.set_source('inclinometer', reset=True)
# turn on red led
red.on()
# start the controller
mip.start()
print("Press Ctrl-C or press the <PAUSE> button to exit")
# fire thread to update velocities
thread = threading.Thread(target=get_arrows, args=(mip, fd))
thread.daemon = False
thread.start()
# and wait until controller dies
mip.join()
# print message
print("\nDone with balancing")
except KeyboardInterrupt:
print("\nBalancing aborted")
finally:
# turn off red led
red.off()
# make sure it exits
mip.set_state(pyctrl.EXITING)
print("Press any key to exit")
thread.join()
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
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))
def main():
# import blocks and controller
from pyctrl.rc.mip 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
from rcpy.gpio import GRN_LED, PAUSE_BTN
from rcpy.led import red
# export json?
export_json = True
# create mip
mip = Controller()
# 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('pwm', 'pwm_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', 'pwm', 'steer_reference'],
['pwm_fade', 'steer_reference_fade'])
# steering biasing
mip.add_filter('steer', ControlledCombination(),
['steer_reference_fade', 'pwm_fade', 'pwm_fade'],
['pwm1', 'pwm2'])
# set references
mip.set_signal('steer_reference', 0.5)
# print controller
print(mip.info('all'))
# export json?
if export_json:
from pyctrl.flask import JSONEncoder
# export controller as json
json = JSONEncoder(sort_keys=True, indent=4).encode(mip)
with open('rc_mip_drive.json', 'w') as f:
f.write(json)
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
print("""
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* M I P D R I V E *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
""")
print("""
Use your keyboard to drive the mip as a car:
* UP and DOWN arrows move forward and back
* LEFT and RIGHT arrows steer
* / stops forward motion
* . stops steering
* SPACE resets forward motion and steering
""")
# reset everything
mip.set_source('clock', reset=True)
mip.set_source('encoder1', reset=True)
mip.set_source('encoder2', reset=True)
mip.set_source('inclinometer', reset=True)
# turn on red led
red.on()
# start the controller
mip.start()
print("Press Ctrl-C or press the <PAUSE> button to exit")
# fire thread to update velocities
thread = threading.Thread(target=get_arrows, args=(mip, fd))
thread.daemon = False
thread.start()
# and wait until controller dies
mip.join()
# print message
print("\nDone with driving")
except KeyboardInterrupt:
print("\nDriving aborted")
finally:
# turn off red led
red.off()
# make sure it exits
mip.set_state(pyctrl.EXITING)
print("Press any key to exit")
thread.join()
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
def main():
# import python's standard math module and numpy
import math, numpy, sys
# import Controller and other blocks from modules
from pyctrl.rc import Controller
from pyctrl.block import Interp, Logger, Constant
from pyctrl.block.system import System, Differentiator
from pyctrl.system.tf import DTTF, LPF
# initialize controller
Ts = 0.01
bbb = Controller(period=Ts)
# add encoder as source
bbb.add_source('encoder1', ('pyctrl.rc.encoder', 'Encoder'), ['encoder'],
kwargs={
'encoder': 3,
'ratio': 60 * 35.557
})
# add motor as sink
bbb.add_sink('motor1', ('pyctrl.rc.motor', 'Motor'), ['pwm'],
kwargs={'motor': 3},
enable=True)
# build interpolated input signal
ts = [0, 1, 2, 3, 4, 5, 5, 6]
us = [0, 0, 100, 100, -50, -50, 0, 0]
# add filter to interpolate data
bbb.add_filter('input', Interp(xp=us, fp=ts), ['clock'], ['pwm'])
# add motor speed signal
bbb.add_signal('speed')
# add motor speed filter
bbb.add_filter('speed', Differentiator(), ['clock', 'encoder'], ['speed'])
# add low-pass signal
bbb.add_signal('fspeed')
# add low-pass filter
bbb.add_filter('LPF', System(model=LPF(fc=5, period=Ts)), ['speed'],
['fspeed'])
# add logger
bbb.add_sink('logger', Logger(),
['clock', 'pwm', 'encoder', 'speed', 'fspeed'])
# Add a timer to stop the controller
bbb.add_timer('stop',
Constant(value=0),
None, ['is_running'],
period=6,
repeat=False)
# print controller info
print(bbb.info('all'))
try:
# run the controller
print('> Run the controller.')
# reset clock
bbb.set_source('clock', reset=True)
with bbb:
# wait for the controller to finish on its own
bbb.join()
print('> Done with the controller.')
except KeyboardInterrupt:
pass
# read logger
data = bbb.get_sink('logger', 'log')
try:
# import matplotlib
import matplotlib.pyplot as plt
except:
print('! Could not load matplotlib, skipping plots')
sys.exit(0)
print('> Will plot')
try:
# start plot
plt.figure()
except:
print('! Could not plot graphics')
print('> Make sure you have a connection to a windows manager')
sys.exit(0)
# plot pwm
plt.subplot(2, 1, 1)
plt.plot(data['clock'], data['pwm'], 'b')
plt.ylabel('pwm (%)')
plt.ylim((-120, 120))
plt.xlim(0, 6)
plt.grid()
# plot encoder
plt.subplot(2, 1, 2)
plt.plot(data['clock'], data['encoder'], 'b')
plt.ylabel('encoder (cycles)')
plt.ylim((0, 25))
plt.xlim(0, 6)
plt.grid()
# start plot
plt.figure()
# plot pwm
ax1 = plt.gca()
ax1.plot(data['clock'], data['pwm'], 'g', label='pwm')
ax1.set_ylabel('pwm (%)')
ax1.set_ylim((-60, 120))
ax1.grid()
plt.legend(loc=2)
# plot velocity
ax2 = plt.twinx()
ax2.plot(data['clock'], data['speed'], 'b', label='speed')
ax2.plot(data['clock'], data['fspeed'], 'r', label='fspeed')
ax2.set_ylabel('speed (Hz)')
ax2.set_ylim((-6, 12))
ax2.set_xlim(0, 6)
ax2.grid()
plt.legend(loc=1)
# show plots
plt.show()
def main():
# import blocks and controller
from pyctrl.rc.mip import Controller
from pyctrl.block.system import System, Subtract, Differentiator, Sum, Gain
from pyctrl.block.nl import ControlledCombination
from pyctrl.block import Logger, ShortCircuit
from pyctrl.block.logic import CompareAbs
from pyctrl.system.ss import DTSS
# create mip
mip = Controller()
# 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 reference
mip.add_signal('phi_dot_reference')
# 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'], ['pwm'])
# steering biasing
mip.add_signal('steer_reference')
mip.add_filter('steer', ControlledCombination(),
['steer_reference', 'pwm', 'pwm'], ['pwm1', 'pwm2'])
# set references
mip.set_signal('phi_dot_reference', 0)
mip.set_signal('steer_reference', 0.5)
# add kill switch
mip.add_filter('kill', CompareAbs(threshold=0.2), ['theta'],
['is_running'])
# print controller
print(mip.info('all'))
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
print("""
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* M I P B A L A N C E *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
""")
input('Hold your MIP upright and hit <ENTER> to start balancing')
print("""
Use your keyboard to control the mip:
* UP and DOWN arrows move forward and back
* LEFT and RIGHT arrows steer
* / stops forward motion
* . stops steering
* SPACE resets forward motion and steering
""")
# reset everything
mip.set_source('clock', reset=True)
mip.set_source('encoder1', reset=True)
mip.set_source('encoder2', reset=True)
mip.set_filter('controller', reset=True)
mip.set_source('inclinometer', reset=True)
# start the controller
mip.start()
print("Press Ctrl-C to exit")
# fire thread to update velocities
thread = threading.Thread(target=get_arrows, args=(mip, fd))
thread.daemon = True
thread.start()
# and wait until controller dies
mip.join()
except KeyboardInterrupt:
print("> Balancing aborted")
mip.set_state(pyctrl.EXITING)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
def main():
# import python's standard math module and numpy
import math, numpy, sys
# import Controller and other blocks from modules
from pyctrl.timer import Controller
from pyctrl.block import Interp, Logger, Constant
from pyctrl.block.system import System, Differentiator
from pyctrl.system.tf import DTTF, LPF
# initialize controller
Ts = 0.01
simotor = Controller(period=Ts)
# build interpolated input signal
ts = [0, 1, 2, 3, 4, 5, 5, 6]
us = [0, 0, 100, 100, -50, -50, 0, 0]
# add pwm signal
simotor.add_signal('pwm')
# add filter to interpolate data
simotor.add_filter('input', Interp(xp=us, fp=ts), ['clock'], ['pwm'])
# Motor model parameters
tau = 1 / 55 # time constant (s)
g = 0.092 # gain (cycles/sec duty)
c = math.exp(-Ts / tau)
d = (g * Ts) * (1 - c) / 2
# add motor signals
simotor.add_signal('encoder')
# add motor filter
simotor.add_filter(
'motor',
System(model=DTTF(numpy.array((0, d, d)), numpy.array((1, -(1 + c),
c)))), ['pwm'],
['encoder'])
# add motor speed signal
simotor.add_signal('speed')
# add motor speed filter
simotor.add_filter('speed', Differentiator(), ['clock', 'encoder'],
['speed'])
# add low-pass signal
simotor.add_signal('fspeed')
# add low-pass filter
simotor.add_filter('LPF', System(model=LPF(fc=5, period=Ts)), ['speed'],
['fspeed'])
# add logger
simotor.add_sink('logger', Logger(),
['clock', 'pwm', 'encoder', 'speed', 'fspeed'])
# Add a timer to stop the controller
simotor.add_timer('stop',
Constant(value=0),
None, ['is_running'],
period=6,
repeat=False)
# print controller info
print(simotor.info('all'))
try:
# run the controller
print('> Run the controller.')
with simotor:
# wait for the controller to finish on its own
simotor.join()
print('> Done with the controller.')
except KeyboardInterrupt:
pass
finally:
pass
# read logger
data = simotor.get_sink('logger', 'log')
try:
# import matplotlib
import matplotlib.pyplot as plt
except:
print('! Could not load matplotlib, skipping plots')
sys.exit(0)
print('> Will plot')
try:
# start plot
plt.figure()
except:
print('! Could not plot graphics')
print('> Make sure you have a connection to a windows manager')
sys.exit(0)
# plot pwm
ax1 = plt.gca()
ax1.plot(data['clock'], data['pwm'], 'g', label='pwm')
ax1.set_ylabel('pwm (%)')
ax1.set_ylim((-60, 120))
ax1.grid()
plt.legend(loc=2)
# plot velocity
ax2 = plt.twinx()
ax2.plot(data['clock'], data['speed'], 'b', label='speed')
ax2.plot(data['clock'], data['fspeed'], 'r', label='fspeed')
ax2.set_ylabel('speed (Hz)')
ax2.set_ylim((-6, 12))
ax2.set_xlim(0, 6)
ax2.grid()
plt.legend(loc=1)
# show plots
plt.show()