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.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.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 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()