def test_set():
blk = block.Printer()
assert blk.get() == {
'enabled': True,
'endln': '\n',
'frmt': '{: 12.4f}',
'sep': ' ',
'file': None,
'message': None
}
assert blk.get('enabled', 'frmt') == {'frmt': '{: 12.4f}', 'enabled': True}
assert blk.get('enabled') == True
with pytest.raises(KeyError):
blk.get('*enabled')
blk.set(enabled=False)
assert blk.get('enabled') == False
blk.set(enabled=True)
assert blk.get('enabled') == True
with pytest.raises(block.BlockException):
blk.set(_enabled=True)
blk.set(sep='-')
assert blk.get('sep') == '-'
blk = block.BufferBlock()
assert blk.get() == {'enabled': True, 'demux': False, 'mux': False}
# test twice to make sure it is copying
assert blk.get() == {'enabled': True, 'demux': False, 'mux': False}
with pytest.raises(KeyError):
blk.get('buffer')
blk.set(demux=True)
assert blk.get('demux') == True
with pytest.raises(block.BlockException):
blk.set(buffer=(1, ))
with pytest.raises(block.BlockException):
blk.set(controller=None)
with pytest.raises(KeyError):
blk.get('buffer')
with pytest.raises(KeyError):
blk.get('controller')
def test_Printer():
obj = block.Printer()
obj.write(1.5)
obj.write([1.5, 1.3])
obj.write((1.5, 1.3))
obj.write(*[1.5, 1.3])
obj.write(*(1.5, 1.3))
assert obj.get() == {
'enabled': True,
'endln': '\n',
'frmt': '{: 12.4f}',
'sep': ' ',
'file': None,
'message': None
}
assert obj.get('enabled') == True
print("-> '{}'".format(obj.get('endln', 'frmt')))
assert obj.get('endln', 'frmt') == {'endln': '\n', 'frmt': '{: 12.4f}'}
with pytest.raises(block.BlockException):
obj = block.Printer(adsadsda=1)
with pytest.raises(TypeError):
obj = block.Printer(1, "adsadsda")
with pytest.raises(block.BlockException):
obj = block.Printer(par=1)
with pytest.raises(block.BlockException):
obj = block.Printer(par="adsadsda")
with pytest.raises(block.BlockException):
obj = block.Printer(par="adsadsda", sadasd=1)
obj = block.Printer(enabled=False)
def test_theta(args):
with mip:
print('< Lean the MIP:')
# Test IMU
mip.add_sink(
'printer',
block.Printer(message='Theta = {:+4.2f} cycles', endln='\r'),
['theta'])
print("""
|
| ---
___O O O
========== ========= ==========
+90 deg 0 deg -90 deg
""")
answer = input(
'< | near +90 deg (0.25 cycles) and hit <ENTER>\r'
).lower()
(theta, thetaDot) = mip.read_source('inclinometer')
if theta < 0:
return False, 'Motors are likely reversed'
if theta < .2:
return False, 'MIP does not seem to be leaning at +90 degree position'
answer = input(
'< | near -90 deg (-0.25 cycles) and hit <ENTER>\r'
).lower()
(theta, thetaDot) = mip.read_source('inclinometer')
if theta > 0:
return False, 'Motors are likely reversed'
if theta > -.2:
return False, 'MIP does not seem to be leaning at -90 degree position'
return True, []
def run_test_basic(self, controller):
import numpy
import pyctrl.block as block
import pyctrl.block.system as system
print('\n> * * * TEST BASIC {} * * *'.format(controller.__class__))
# reset controller
controller.reset()
# initial signals
_signals = controller.list_signals()
_sinks = controller.list_sinks()
_sources = controller.list_sources()
_filters = controller.list_filters()
# period
# self.assertTrue( controller.get_period() == 0.01 # default
# controller.set_period(0.1)
# self.assertTrue( controller.get_period() == 0.1
# test signals
# self.assertTrue( 'clock' in controller.list_signals() # clock is default
controller.add_signal('_test_')
self.assertTrue('_test_' in controller.list_signals())
# with self.assertRaises(pyctrl.ControllerException):
controller.add_signal('_test_')
self.assertTrue(controller.get_signal('_test_') == 0)
controller.set_signal('_test_', 1.2)
self.assertTrue(controller.get_signal('_test_') == 1.2)
controller.remove_signal('_test_')
self.assertTrue('_test_' not in controller.list_signals())
with self.assertRaises(Exception):
controller.set_signal('_test_', 1.2)
controller.add_signals('_test1_', '_test2_')
self.assertTrue('_test1_' in controller.list_signals())
self.assertTrue('_test2_' in controller.list_signals())
controller.remove_signal('_test1_')
controller.remove_signal('_test2_')
self.assertTrue('_test1_' not in controller.list_signals())
self.assertTrue('_test2_' not in controller.list_signals())
# test info
self.assertTrue(isinstance(controller.info(), str))
self.assertTrue(isinstance(controller.info('summary'), str))
self.assertTrue(isinstance(controller.info('sources', 'sinks'), str))
# test sink
controller.add_signal('clock')
logger = block.Logger()
controller.add_sink('_logger_', logger, ['_test_'])
self.assertTrue('_logger_' in controller.list_sinks())
self.assertTrue('_test_' in controller.list_signals())
self.assertTrue(
controller.get_sink('_logger_') == {
'current': 0,
'auto_reset': False,
'page': 0,
'enabled': True,
'labels': ['_test_'],
'index': None
})
self.assertTrue(
controller.get_sink('_logger_', 'current', 'auto_reset') == {
'current': 0,
'auto_reset': False
})
self.assertTrue(controller.get_sink('_logger_', 'current') == 0)
controller.set_sink('_logger_', current=1)
self.assertTrue(controller.get_sink('_logger_', 'current') == 1)
# try to remove signal _test_
controller.remove_signal('_test_')
self.assertTrue('_test_' in controller.list_signals())
controller.add_sink('_logger_', block.Logger(), ['clock'])
self.assertTrue('_logger_' in controller.list_sinks())
# TODO: test for changed signals
controller.set_sink('_logger_', reset=True)
log = controller.get_sink('_logger_', 'log')
self.assertTrue(not hasattr(logger, 'log'))
self.assertTrue(isinstance(log['clock'], numpy.ndarray))
self.assertShape(log['clock'].shape, (0, 1))
with self.assertRaises(block.BlockException):
controller.set_sink('_logger_', _reset=True)
with controller:
time.sleep(.2)
log = controller.get_sink('_logger_', 'log')
self.assertTrue(isinstance(log['clock'], numpy.ndarray))
self.assertShape(log['clock'].shape, 1, 1)
self.assertShapeGreater(log['clock'].shape, 1, 0)
controller.set_sink('_logger_', reset=True)
log = controller.get_sink('_logger_', 'log')
self.assertTrue(isinstance(log['clock'], numpy.ndarray))
self.assertShape(log['clock'].shape, (0, 1))
controller.remove_sink('_logger_')
self.assertTrue('_logger_' not in controller.list_sinks())
controller.add_signal('_test_')
controller.add_sink('_logger_', block.Logger(), ['clock', '_test_'])
self.assertTrue('_logger_' in controller.list_sinks())
with controller:
time.sleep(.2)
log = controller.get_sink('_logger_', 'log')
self.assertTrue(isinstance(log['clock'], numpy.ndarray))
self.assertTrue(isinstance(log['_test_'], numpy.ndarray))
self.assertShape(log['clock'].shape, 1, 1)
self.assertShapeGreater(log['clock'].shape, 1, 0)
self.assertShape(log['_test_'].shape, 1, 1)
self.assertShapeGreater(log['_test_'].shape, 1, 0)
controller.set_sink('_logger_', reset=True)
log = controller.get_sink('_logger_', 'log')
self.assertTrue(isinstance(log['clock'], numpy.ndarray))
self.assertTrue(isinstance(log['_test_'], numpy.ndarray))
self.assertShape(log['clock'].shape, (0, 1))
self.assertShape(log['_test_'].shape, (0, 1))
controller.remove_sink('_logger_')
self.assertTrue('_logger_' not in controller.list_sinks())
# test source
# controller.add_source('_rand_', blkrnd.Uniform(), ['clock'])
controller.add_source('_rand_', block.Constant(), ['clock'])
self.assertTrue('_rand_' in controller.list_sources())
# controller.add_source('_rand_', blkrnd.Uniform(), ['_test_'])
controller.add_source('_rand_', block.Constant(value=2), ['_test_'])
self.assertTrue('_rand_' in controller.list_sources())
self.assertTrue(
controller.get_source('_rand_') == {
'demux': False,
'mux': False,
'value': 2,
'enabled': True
})
self.assertTrue(
controller.get_source('_rand_', 'value', 'demux') == {
'value': 2,
'demux': False
})
self.assertTrue(controller.get_source('_rand_', 'value') == 2)
controller.set_source('_rand_', value=1)
self.assertTrue(controller.get_source('_rand_', 'value') == 1)
# TODO: test for changed signals
controller.set_source('_rand_', reset=True)
a, = controller.read_source('_rand_')
self.assertTrue(a == 1)
with self.assertRaises(block.BlockException):
controller.set_source('_rand_', _reset=True)
controller.remove_source('_rand_')
self.assertTrue('_rand_' not in controller.list_sources())
# test filter
controller.add_signal('_output_')
controller.add_source('_rand_', block.Constant(), ['_test_'])
self.assertTrue('_rand_' in controller.list_sources())
controller.add_filter('_gain_', block.ShortCircuit(), ['_test_'],
['_output_'])
self.assertTrue('_gain_' in controller.list_filters())
# TODO: test for changed block
controller.add_filter('_gain_', system.Gain(gain=2), ['_test_'],
['_output_'])
self.assertTrue('_gain_' in controller.list_filters())
self.assertTrue(
controller.get_filter('_gain_') == {
'demux': False,
'enabled': True,
'gain': 2,
'mux': False
})
self.assertTrue(
controller.get_filter('_gain_', 'demux', 'gain') == {
'demux': False,
'gain': 2
})
self.assertTrue(controller.get_filter('_gain_', 'gain') == 2)
with controller:
time.sleep(.2)
controller.add_sink('_printer_', block.Printer(),
['_test_', '_output_'])
self.assertTrue('_printer_' in controller.list_sinks())
controller.add_sink('_logger_', block.Logger(), ['_test_', '_output_'])
self.assertTrue('_logger_' in controller.list_sinks())
with controller:
time.sleep(.2)
log = controller.get_sink('_logger_', 'log')
self.assertTrue(isinstance(log['_test_'], numpy.ndarray))
self.assertTrue(isinstance(log['_output_'], numpy.ndarray))
self.assertShape(log['_test_'].shape, 1, 1)
self.assertShapeGreater(log['_test_'].shape, 1, 0)
self.assertShape(log['_test_'].shape, 1, 1)
self.assertShapeGreater(log['_test_'].shape, 1, 0)
self.assertTrue(numpy.array_equal(log['_output_'], log['_test_'] * 2))
# test reset
signals = controller.list_signals()
sinks = controller.list_sinks()
sources = controller.list_sources()
filters = controller.list_filters()
controller.reset()
signals = controller.list_signals()
sinks = controller.list_sinks()
sources = controller.list_sources()
filters = controller.list_filters()
self.assertTrue(signals == _signals)
self.assertTrue(sources == _sources)
self.assertTrue(filters == _filters)
self.assertTrue(sinks == _sinks)
# test is_running
self.assertTrue(controller.get_signal('is_running') == False)
controller.start()
time.sleep(1)
self.assertTrue(controller.get_signal('is_running') == True)
controller.stop()
time.sleep(1)
self.assertTrue(controller.get_signal('is_running') == False)
def main():
try:
print("""
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* S E T U P M I P *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
Welcome to SETUP MIP. This program will help you make sure that your
MIP is working and wired correctly.
The following tests are meant to be performed with the MIP oriented
as in the following diagram
/
/
O
=========
The angle theta is measured relative to the vertical with zero being
above the wheels. For example
|
| ---
___O O O
========== ========= ==========
+90 deg 0 deg -90 deg
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
""")
input('< Hit <ENTER> to start')
verbose = False
if verbose:
mip.add_sink('printer', block.Printer(endln='\r'), [
'clock', 'pwm1', 'encoder1', 'pmw2', 'encoder2', 'theta',
'theta_dot'
])
print(mip.info('all'))
input(
'< We will spin the wheels. Lift your MIP up in the air and hit <ENTER>'
)
k = 1
position1, position2 \
= test('{}: MOTOR 1 COUNTER-CLOCKWISE'.format(k),
('pwm1','encoder1'),
'Did the wheel nearest to you spin counter-clockwise for two seconds?',
'motor1 not working properly',
test_motor_forward)
k += 1
test('{}: ENCODER 1 COUNTER-CLOCKWISE'.format(k),
(position1, position2), '', '', test_encoder)
input(
'< We will spin the wheels. Lift your MIP up in the air and hit <ENTER>'
)
k += 1
position1, position2 \
= test('{}: MOTOR 2 COUNTER-CLOCKWISE'.format(k),
('pwm2','encoder2'),
'Did the wheel farthest from you spin counter-clockwise for two seconds?',
'motor2 not working properly',
test_motor_forward)
k += 1
test('{}: ENCODER 2 COUNTER-CLOCKWISE'.format(k),
(position1, position2), '', '', test_encoder)
k += 1
test('{}: CLOCK RESET'.format(k), (), '', '', test_reset_clock)
input(
'< We will spin the wheels. Lift your MIP up in the air and hit <ENTER>'
)
k += 1
position1, position2 \
= test('{}: MOTOR 1 CLOCKWISE'.format(k),
('pwm1','encoder1'),
'Did the wheel nearest to you spin clockwise for two seconds?',
'motor1 not working properly',
test_motor_backward)
k += 1
test('{}: ENCODER 1 CLOCKWISE'.format(k), (position2, position1), '',
'', test_encoder)
input(
'< We will spin the wheels. Lift your MIP up in the air and hit <ENTER>'
)
k += 1
position1, position2 \
= test('{}: MOTOR 2 CLOCKWISE'.format(k),
('pwm2','encoder2'),
'Did the wheel farthest from you spin clockwise for two seconds?',
'motor2 not working properly',
test_motor_backward)
k += 1
test('{}: ENCODER 2 CLOCKWISE'.format(k), (position2, position1), '',
'', test_encoder)
input(
'< We will spin the wheels. Lift your MIP up in the air and hit <ENTER>'
)
k += 1
test(
'{}: MOTOR 1 TWO SPEEDS'.format(k), ('pwm1', 'encoder1'),
'Did wheel nearest to you spin counter-clockwise at full speed then slowed down to half speed?',
'motor1 not working properly', test_motor_speeds)
input(
'< We will spin the wheels. Lift your MIP up in the air and hit <ENTER>'
)
k += 1
test(
'{}: MOTOR 2 TWO SPEEDS'.format(k), ('pwm2', 'encoder2'),
'Did the wheel farthest from you spin counter-clockwise at full speed then slowed down to half speed?',
'motor2 not working properly', test_motor_speeds)
k += 1
position1, position2 \
= test('{}: ENCODER 1 RESET'.format(k),
('encoder1',),
'',
'',
test_reset_encoder)
k += 1
position1, position2 \
= test('{}: ENCODER 2 RESET'.format(k),
('encoder2',),
'',
'',
test_reset_encoder)
k += 1
test('{}: TEST THETA'.format(k), ('imu', ), '', '', test_theta)
print("""
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* ! ! ! C O N G R A T U L A T I O N S ! ! ! *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
Your MIP seems to be working and is wired correctly.
""")
except KeyboardInterrupt:
print("> SETUP MIP aborted...")
def get_period(self):
return self.period
if __name__ == "__main__":
import time
import pyctrl.sim as sim
import pyctrl.block as block
import pyctrl.block.logger as logger
a = 17 # 1/s
k = 0.11 # cycles/s duty
controller = sim.Controller(a=a, k=k)
controller.add_sink('printer', block.Printer(endln='\r'),
['clock', 'motor1', 'input1', 'encoder1'])
print(controller.info('all'))
logger = logger.Logger()
controller.add_sink('logger', logger, ['clock', 'encoder1'])
print('> IDLE')
with controller:
time.sleep(1)
log = controller.get_sink('logger', 'log')
controller.remove_sink('logger')
print(72 * '*')
print('*' + 29 * ' ' + 'COSMOS SETUP' + 29 * ' ' + '*')
print(72 * '*')
print('> Confirm parameters:')
host = input(' HOST[{}] = '.format(HOST))
if host:
HOST = host
port = input(' PORT[{}] = '.format(PORT))
if port:
PORT = int(port)
print('> Conecting to {}({})...'.format(HOST, PORT))
device = Controller(host=HOST, port=PORT)
device.add_sink('logger', logger.Logger(), ['clock', 'encoder1'])
device.add_sink('printer', block.Printer(endln='\r'),
['clock', 'motor1', 'encoder1'])
print(device.info('all'))
# define tests
def test(k, args, query_msg, failed_msg, test_function):
print('\n> TEST #{}'.format(k))
passed, retval = test_function(args)
if not passed:
print("> Failed test #{}: {}".format(k, retval))
sys.exit(2)
time.sleep(.1)
if query_msg:
import time, math
import pyctrl.block as block
from pyctrl.block.linear import Feedback, Gain
# initialize robut
robut = Controller()
print("> WELCOME TO ROBUT")
print(robut.info('all'))
# install printer
robut.add_sink(
'printer',
block.Printer(endln='\r'),
[
'clock',
'motor1',
'encoder1',
'motor2',
'encoder2',
#'imu',
'mic1',
'mic2',
'prox1',
'prox2'
])
# install controller
robut.add_signal('reference1')