From 21bbaf1ddf0a3039acf2f089f24d3793aaa5da95 Mon Sep 17 00:00:00 2001 From: bp Date: Mon, 18 Jul 2022 13:08:34 +0200 Subject: [PATCH 01/51] Add: the VelocityRampRunner from the PyTrinamicTools repo * because PyTrinamicTools will be retired --- pytrinamic/tools/__init__.py | 1 + pytrinamic/tools/tests/test_ramp_runner.py | 82 ++++++++++++++++++++++ pytrinamic/tools/velocity_ramp_runner.py | 78 ++++++++++++++++++++ 3 files changed, 161 insertions(+) create mode 100644 pytrinamic/tools/__init__.py create mode 100644 pytrinamic/tools/tests/test_ramp_runner.py create mode 100644 pytrinamic/tools/velocity_ramp_runner.py diff --git a/pytrinamic/tools/__init__.py b/pytrinamic/tools/__init__.py new file mode 100644 index 00000000..31a979de --- /dev/null +++ b/pytrinamic/tools/__init__.py @@ -0,0 +1 @@ +from .velocity_ramp_runner import VelocityRampRunner diff --git a/pytrinamic/tools/tests/test_ramp_runner.py b/pytrinamic/tools/tests/test_ramp_runner.py new file mode 100644 index 00000000..c78107c4 --- /dev/null +++ b/pytrinamic/tools/tests/test_ramp_runner.py @@ -0,0 +1,82 @@ +"""Testing the ramp module + +Run this test using ether the command-line of the unittest framework [1] or simply call the script directly + +[1]: https://docs.python.org/3/library/unittest.html#command-line-interface +""" + +import time +import unittest +from unittest.mock import Mock, call + +from pytrinamic.tools import VelocityRampRunner + + +class TestVelocityRampRunner(unittest.TestCase): + """Contains the tests for the VelocityRampRunner""" + + def test_fixed_cycle_time(self): + """Run a linear ramp and see if the velocity is updated as expected.""" + update_mock = Mock() + ramp_runner = VelocityRampRunner(update_mock, update_cycle_time_ms=10) + + # call the method under test + ramp_runner.run_linear_ramp(0, 100, 40) + + expected_velocity_update_calls = [call(0), call(25), call(50), call(75), call(100)] + update_mock.assert_has_calls(expected_velocity_update_calls) + + def test_fixed_cycle_time_delay_time(self): + """Run a linear ramp and see if the delay is plausible""" + update_mock = Mock() + ramp_runner = VelocityRampRunner(update_mock, update_cycle_time_ms=100) + + # call the method under test + start_time = time.perf_counter() + ramp_runner.run_linear_ramp(0, 100, 400) + stop_time = time.perf_counter() + + delay_s = stop_time-start_time + assert 0.35 < delay_s < 0.45 + + def test_fixed_cycle_time_imprecise_25(self): + """Run a linear ramp with an interval that is not a multiple of update_cycle_time_ms""" + update_mock = Mock() + ramp_runner = VelocityRampRunner(update_mock, update_cycle_time_ms=10) + + # call the method under test + ramp_runner.run_linear_ramp(0, 100, 25) + + expected_velocity_update_calls = [call(0), call(40), call(80), call(100)] + update_mock.assert_has_calls(expected_velocity_update_calls) + + def test_fixed_cycle_time_imprecise_35(self): + """Run a linear ramp with an interval that is not a multiple of update_cycle_time_ms""" + update_mock = Mock() + ramp_runner = VelocityRampRunner(update_mock, update_cycle_time_ms=10) + + # call the method under test + ramp_runner.run_linear_ramp(0, 100, 35) + + expected_velocity_update_calls = [call(0), call(28), call(57), call(85), call(100)] + update_mock.assert_has_calls(expected_velocity_update_calls) + + def test_without_given_cycle_time(self): + """Run a linear ramp and see if the velocity is updated as expected.""" + update_mock = Mock() + ramp_runner = VelocityRampRunner(update_mock) + + # we mock the internal time method to emulate calls to time.time + ramp_runner._time_ms = Mock() + # the _time_ms method is about to return thees values + ramp_runner._time_ms.side_effect = [10, 30, 50] + + # call the method under test + ramp_runner.run_linear_ramp(0, 100, 40) + + expected_velocity_update_calls = [call(0), call(50), call(100)] + update_mock.assert_has_calls(expected_velocity_update_calls) + + +if __name__ == "__main__": + unittest.main() diff --git a/pytrinamic/tools/velocity_ramp_runner.py b/pytrinamic/tools/velocity_ramp_runner.py new file mode 100644 index 00000000..8d1fc207 --- /dev/null +++ b/pytrinamic/tools/velocity_ramp_runner.py @@ -0,0 +1,78 @@ +"""Module to run different ramp motions. + +Please note that thees functions work in blocking mode. +""" + +import time +import math + + +class VelocityRampRunner: + """The VelocityRampRunner allows you to ramp up or down the velocity of a motor. + + This is helpful for modules/drives that do not have a velocity ramp features build in. + + You just give a start velocity, a target velocity and the time in which you like the velocity to go up/down from + the start to the target velocity, and run_linear_ramp will update the velocity at the maximum possible interval, or + by the interval given in the constructor (update_cycle_time_ms). + """ + + def __init__(self, velocity_update_callback, update_cycle_time_ms=0): + """If update_cycle_time_ms is set to 0, the velocity is updated as fast as possible. + + :argument velocity_update_callback: You need to give a function that updates the velocity of your motor. + The signature of the function should look like this: + def callback(int velocity): + :argument update_cycle_time_ms: Optional update interval, if set to zero or omitted the velocity will be updated + as fast as possible. + """ + self._velocity_update_callback = velocity_update_callback + self._update_cycle_time_ms = update_cycle_time_ms + + def run_linear_ramp(self, start_velocity_rpm, target_velocity_rpm, time_delta_ms): + """Update the velocity on a linear basis from start_velocity to the target_velocity within the given time.""" + if self._update_cycle_time_ms: + self._velocity_ramp_fixed_cycle(start_velocity_rpm, target_velocity_rpm, time_delta_ms) + else: + self._velocity_ramp_fast(start_velocity_rpm, target_velocity_rpm, time_delta_ms) + + def _velocity_ramp_fixed_cycle(self, start_velocity_rpm, target_velocity_rpm, time_delta_ms): + """Sub function that updates the velocity on a update_cycle_time_ms basis. + + Note that if time_delta_ms can not be divided by update_cycle_time_ms, + the linear ramp will be longer than expected. This way we make sure there is + no high acceleration at the end. + """ + update_cycles = math.ceil(time_delta_ms / self._update_cycle_time_ms) + acceleration = (target_velocity_rpm - start_velocity_rpm) / time_delta_ms + for i in range(update_cycles): + start_time = self._time_ms() + velocity_update = acceleration * (i * self._update_cycle_time_ms) + start_velocity_rpm + self._velocity_update_callback(int(velocity_update)) + stop_time = self._time_ms() + delay_time_ms = (stop_time - start_time) + if delay_time_ms < self._update_cycle_time_ms: + time.sleep((self._update_cycle_time_ms - delay_time_ms) / 1000) + # always set the target_velocity_rpm at the end + self._velocity_update_callback(int(target_velocity_rpm)) + + def _velocity_ramp_fast(self, start_velocity_rpm, target_velocity_rpm, time_delta_ms): + """Sub function that updates the velocity as fast as possible. + + Remark: Seems to update the velocity on a 1/4 ms basis on my machine + """ + start_time = self._time_ms() + acceleration = (target_velocity_rpm - start_velocity_rpm) / time_delta_ms + self._velocity_update_callback(start_velocity_rpm) + stop_time = self._time_ms() + delay_time_ms = (stop_time - start_time) + while delay_time_ms < time_delta_ms: + velocity_update = acceleration * delay_time_ms + start_velocity_rpm + self._velocity_update_callback(int(velocity_update)) + stop_time = self._time_ms() + delay_time_ms = (stop_time - start_time) + self._velocity_update_callback(target_velocity_rpm) + + @classmethod + def _time_ms(cls): + return time.perf_counter() * 1000 From fa7953e288af55b466ab7c972bfc612b39c751c2 Mon Sep 17 00:00:00 2001 From: bp Date: Wed, 24 Aug 2022 14:52:28 +0200 Subject: [PATCH 02/51] Add: optional rx timeout arguments for the interfaces * the timeout can also be given to the ConnectionManager --- pytrinamic/connections/connection_manager.py | 27 ++++++- .../connections/dummy_tmcl_interface.py | 3 +- .../connections/ixxat_tmcl_interface.py | 5 +- .../connections/kvaser_tmcl_interface.py | 5 +- pytrinamic/connections/pcan_tmcl_interface.py | 5 +- .../connections/serial_tmcl_interface.py | 6 +- .../connections/slcan_tmcl_interface.py | 5 +- .../connections/socketcan_tmcl_interface.py | 5 +- pytrinamic/connections/uart_ic_interface.py | 4 +- tests/test_interface_timeouts.py | 70 +++++++++++++++++++ 10 files changed, 116 insertions(+), 19 deletions(-) create mode 100644 tests/test_interface_timeouts.py diff --git a/pytrinamic/connections/connection_manager.py b/pytrinamic/connections/connection_manager.py index bf08699f..39e12016 100644 --- a/pytrinamic/connections/connection_manager.py +++ b/pytrinamic/connections/connection_manager.py @@ -65,6 +65,11 @@ class which allows repeated connect() and disconnect() calls. Default value: 115200 + --timeout + The rx timeout. The value should be larger than 0. + + Default value: 5.0 + --host-id The host id to use with a TMCL connection. @@ -156,6 +161,9 @@ def __init__(self, arg_list=None, connection_type="any", debug=False): # No data rate has been set -> keep old value pass + # Timeout + self.__timeout_s = args.timeout_s[0] + # Host ID try: self.__host_id = int(args.host_id[0]) @@ -174,6 +182,7 @@ def __init__(self, arg_list=None, connection_type="any", debug=False): print("\tPort: " + self.__port) print("\tBlacklist: " + str(self.__no_port)) print("\tData rate: " + str(self.__data_rate)) + print("\tTimeout: " + str(self.__timeout_s)) print("\tHost ID: " + str(self.__host_id)) print("\tModule ID: " + str(self.__module_id)) print() @@ -243,10 +252,10 @@ def connect(self, debug_interface=None): if self.__interface.supports_tmcl(): # Open the connection to a TMCL interface self.__connection = self.__interface(port, self.__data_rate, self.__host_id, self.__module_id, - debug=debug_interface) + debug=debug_interface, timeout_s=self.__timeout_s) else: # Open the connection to a direct IC interface - self.__connection = self.__interface(port, self.__data_rate, debug=debug_interface) + self.__connection = self.__interface(port, self.__data_rate, debug=debug_interface, timeout_s=self.__timeout_s) except ConnectionError as e: raise ConnectionError("Couldn't connect to port " + port + ". Connection failed.") from e @@ -303,6 +312,18 @@ def argparse(arg_parser): script, this function adds the arguments of the ConnectionManager to the argparse parser. """ + class GreaterThanZeroFloat: + """Checker for float choice. + + assert 1.0 == GreaterThanZeroFloat() + assert 0.0 != GreaterThanZeroFloat() + """ + def __eq__(self, other): + return other > 0.0 + + def __repr__(self): + return "x > 0.0" + group = arg_parser.add_argument_group("ConnectionManager options") group.add_argument('--interface', dest='interface', action='store', nargs=1, type=str, choices=[actual_interface[0] for actual_interface in ConnectionManager.INTERFACES], @@ -313,6 +334,8 @@ def argparse(arg_parser): help='Exclude ports') group.add_argument('--data-rate', dest='data_rate', action='store', nargs=1, type=int, help='Connection data-rate (default: %(default)s)') + group.add_argument('--timeout', dest='timeout_s', action='store', nargs=1, type=float, default=[5.0], choices=[GreaterThanZeroFloat()], + help='Connection rx timeout in seconds(default: %(default)s)', metavar='') group = arg_parser.add_argument_group("ConnectionManager TMCL options") diff --git a/pytrinamic/connections/dummy_tmcl_interface.py b/pytrinamic/connections/dummy_tmcl_interface.py index e83e49cb..02948ce9 100644 --- a/pytrinamic/connections/dummy_tmcl_interface.py +++ b/pytrinamic/connections/dummy_tmcl_interface.py @@ -3,7 +3,7 @@ class DummyTmclInterface(TmclInterface): - def __init__(self, port, datarate=115200, host_id=2, module_id=1, debug=True): + def __init__(self, port, datarate=115200, host_id=2, module_id=1, debug=True, timeout_s=5): """ Opens a dummy TMCL connection """ @@ -17,6 +17,7 @@ def __init__(self, port, datarate=115200, host_id=2, module_id=1, debug=True): print("\tData rate: " + str(datarate)) print("\tHost ID: " + str(host_id)) print("\tModule ID: " + str(module_id)) + print("\tTimeout: " + str(timeout_s)) def __enter__(self): return self diff --git a/pytrinamic/connections/ixxat_tmcl_interface.py b/pytrinamic/connections/ixxat_tmcl_interface.py index 998f2e86..5e8662d5 100644 --- a/pytrinamic/connections/ixxat_tmcl_interface.py +++ b/pytrinamic/connections/ixxat_tmcl_interface.py @@ -27,7 +27,7 @@ class IxxatTmclInterface(TmclInterface): print("Found IXXAT adapter with hardware id '%s'." % hwid) """ - def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=False): + def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): if not isinstance(port, str): raise TypeError @@ -37,6 +37,7 @@ def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=Fal TmclInterface.__init__(self, host_id, module_id, debug) self._channel = port self._bitrate = datarate + self._timeout_s = timeout_s try: if self._debug: @@ -100,7 +101,7 @@ def _recv(self, host_id, module_id): del module_id try: - msg = self._connection.recv(timeout=3) + msg = self._connection.recv(timeout=self._timeout_s) except CanError as e: raise ConnectionError( f"Failed to receive a TMCL message from {self.__class__.__name__} (channel {str(self._channel)})" diff --git a/pytrinamic/connections/kvaser_tmcl_interface.py b/pytrinamic/connections/kvaser_tmcl_interface.py index f283f20f..3c6d7f69 100644 --- a/pytrinamic/connections/kvaser_tmcl_interface.py +++ b/pytrinamic/connections/kvaser_tmcl_interface.py @@ -10,7 +10,7 @@ class KvaserTmclInterface(TmclInterface): This class implements a TMCL connection for Kvaser adapter using CANLIB. Try 0 as default channel. """ - def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=False): + def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): if not isinstance(port, str): raise TypeError @@ -20,6 +20,7 @@ def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=Fal TmclInterface.__init__(self, host_id, module_id, debug) self._channel = port self._bitrate = datarate + self._timeout_s = timeout_s try: if self._debug: @@ -75,7 +76,7 @@ def _recv(self, host_id, module_id): del module_id try: - msg = self._connection.recv(timeout=3) + msg = self._connection.recv(timeout=self._timeout_s) except CanError as e: raise ConnectionError("Failed to receive a TMCL message") from e diff --git a/pytrinamic/connections/pcan_tmcl_interface.py b/pytrinamic/connections/pcan_tmcl_interface.py index b72b499c..ddda6db8 100644 --- a/pytrinamic/connections/pcan_tmcl_interface.py +++ b/pytrinamic/connections/pcan_tmcl_interface.py @@ -33,7 +33,7 @@ class PcanTmclInterface(TmclInterface): """ This class implements a TMCL connection over a PCAN adapter. """ - def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False): + def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): if not isinstance(port, str): raise TypeError @@ -43,6 +43,7 @@ def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False): TmclInterface.__init__(self, host_id, module_id, debug) self._channel = port self._bitrate = datarate + self._timeout_s = timeout_s try: self._connection = can.Bus(interface="pcan", channel=self._channel, bitrate=self._bitrate) @@ -95,7 +96,7 @@ def _recv(self, host_id, module_id): del module_id try: - msg = self._connection.recv(timeout=3) + msg = self._connection.recv(timeout=self._timeout_s) except CanError as e: raise ConnectionError("Failed to receive a TMCL message") from e diff --git a/pytrinamic/connections/serial_tmcl_interface.py b/pytrinamic/connections/serial_tmcl_interface.py index 5f6a8a0b..b719292c 100644 --- a/pytrinamic/connections/serial_tmcl_interface.py +++ b/pytrinamic/connections/serial_tmcl_interface.py @@ -8,7 +8,7 @@ class SerialTmclInterface(TmclInterface): """ Opens a serial TMCL connection """ - def __init__(self, com_port, datarate=115200, host_id=2, module_id=1, debug=False): + def __init__(self, com_port, datarate=115200, host_id=2, module_id=1, debug=False, timeout_s=5): if not isinstance(com_port, str): raise TypeError @@ -16,12 +16,10 @@ def __init__(self, com_port, datarate=115200, host_id=2, module_id=1, debug=Fals self._baudrate = datarate try: - self._serial = Serial(com_port, self._baudrate) + self._serial = Serial(com_port, self._baudrate, timeout=timeout_s) except SerialException as e: raise ConnectionError from e - self._serial.timeout = 5 - if self._debug: print("Opened port: " + self._serial.portstr) diff --git a/pytrinamic/connections/slcan_tmcl_interface.py b/pytrinamic/connections/slcan_tmcl_interface.py index e3b953b5..335af792 100644 --- a/pytrinamic/connections/slcan_tmcl_interface.py +++ b/pytrinamic/connections/slcan_tmcl_interface.py @@ -12,13 +12,14 @@ class SlcanTmclInterface(TmclInterface): Maybe SerialBaudrate has to be changed based on adapter. """ - def __init__(self, com_port, datarate=1000000, host_id=2, module_id=1, debug=True, serial_baudrate=115200): + def __init__(self, com_port, datarate=1000000, host_id=2, module_id=1, debug=True, timeout_s=5, serial_baudrate=115200): if not isinstance(com_port, str): raise TypeError TmclInterface.__init__(self, host_id, module_id, debug) self._bitrate = datarate self._port = com_port + self._timeout_s = timeout_s self._serial_baudrate = serial_baudrate try: @@ -74,7 +75,7 @@ def _recv(self, host_id, module_id): del module_id try: - msg = self._connection.recv(timeout=3) + msg = self._connection.recv(timeout=self._timeout_s) except CanError as e: raise ConnectionError("Failed to receive a TMCL message") from e diff --git a/pytrinamic/connections/socketcan_tmcl_interface.py b/pytrinamic/connections/socketcan_tmcl_interface.py index f313f93a..8eeaba20 100644 --- a/pytrinamic/connections/socketcan_tmcl_interface.py +++ b/pytrinamic/connections/socketcan_tmcl_interface.py @@ -12,7 +12,7 @@ class SocketcanTmclInterface(TmclInterface): Use following command under linux to activate can socket sudo ip link set can0 down type can bitrate 1000000 """ - def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False): + def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): if not isinstance(port, str): raise TypeError @@ -22,6 +22,7 @@ def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False): TmclInterface.__init__(self, host_id, module_id, debug) self._channel = port self._bitrate = datarate + self._timeout_s = timeout_s try: self._connection = can.Bus(interface="socketcan", channel=self._channel, bitrate=self._bitrate) @@ -76,7 +77,7 @@ def _recv(self, host_id, module_id): del module_id try: - msg = self._connection.recv(timeout=3) + msg = self._connection.recv(timeout=self._timeout_s) except CanError as e: raise ConnectionError("Failed to receive a TMCL message") from e diff --git a/pytrinamic/connections/uart_ic_interface.py b/pytrinamic/connections/uart_ic_interface.py index cd6f6132..9f45a715 100644 --- a/pytrinamic/connections/uart_ic_interface.py +++ b/pytrinamic/connections/uart_ic_interface.py @@ -32,10 +32,10 @@ def value(self): class UartIcInterface: - def __init__(self, com_port, datarate=9600, debug=False): + def __init__(self, com_port, datarate=9600, debug=False, timeout_s=5): self._debug = debug self.baudrate = datarate - self.serial = Serial(com_port, self.baudrate) + self.serial = Serial(com_port, self.baudrate, timeout=timeout_s) print("Open port: " + self.serial.portstr) def __enter__(self): diff --git a/tests/test_interface_timeouts.py b/tests/test_interface_timeouts.py new file mode 100644 index 00000000..4223e688 --- /dev/null +++ b/tests/test_interface_timeouts.py @@ -0,0 +1,70 @@ +"""Test for the ConnectionManager timeout parameter. + +A (virtual) comport and a Kvaser CAN-Adapter are needed to run these tests. +Note, you may need change the comport number. +""" + +import time + +import pytest + +from pytrinamic.connections import ConnectionManager + + +@pytest.mark.parametrize('add_argument,expected_timeout', [ + ('', 5), + (' --timeout 7', 7), + (' --timeout 200', 200), + (' --timeout 33.3', 33.3), +]) +def test_serial_tmcl(add_argument, expected_timeout): + """Check if the timeout is forwarded to the serial interface.""" + + cm = ConnectionManager("--interface serial_tmcl --port COM4 --data-rate 115200" + add_argument) + + with cm.connect() as myinterface: + assert myinterface._serial.timeout == expected_timeout + + +@pytest.mark.parametrize('add_argument,expected_timeout', [ + ('', 5), + (' --timeout 7', 7), + (' --timeout 200', 200), + (' --timeout 33.3', 33.3), +]) +def test_kvaser_tmcl(add_argument, expected_timeout): + """Check if the timeout is forwarded to the Kvaser interface.""" + + cm = ConnectionManager("--interface kvaser_tmcl" + add_argument) + + with cm.connect() as myinterface: + assert myinterface._timeout_s == expected_timeout + + +@pytest.mark.parametrize('con_man_call,expected_exception', [ + ('--interface serial_tmcl --port COM4 --data-rate 115200', RuntimeError), + ('--interface kvaser_tmcl', ConnectionError), +]) +def test_actual_timeout(con_man_call, expected_exception): + """We just call the receive-function without sending anything and check if a timeout will be raised.""" + + cm = ConnectionManager(f"{con_man_call} --timeout 1.5") + + with pytest.raises(expected_exception) as exec_info: + with cm.connect() as myinterface: + start_time = time.perf_counter() + myinterface._recv(0, 0) + stop_time = time.perf_counter() + duration = stop_time - start_time + assert 1.5 < duration < 1.7 + + +@pytest.mark.parametrize('timeout_argument', [ + 'string', + '0', + '0x1F' +]) +def test_invalid_timeout(timeout_argument): + """Test some invalid arguments for the timeout value.""" + with pytest.raises(SystemExit) as exec_info: + ConnectionManager(f"--interface serial_tmcl --port COM4 --data-rate 115200 --timeout {timeout_argument}") From 2f977678e83f913427158ed38fa6a114c0f15425 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Thu, 25 Aug 2022 12:11:43 +0200 Subject: [PATCH 03/51] Updated optional RX timeout argparse option - Changed the visual representation to not be a choice (the square brackets shown in the defailt) - Changed timeout to allow zero (to be implemented as timeout disabled) - Removed the "" hint in the help message --- pytrinamic/connections/connection_manager.py | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/pytrinamic/connections/connection_manager.py b/pytrinamic/connections/connection_manager.py index 39e12016..280f5149 100644 --- a/pytrinamic/connections/connection_manager.py +++ b/pytrinamic/connections/connection_manager.py @@ -312,17 +312,15 @@ def argparse(arg_parser): script, this function adds the arguments of the ConnectionManager to the argparse parser. """ - class GreaterThanZeroFloat: - """Checker for float choice. - - assert 1.0 == GreaterThanZeroFloat() - assert 0.0 != GreaterThanZeroFloat() + def _positive_float(value): + """ + Argparse checker for float a positive float type. """ - def __eq__(self, other): - return other > 0.0 + value_float = float(value) + if value_float < 0: + raise argparse.ArgumentTypeError("Expected a positive float, got {}".format(value_float)) - def __repr__(self): - return "x > 0.0" + return value_float group = arg_parser.add_argument_group("ConnectionManager options") group.add_argument('--interface', dest='interface', action='store', nargs=1, type=str, @@ -334,8 +332,8 @@ def __repr__(self): help='Exclude ports') group.add_argument('--data-rate', dest='data_rate', action='store', nargs=1, type=int, help='Connection data-rate (default: %(default)s)') - group.add_argument('--timeout', dest='timeout_s', action='store', nargs=1, type=float, default=[5.0], choices=[GreaterThanZeroFloat()], - help='Connection rx timeout in seconds(default: %(default)s)', metavar='') + group.add_argument('--timeout', dest='timeout_s', action='store', nargs=1, type=_positive_float, default=5.0, + help='Connection rx timeout in seconds (default: %(default)s)', metavar="SECONDS") group = arg_parser.add_argument_group("ConnectionManager TMCL options") From 394a1ab710ba492cf112af1f107c56a7f20ec27f Mon Sep 17 00:00:00 2001 From: Augustin Bielefeld Date: Thu, 1 Sep 2022 13:24:02 +0200 Subject: [PATCH 04/51] TMC5160/rotate_demo: Set default lower current in demo By default, using a NEMA17, the current is way too high and could damage motor, power stage, or power supply. --- examples/evalboards/TMC5160/rotate_demo.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/examples/evalboards/TMC5160/rotate_demo.py b/examples/evalboards/TMC5160/rotate_demo.py index f07d3be3..bbdab8d0 100644 --- a/examples/evalboards/TMC5160/rotate_demo.py +++ b/examples/evalboards/TMC5160/rotate_demo.py @@ -1,5 +1,9 @@ """ Move a motor back and forth using velocity and position mode of the TMC5160 + +Line 31, we set a lower run/standby current for the motor. Using NEMA17, this should result in a coil current around 800mA. +If the motor is stalling due to too low current, set motorCurrent higher. +If a lower value still is needed, set GLOBAL_SCALER register to 128 to half motor current. """ import time import pytrinamic @@ -16,7 +20,7 @@ mc = eval_board.ics[0] motor = eval_board.motors[0] - print("Preparing parameter...") + print("Preparing parameters...") eval_board.write_register(mc.REG.A1, 1000) eval_board.write_register(mc.REG.V1, 50000) eval_board.write_register(mc.REG.D1, 500) @@ -25,11 +29,16 @@ eval_board.write_register(mc.REG.VSTOP, 10) eval_board.write_register(mc.REG.AMAX, 1000) + # Set lower run/standby current + motorCurrent = 2 + motor.set_axis_parameter(motor.AP.MaxCurrent, motorCurrent) + motor.set_axis_parameter(motor.AP.StandbyCurrent, motorCurrent) + # Clear actual positions motor.actual_position = 0 print("Rotating...") - motor.rotate(7*25600) + motor.rotate(7 * 25600) time.sleep(5) print("Stopping...") @@ -37,7 +46,7 @@ time.sleep(1) print("Moving back to 0...") - motor.move_to(0, 100000) + motor.move_to(0, 7 * 25600) # Wait until position 0 is reached while motor.actual_position != 0: From 9ce330c62d53cd3214d214c79d0d7193d3687eee Mon Sep 17 00:00:00 2001 From: bp Date: Thu, 1 Sep 2022 19:27:15 +0200 Subject: [PATCH 05/51] Update timeout attribute, allowing 0 and None as timeout value * both 0 and None, will never cause a timeout, in this case the send() function will block till a response is coming in --- pytrinamic/connections/connection_manager.py | 4 +- .../connections/ixxat_tmcl_interface.py | 5 +- .../connections/kvaser_tmcl_interface.py | 5 +- pytrinamic/connections/pcan_tmcl_interface.py | 5 +- .../connections/serial_tmcl_interface.py | 2 + .../connections/slcan_tmcl_interface.py | 5 +- .../connections/socketcan_tmcl_interface.py | 5 +- pytrinamic/connections/uart_ic_interface.py | 2 + tests/test_interface_timeouts.py | 74 ++++++++++++++----- 9 files changed, 82 insertions(+), 25 deletions(-) diff --git a/pytrinamic/connections/connection_manager.py b/pytrinamic/connections/connection_manager.py index 280f5149..283e99f7 100644 --- a/pytrinamic/connections/connection_manager.py +++ b/pytrinamic/connections/connection_manager.py @@ -162,7 +162,7 @@ def __init__(self, arg_list=None, connection_type="any", debug=False): pass # Timeout - self.__timeout_s = args.timeout_s[0] + self.__timeout_s = args.timeout_s # Host ID try: @@ -332,7 +332,7 @@ def _positive_float(value): help='Exclude ports') group.add_argument('--data-rate', dest='data_rate', action='store', nargs=1, type=int, help='Connection data-rate (default: %(default)s)') - group.add_argument('--timeout', dest='timeout_s', action='store', nargs=1, type=_positive_float, default=5.0, + group.add_argument('--timeout', dest='timeout_s', action='store', type=_positive_float, default=5.0, help='Connection rx timeout in seconds (default: %(default)s)', metavar="SECONDS") group = arg_parser.add_argument_group("ConnectionManager TMCL options") diff --git a/pytrinamic/connections/ixxat_tmcl_interface.py b/pytrinamic/connections/ixxat_tmcl_interface.py index 5e8662d5..e178d1ea 100644 --- a/pytrinamic/connections/ixxat_tmcl_interface.py +++ b/pytrinamic/connections/ixxat_tmcl_interface.py @@ -37,7 +37,10 @@ def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=Fal TmclInterface.__init__(self, host_id, module_id, debug) self._channel = port self._bitrate = datarate - self._timeout_s = timeout_s + if timeout_s == 0: + self._timeout_s = None + else: + self._timeout_s = timeout_s try: if self._debug: diff --git a/pytrinamic/connections/kvaser_tmcl_interface.py b/pytrinamic/connections/kvaser_tmcl_interface.py index 3c6d7f69..40c2e5a8 100644 --- a/pytrinamic/connections/kvaser_tmcl_interface.py +++ b/pytrinamic/connections/kvaser_tmcl_interface.py @@ -20,7 +20,10 @@ def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=Fal TmclInterface.__init__(self, host_id, module_id, debug) self._channel = port self._bitrate = datarate - self._timeout_s = timeout_s + if timeout_s == 0: + self._timeout_s = None + else: + self._timeout_s = timeout_s try: if self._debug: diff --git a/pytrinamic/connections/pcan_tmcl_interface.py b/pytrinamic/connections/pcan_tmcl_interface.py index ddda6db8..3b711a1c 100644 --- a/pytrinamic/connections/pcan_tmcl_interface.py +++ b/pytrinamic/connections/pcan_tmcl_interface.py @@ -43,7 +43,10 @@ def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, TmclInterface.__init__(self, host_id, module_id, debug) self._channel = port self._bitrate = datarate - self._timeout_s = timeout_s + if timeout_s == 0: + self._timeout_s = None + else: + self._timeout_s = timeout_s try: self._connection = can.Bus(interface="pcan", channel=self._channel, bitrate=self._bitrate) diff --git a/pytrinamic/connections/serial_tmcl_interface.py b/pytrinamic/connections/serial_tmcl_interface.py index b719292c..41960b19 100644 --- a/pytrinamic/connections/serial_tmcl_interface.py +++ b/pytrinamic/connections/serial_tmcl_interface.py @@ -14,6 +14,8 @@ def __init__(self, com_port, datarate=115200, host_id=2, module_id=1, debug=Fals TmclInterface.__init__(self, host_id, module_id, debug) self._baudrate = datarate + if timeout_s == 0: + timeout_s = None try: self._serial = Serial(com_port, self._baudrate, timeout=timeout_s) diff --git a/pytrinamic/connections/slcan_tmcl_interface.py b/pytrinamic/connections/slcan_tmcl_interface.py index 335af792..f11231bb 100644 --- a/pytrinamic/connections/slcan_tmcl_interface.py +++ b/pytrinamic/connections/slcan_tmcl_interface.py @@ -19,7 +19,10 @@ def __init__(self, com_port, datarate=1000000, host_id=2, module_id=1, debug=Tru TmclInterface.__init__(self, host_id, module_id, debug) self._bitrate = datarate self._port = com_port - self._timeout_s = timeout_s + if timeout_s == 0: + self._timeout_s = None + else: + self._timeout_s = timeout_s self._serial_baudrate = serial_baudrate try: diff --git a/pytrinamic/connections/socketcan_tmcl_interface.py b/pytrinamic/connections/socketcan_tmcl_interface.py index 8eeaba20..1a3d57fa 100644 --- a/pytrinamic/connections/socketcan_tmcl_interface.py +++ b/pytrinamic/connections/socketcan_tmcl_interface.py @@ -22,7 +22,10 @@ def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, TmclInterface.__init__(self, host_id, module_id, debug) self._channel = port self._bitrate = datarate - self._timeout_s = timeout_s + if timeout_s == 0: + self._timeout_s = None + else: + self._timeout_s = timeout_s try: self._connection = can.Bus(interface="socketcan", channel=self._channel, bitrate=self._bitrate) diff --git a/pytrinamic/connections/uart_ic_interface.py b/pytrinamic/connections/uart_ic_interface.py index 9f45a715..e2f7d799 100644 --- a/pytrinamic/connections/uart_ic_interface.py +++ b/pytrinamic/connections/uart_ic_interface.py @@ -35,6 +35,8 @@ class UartIcInterface: def __init__(self, com_port, datarate=9600, debug=False, timeout_s=5): self._debug = debug self.baudrate = datarate + if timeout_s == 0: + timeout_s = None self.serial = Serial(com_port, self.baudrate, timeout=timeout_s) print("Open port: " + self.serial.portstr) diff --git a/tests/test_interface_timeouts.py b/tests/test_interface_timeouts.py index 4223e688..d3f5c839 100644 --- a/tests/test_interface_timeouts.py +++ b/tests/test_interface_timeouts.py @@ -9,36 +9,73 @@ import pytest from pytrinamic.connections import ConnectionManager +from pytrinamic.connections import KvaserTmclInterface +from pytrinamic.connections import SerialTmclInterface +from pytrinamic.connections import UartIcInterface +@pytest.mark.parametrize('interface,con_man_parameters', [ + ('serial_tmcl', ' --port COM4 --data-rate 115200'), + ('uart_ic', ' --port COM4'), + ('kvaser_tmcl', '') +]) @pytest.mark.parametrize('add_argument,expected_timeout', [ ('', 5), (' --timeout 7', 7), (' --timeout 200', 200), (' --timeout 33.3', 33.3), + (' --timeout 0.0', None), + (' --timeout 0', None), + (' --timeout -0', None), ]) -def test_serial_tmcl(add_argument, expected_timeout): +def test_valid_input_cm(interface, con_man_parameters, add_argument, expected_timeout): """Check if the timeout is forwarded to the serial interface.""" - cm = ConnectionManager("--interface serial_tmcl --port COM4 --data-rate 115200" + add_argument) + cm = ConnectionManager(f"--interface {interface}{con_man_parameters}" + add_argument) with cm.connect() as myinterface: - assert myinterface._serial.timeout == expected_timeout - - -@pytest.mark.parametrize('add_argument,expected_timeout', [ + if interface == 'serial_tmcl': + assert myinterface._serial.timeout == expected_timeout + elif interface == 'uart_ic': + assert myinterface.serial.timeout == expected_timeout + elif interface == 'kvaser_tmcl': + assert myinterface._timeout_s == expected_timeout + else: + pytest.fail('Unexpected interface!') + + +@pytest.mark.parametrize('interface_class', [ + 'kvaser_tmcl', + 'serial_tmcl', + 'uart_ic', +]) +@pytest.mark.parametrize('timeout_input,expected_timeout', [ ('', 5), - (' --timeout 7', 7), - (' --timeout 200', 200), - (' --timeout 33.3', 33.3), + (None, None), + (0, None), + (7, 7), + (33.3, 33.3), ]) -def test_kvaser_tmcl(add_argument, expected_timeout): - """Check if the timeout is forwarded to the Kvaser interface.""" - - cm = ConnectionManager("--interface kvaser_tmcl" + add_argument) - - with cm.connect() as myinterface: +def test_valid_input_direct(interface_class, timeout_input, expected_timeout): + """Test like the test_valid_input_cm() but without the connection manager.""" + if interface_class == 'kvaser_tmcl': + if timeout_input == '': + myinterface = KvaserTmclInterface() + else: + myinterface = KvaserTmclInterface(timeout_s=timeout_input) assert myinterface._timeout_s == expected_timeout + elif interface_class == 'serial_tmcl': + if timeout_input == '': + myinterface = SerialTmclInterface('COM4') + else: + myinterface = SerialTmclInterface('COM4', timeout_s=timeout_input) + assert myinterface._serial.timeout == expected_timeout + elif interface_class == 'uart_ic': + if timeout_input == '': + myinterface = UartIcInterface('COM4') + else: + myinterface = UartIcInterface('COM4', timeout_s=timeout_input) + assert myinterface.serial.timeout == expected_timeout @pytest.mark.parametrize('con_man_call,expected_exception', [ @@ -50,7 +87,7 @@ def test_actual_timeout(con_man_call, expected_exception): cm = ConnectionManager(f"{con_man_call} --timeout 1.5") - with pytest.raises(expected_exception) as exec_info: + with pytest.raises(expected_exception): with cm.connect() as myinterface: start_time = time.perf_counter() myinterface._recv(0, 0) @@ -61,8 +98,9 @@ def test_actual_timeout(con_man_call, expected_exception): @pytest.mark.parametrize('timeout_argument', [ 'string', - '0', - '0x1F' + '0x1F', + '-0.1', + '-100', ]) def test_invalid_timeout(timeout_argument): """Test some invalid arguments for the timeout value.""" From 759382f80d661ca140a888280065fc6f708422a0 Mon Sep 17 00:00:00 2001 From: bp Date: Thu, 1 Sep 2022 19:44:32 +0200 Subject: [PATCH 06/51] Docs: Fix the ConnectionManager docstring --- pytrinamic/connections/connection_manager.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/pytrinamic/connections/connection_manager.py b/pytrinamic/connections/connection_manager.py index 283e99f7..d778c0d3 100644 --- a/pytrinamic/connections/connection_manager.py +++ b/pytrinamic/connections/connection_manager.py @@ -63,10 +63,15 @@ class which allows repeated connect() and disconnect() calls. interpreted depends on the interface used. E.g. the serial connection uses this value as the baud rate. - Default value: 115200 + The Default value also depends on the interface. + * for any CAN interface its 1000000 + * for the serial_tmcl and uard_id interface it is 9600 + * for usb_tmcl it is 115200 --timeout - The rx timeout. The value should be larger than 0. + The rx timeout in seconds. Accepts only values >= 0. + If 0 is given the rx function will block forever. + This might be useful for debugging. Default value: 5.0 From 4a1ededd51521a928cd55efc421c2c628ec6c583 Mon Sep 17 00:00:00 2001 From: Jonathan Hammer Date: Wed, 7 Sep 2022 16:42:44 +0200 Subject: [PATCH 07/51] Added 2240_eval --- examples/evalboards/TMC2240/register_dump.py | 66 ++++++ examples/evalboards/TMC2240/rotate_demo.py | 45 ++++ pytrinamic/evalboards/TMC2240_eval.py | 129 +++++++++++ pytrinamic/evalboards/__init__.py | 1 + pytrinamic/ic/TMC2240.py | 229 +++++++++++++++++++ pytrinamic/ic/__init__.py | 1 + 6 files changed, 471 insertions(+) create mode 100644 examples/evalboards/TMC2240/register_dump.py create mode 100644 examples/evalboards/TMC2240/rotate_demo.py create mode 100644 pytrinamic/evalboards/TMC2240_eval.py create mode 100644 pytrinamic/ic/TMC2240.py diff --git a/examples/evalboards/TMC2240/register_dump.py b/examples/evalboards/TMC2240/register_dump.py new file mode 100644 index 00000000..45161331 --- /dev/null +++ b/examples/evalboards/TMC2240/register_dump.py @@ -0,0 +1,66 @@ +""" +Dump all register values of the TMC2240 IC. + +The connection to a Landungsbrücke is established over USB. TMCL commands are used for communicating with the IC. +""" +import time +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.evalboards import TMC2240_eval + +pytrinamic.show_info() + +with ConnectionManager().connect() as my_interface: + print(my_interface) + + eval_board = TMC2240_eval(my_interface) + mc = eval_board.ics[0] + + + print("GCONF: 0x{0:08X}".format(eval_board.read_register(mc.REG.GCONF))) + print("GSTAT: 0x{0:08X}".format(eval_board.read_register(mc.REG.GSTAT))) + print("IFCNT: 0x{0:08X}".format(eval_board.read_register(mc.REG.IFCNT))) + print("SLAVECONF: 0x{0:08X}".format(eval_board.read_register(mc.REG.SLAVECONF))) + print("IOIN: 0x{0:08X}".format(eval_board.read_register(mc.REG.IOIN))) + print("DRV_CONF: 0x{0:08X}".format(eval_board.read_register(mc.REG.DRV_CONF))) + print("GLOBAL_SCALE 0x{0:08X}".format(eval_board.read_register(mc.REG.GLOBAL_SCALER))) + print("IHOLD_IRUN: 0x{0:08X}".format(eval_board.read_register(mc.REG.IHOLD_IRUN))) + print("TPOWERDOWN: 0x{0:08X}".format(eval_board.read_register(mc.REG.TPOWERDOWN))) + print("TSTEP: 0x{0:08X}".format(eval_board.read_register(mc.REG.TSTEP))) + print("TPWMTHRS: 0x{0:08X}".format(eval_board.read_register(mc.REG.TPWMTHRS))) + print("TCOOLTHRS: 0x{0:08X}".format(eval_board.read_register(mc.REG.TCOOLTHRS))) + print("THIGH: 0x{0:08X}".format(eval_board.read_register(mc.REG.THIGH))) + print("DIRECT_MODE: 0x{0:08X}".format(eval_board.read_register(mc.REG.DIRECT_MODE))) + print("ENCMODE: 0x{0:08X}".format(eval_board.read_register(mc.REG.ENCMODE))) + print("X_ENC: 0x{0:08X}".format(eval_board.read_register(mc.REG.X_ENC))) + print("ENC_CONST: 0x{0:08X}".format(eval_board.read_register(mc.REG.ENC_CONST))) + print("ENC_STATUS: 0x{0:08X}".format(eval_board.read_register(mc.REG.ENC_STATUS))) + print("ENC_LATCH: 0x{0:08X}".format(eval_board.read_register(mc.REG.ENC_LATCH))) + print("ADC_VSUPPLY_AIN: 0x{0:08X}".format(eval_board.read_register(mc.REG.ADC_VSUPPLY_AIN))) + print("ADC_TEMP: 0x{0:08X}".format(eval_board.read_register(mc.REG.ADC_TEMP))) + print("OTW_OV_VTH: 0x{0:08X}".format(eval_board.read_register(mc.REG.OTW_OV_VTH))) + print("MSLUT_0: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSLUT_0))) + print("MSLUT_1: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSLUT_1))) + print("MSLUT_2: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSLUT_2))) + print("MSLUT_3: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSLUT_3))) + print("MSLUT_4: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSLUT_4))) + print("MSLUT_5: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSLUT_5))) + print("MSLUT_6: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSLUT_6))) + print("MSLUT_7: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSLUT_7))) + print("MSLUTSEL: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSLUTSEL))) + print("MSLUTSTART: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSLUTSTART))) + print("MSCNT: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSCNT))) + print("MSCURACT: 0x{0:08X}".format(eval_board.read_register(mc.REG.MSCURACT))) + print("CHOPCONF: 0x{0:08X}".format(eval_board.read_register(mc.REG.CHOPCONF))) + print("COOLCONF: 0x{0:08X}".format(eval_board.read_register(mc.REG.COOLCONF))) + print("DCCTRL: 0x{0:08X}".format(eval_board.read_register(mc.REG.DCCTRL))) + print("DRV_STATUS: 0x{0:08X}".format(eval_board.read_register(mc.REG.DRV_STATUS))) + print("PWMCONF: 0x{0:08X}".format(eval_board.read_register(mc.REG.PWMCONF))) + print("PWM_SCALE: 0x{0:08X}".format(eval_board.read_register(mc.REG.PWM_SCALE))) + print("PWM_AUTO: 0x{0:08X}".format(eval_board.read_register(mc.REG.PWM_AUTO))) + print("SG4_THRS: 0x{0:08X}".format(eval_board.read_register(mc.REG.SG4_THRS))) + print("SG4_RESULT: 0x{0:08X}".format(eval_board.read_register(mc.REG.SG4_RESULT))) + print("SG4_IND: 0x{0:08X}".format(eval_board.read_register(mc.REG.SG4_IND))) + + +print("\nReady.") diff --git a/examples/evalboards/TMC2240/rotate_demo.py b/examples/evalboards/TMC2240/rotate_demo.py new file mode 100644 index 00000000..f6575590 --- /dev/null +++ b/examples/evalboards/TMC2240/rotate_demo.py @@ -0,0 +1,45 @@ +""" +Move a motor back and forth using velocity and position mode of the TMC2240 +""" +import time +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.evalboards import TMC2240_eval + +pytrinamic.show_info() + +with ConnectionManager().connect() as my_interface: + print(my_interface) + + # Create TMC2240-EVAL class which communicates over the Landungsbrücke via TMCL + eval_board = TMC2240_eval(my_interface) + mc = eval_board.ics[0] + motor = eval_board.motors[0] + + print("Preparing parameter...") + motor.set_axis_parameter(motor.AP.MaxAcceleration, 20000) + motor.set_axis_parameter(motor.AP.MaxVelocity, 100000) + motor.set_axis_parameter(motor.AP.MaxCurrent, 30) + + # Clear actual positions + motor.actual_position = 0 + + print("Rotating...") + motor.rotate(7*25600) + time.sleep(5) + + print("Stopping...") + motor.stop() + time.sleep(1) + + print("Moving back to 0...") + motor.move_to(0, 100000) + + # Wait until position 0 is reached + while motor.actual_position != 0: + print("Actual position: " + str(motor.actual_position)) + time.sleep(0.2) + + print("Reached position 0") + +print("\nReady.") diff --git a/pytrinamic/evalboards/TMC2240_eval.py b/pytrinamic/evalboards/TMC2240_eval.py new file mode 100644 index 00000000..c3091265 --- /dev/null +++ b/pytrinamic/evalboards/TMC2240_eval.py @@ -0,0 +1,129 @@ +from pytrinamic.evalboards import TMCLEval +from pytrinamic.ic import TMC2240 +from pytrinamic.features import MotorControlModule +from pytrinamic.helpers import TMC_helpers + + +class TMC2240_eval(TMCLEval): + """ + This class represents a TMC2240 Evaluation board. + + Communication is done over the TMCL commands writeMC and readMC. An + implementation without TMCL may still use this class if these two functions + are provided properly. See __init__ for details on the function + requirements. + """ + def __init__(self, connection, module_id=1): + """ + Parameters: + connection: + Type: class + A class that provides the necessary functions for communicating + with a TMC2240. The required functions are + connection.writeMC(registerAddress, value, moduleID) + connection.readMC(registerAddress, moduleID, signed) + for writing/reading to registers of the TMC2240. + module_id: + Type: int, optional, default value: 1 + The TMCL module ID of the TMC2240. This ID is used as a + parameter for the writeMC and readMC functions. + """ + TMCLEval.__init__(self, connection, module_id) + self.motors = [self._MotorTypeA(self, 0)] + self.ics = [TMC2240()] + + # Use the motion controller functions for register access + + def write_register(self, register_address, value): + return self._connection.read_drv(register_address, value, self._module_id) + + def read_register(self, register_address, signed=False): + return self._connection.read_drv(register_address, self._module_id, signed) + + def write_register_field(self, field, value): + return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), + field[1], field[2], value)) + + def read_register_field(self, field): + return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) + + # Motion control functions + + def rotate(self, motor, value): + self._connection.rotate(motor, value) + + def stop(self, motor): + self._connection.stop(motor) + + def move_to(self, motor, position, velocity=None): + if velocity and velocity != 0: + # Set maximum positioning velocity + self.motors[motor].set_axis_parameter(self.motors[motor].AP.MaxVelocity, velocity) + self._connection.move_to(motor, position, self._module_id) + + class _MotorTypeA(MotorControlModule): + def __init__(self, eval_board, axis): + MotorControlModule.__init__(self, eval_board, axis, self.AP) + + class AP: + TargetPosition = 0 + ActualPosition = 1 + TargetVelocity = 2 + ActualVelocity = 3 + MaxVelocity = 4 + MaxAcceleration = 5 + MaxCurrent = 6 + StandbyCurrent = 7 + PositionReachedFlag = 8 + THIGH = 26 + HighSpeedChopperMode = 28 + HighSpeedFullstepMode = 29 + MeasuredSpeed = 30 + internal_Rsense = 34 + GlobalCurrentScaler = 35 + MicrostepResolution = 140 + ChopperBlankTime = 162 + ConstantTOffMode = 163 + DisableFastDecayComparator = 164 + ChopperHysteresisEnd = 165 + ChopperHysteresisStart = 166 + TOff = 167 + SEIMIN = 168 + SECDS = 169 + smartEnergyHysteresis = 170 + SECUS = 171 + smartEnergyHysteresisStart = 172 + SG2FilterEnable = 173 + SG2Threshold = 174 + smartEnergyActualCurrent = 180 + smartEnergyStallVelocity = 181 + smartEnergyThresholdSpeed = 182 + SG4FilterEnable = 183 + SGAngleOffset = 184 + ChopperSynchronization = 185 + PWMThresholdSpeed = 186 + PWMGrad = 187 + PWMAmplitude = 188 + PWMFrequency = 191 + PWMAutoscale = 192 + PWMScaleSum = 193 + MSCNT = 194 + MEAS_SD_EN = 195 + DIS_REG_STST = 196 + FreewheelingMode = 204 + LoadValue = 206 + EncoderPosition = 209 + EncoderResolution = 210 + CurrentScalingSelector = 211 + CurrentRange = 212 + ADCTemperature = 213 + ADCIN = 214 + ADCSupply = 215 + ADCOvervoltageLimit = 216 + ADCOvertemperatureWarningLimit = 217 + Temperature = 218 + AIN = 219 + VSupply = 220 + OvervoltageLimit = 221 + OvertemperatureWarningLimit = 222 + nSLEEP = 223 diff --git a/pytrinamic/evalboards/__init__.py b/pytrinamic/evalboards/__init__.py index 386690a7..078e6791 100644 --- a/pytrinamic/evalboards/__init__.py +++ b/pytrinamic/evalboards/__init__.py @@ -7,6 +7,7 @@ from .TMC2209_eval import TMC2209_eval from .TMC2224_eval import TMC2224_eval from .TMC2225_eval import TMC2225_eval +from .TMC2240_eval import TMC2240_eval from .TMC2300_eval import TMC2300_eval from .TMC2590_eval import TMC2590_eval from .TMC2660_eval import TMC2660_eval diff --git a/pytrinamic/ic/TMC2240.py b/pytrinamic/ic/TMC2240.py new file mode 100644 index 00000000..c5e20454 --- /dev/null +++ b/pytrinamic/ic/TMC2240.py @@ -0,0 +1,229 @@ +from ..ic.tmc_ic import TMCIc + + +class TMC2240(TMCIc): + """ + The TMC2240-A is a stepper motor controller and driver IC with serial communication interfaces. + Supply voltage: 4.5-36V. + """ + def __init__(self): + super().__init__("TMC2240", self.__doc__) + + class REG: + """ + Define all registers of the TMC2240. + """ + GCONF = 0x00 + GSTAT = 0x01 + IFCNT = 0x02 + SLAVECONF = 0x03 + IOIN = 0x04 + DRV_CONF = 0x0A + GLOBAL_SCALER = 0x0B + IHOLD_IRUN = 0x10 + TPOWERDOWN = 0x11 + TSTEP = 0x12 + TPWMTHRS = 0x13 + TCOOLTHRS = 0x14 + THIGH = 0x15 + DIRECT_MODE = 0x2D + ENCMODE = 0x38 + X_ENC = 0x39 + ENC_CONST = 0x3A + ENC_STATUS = 0x3B + ENC_LATCH = 0x3C + ADC_VSUPPLY_AIN = 0x50 + ADC_TEMP = 0x51 + OTW_OV_VTH = 0x52 + MSLUT_0 = 0x60 + MSLUT_1 = 0x61 + MSLUT_2 = 0x62 + MSLUT_3 = 0x63 + MSLUT_4 = 0x64 + MSLUT_5 = 0x65 + MSLUT_6 = 0x66 + MSLUT_7 = 0x67 + MSLUTSEL = 0x68 + MSLUTSTART = 0x69 + MSCNT = 0x6A + MSCURACT = 0x6B + CHOPCONF = 0x6C + COOLCONF = 0x6D + DCCTRL = 0x6E + DRV_STATUS = 0x6F + PWMCONF = 0x70 + PWM_SCALE = 0x71 + PWM_AUTO = 0x72 + SG4_THRS = 0x74 + SG4_RESULT = 0x75 + SG4_IND = 0x76 + + class FIELD: + """ + Define all register bitfields of the TMC2240. + + Each field is defined as a tuple consisting of ( Address, Mask, Shift ). + + The name of the register is written as a comment behind each tuple. This is + intended for IDE users viewing the definition of a field by hovering over + it. This allows the user to see the corresponding register name of a field + without opening this file and searching for the definition. + """ + + FAST_STANDSTILL = ( 0x00, 0x00000002, 1 ) + EN_PWM_MODE = ( 0x00, 0x00000004, 2 ) + MULTISTEP_FILT = ( 0x00, 0x00000008, 3 ) + SHAFT = ( 0x00, 0x00000010, 4 ) + DIAG0_ERROR = ( 0x00, 0x00000020, 5 ) + DIAG0_OTPW = ( 0x00, 0x00000040, 6 ) + DIAG0_STALL = ( 0x00, 0x00000080, 7 ) + DIAG1_STALL = ( 0x00, 0x00000100, 8 ) + DIAG1_INDEX = ( 0x00, 0x00000200, 9 ) + DIAG1_ONSTATE = ( 0x00, 0x00000400, 10 ) + DIAG0_PUSHPULL = ( 0x00, 0x00001000, 12 ) + DIAG1_PUSHPULL = ( 0x00, 0x00002000, 13 ) + SMALL_HYSTERESIS = ( 0x00, 0x00004000, 14 ) + STOP_ENABLE = ( 0x00, 0x00008000, 15 ) + DIRECT_MODE = ( 0x00, 0x00010000, 16 ) + RESET = ( 0x01, 0x00000001, 0 ) + DRV_ERR = ( 0x01, 0x00000002, 1 ) + UV_CP = ( 0x01, 0x00000004, 2 ) + REGISTER_RESET = ( 0x01, 0x00000008, 3 ) + VM_UVLO = ( 0x01, 0x00000010, 4 ) + IFCNT = ( 0x02, 0x000000FF, 0 ) + SLAVEADDR = ( 0x03, 0x000000FF, 0 ) + SENDDELAY = ( 0x03, 0x00000F00, 8 ) + REFL_STEP = ( 0x04, 0x00000001, 0 ) + REFR_DIR = ( 0x04, 0x00000002, 1 ) + ENCB_CFG4 = ( 0x04, 0x00000004, 2 ) + ENCA_CFG5 = ( 0x04, 0x00000008, 3 ) + DRV_ENN = ( 0x04, 0x00000010, 4 ) + ENCN_CFG6 = ( 0x04, 0x00000020, 5 ) + UART_EN = ( 0x04, 0x00000040, 6 ) + RESERVED = ( 0x04, 0x00000080, 7 ) + COMP_A = ( 0x04, 0x00000100, 8 ) + COMP_B = ( 0x04, 0x00000200, 9 ) + COMP_A1_A2 = ( 0x04, 0x00000400, 10 ) + COMP_B1_B2 = ( 0x04, 0x00000800, 11 ) + OUTPUT = ( 0x04, 0x00001000, 12 ) + EXT_RES_DET = ( 0x04, 0x00002000, 13 ) + EXT_CLK = ( 0x04, 0x00004000, 14 ) + ADC_ERR = ( 0x04, 0x00008000, 15 ) + SILICON_RV = ( 0x04, 0x00070000, 16 ) + VERSION = ( 0x04, 0xFF000000, 24 ) + CURRENT_RANGE = ( 0x0A, 0x00000003, 0 ) + SLOPE_CONTROL = ( 0x0A, 0x00000030, 4 ) + GLOBALSCALER = ( 0x0B, 0x000000FF, 0 ) + IHOLD = ( 0x10, 0x0000001F, 0 ) + IRUN = ( 0x10, 0x00001F00, 8 ) + IHOLDDELAY = ( 0x10, 0x000F0000, 16 ) + IRUNDELAY = ( 0x10, 0x0F000000, 24 ) + TPOWERDOWN = ( 0x11, 0x000000FF, 0 ) + TSTEP = ( 0x12, 0x000FFFFF, 0 ) + TPWMTHRS = ( 0x13, 0x000FFFFF, 0 ) + TCOOLTHRS = ( 0x14, 0x000FFFFF, 0 ) + THIGH = ( 0x15, 0x000FFFFF, 0 ) + DIRECT_COIL_A = ( 0x2D, 0x000001FF, 0 ) + DIRECT_COIL_B = ( 0x2D, 0x01FF0000, 16 ) + POL_A = ( 0x38, 0x00000001, 0 ) + POL_B = ( 0x38, 0x00000002, 1 ) + POL_N = ( 0x38, 0x00000004, 2 ) + IGNORE_AB = ( 0x38, 0x00000008, 3 ) + CLR_CONT = ( 0x38, 0x00000010, 4 ) + CLR_ONCE = ( 0x38, 0x00000020, 5 ) + POS_NEG_EDGE = ( 0x38, 0x000000C0, 6 ) + CLR_ENC_X = ( 0x38, 0x00000100, 8 ) + LATCH_X_ACT = ( 0x38, 0x00000200, 9 ) + ENC_SEL_DECIMAL = ( 0x38, 0x00000400, 10 ) + X_ENC = ( 0x39, 0xFFFFFFFF, 0 ) + ENC_CONST = ( 0x3A, 0xFFFFFFFF, 0 ) + N_EVENT = ( 0x3B, 0x00000001, 0 ) + DEVIATION_WARN = ( 0x3B, 0x00000002, 1 ) + ENC_LATCH = ( 0x3C, 0xFFFFFFFF, 0 ) + ADC_VSUPPLY = ( 0x50, 0x00001FFF, 0 ) + ADC_AIN = ( 0x50, 0x1FFF0000, 16 ) + ADC_TEMP = ( 0x51, 0x00001FFF, 0 ) + #RESERVED = ( 0x51, 0x1FFF0000, 16 ) + OVERVOLTAGE_VTH = ( 0x52, 0x00001FFF, 0 ) + OVERTEMPPREWARNING_VTH = ( 0x52, 0x1FFF0000, 16 ) + MSLUT__ = ( 0x60, 0xFFFFFFFF, 0 ) + #MSLUT__ = ( 0x61, 0xFFFFFFFF, 0 ) + #MSLUT__ = ( 0x62, 0xFFFFFFFF, 0 ) + #MSLUT__ = ( 0x63, 0xFFFFFFFF, 0 ) + #MSLUT__ = ( 0x64, 0xFFFFFFFF, 0 ) + #MSLUT__ = ( 0x65, 0xFFFFFFFF, 0 ) + #MSLUT__ = ( 0x66, 0xFFFFFFFF, 0 ) + #MSLUT__ = ( 0x67, 0xFFFFFFFF, 0 ) + W0 = ( 0x68, 0x00000003, 0 ) + W1 = ( 0x68, 0x0000000C, 2 ) + W2 = ( 0x68, 0x00000030, 4 ) + W3 = ( 0x68, 0x000000C0, 6 ) + X1 = ( 0x68, 0x0000FF00, 8 ) + X2 = ( 0x68, 0x00FF0000, 16 ) + X3 = ( 0x68, 0xFF000000, 24 ) + START_SIN = ( 0x69, 0x000000FF, 0 ) + START_SIN90 = ( 0x69, 0x00FF0000, 16 ) + OFFSET_SIN90 = ( 0x69, 0xFF000000, 24 ) + MSCNT = ( 0x6A, 0x000003FF, 0 ) + CUR_B = ( 0x6B, 0x000001FF, 0 ) + CUR_A = ( 0x6B, 0x01FF0000, 16 ) + TOFF = ( 0x6C, 0x0000000F, 0 ) + HSTRT_TFD210 = ( 0x6C, 0x00000070, 4 ) + HEND_OFFSET = ( 0x6C, 0x00000780, 7 ) + FD3 = ( 0x6C, 0x00000800, 11 ) + DISFDCC = ( 0x6C, 0x00001000, 12 ) + CHM = ( 0x6C, 0x00004000, 14 ) + TBL = ( 0x6C, 0x00018000, 15 ) + VHIGHFS = ( 0x6C, 0x00040000, 18 ) + VHIGHCHM = ( 0x6C, 0x00080000, 19 ) + TPFD = ( 0x6C, 0x00F00000, 20 ) + MRES = ( 0x6C, 0x0F000000, 24 ) + INTPOL = ( 0x6C, 0x10000000, 28 ) + DEDGE = ( 0x6C, 0x20000000, 29 ) + DISS2G = ( 0x6C, 0x40000000, 30 ) + DISS2VS = ( 0x6C, 0x80000000, 31 ) + SEMIN = ( 0x6D, 0x0000000F, 0 ) + SEUP = ( 0x6D, 0x00000060, 5 ) + SEMAX = ( 0x6D, 0x00000F00, 8 ) + SEDN = ( 0x6D, 0x00006000, 13 ) + SEIMIN = ( 0x6D, 0x00008000, 15 ) + SGT = ( 0x6D, 0x007F0000, 16 ) + SFILT = ( 0x6D, 0x01000000, 24 ) + DC_TIME = ( 0x6E, 0x000003FF, 0 ) + DC_SG = ( 0x6E, 0x00FF0000, 16 ) + SG_RESULT = ( 0x6F, 0x000003FF, 0 ) + S2VSA = ( 0x6F, 0x00001000, 12 ) + S2VSB = ( 0x6F, 0x00002000, 13 ) + STEALTH = ( 0x6F, 0x00004000, 14 ) + FSACTIVE = ( 0x6F, 0x00008000, 15 ) + CS_ACTUAL = ( 0x6F, 0x001F0000, 16 ) + STALLGUARD = ( 0x6F, 0x01000000, 24 ) + OT = ( 0x6F, 0x02000000, 25 ) + OTPW = ( 0x6F, 0x04000000, 26 ) + S2GA = ( 0x6F, 0x08000000, 27 ) + S2GB = ( 0x6F, 0x10000000, 28 ) + OLA = ( 0x6F, 0x20000000, 29 ) + OLB = ( 0x6F, 0x40000000, 30 ) + STST = ( 0x6F, 0x80000000, 31 ) + PWM_OFS = ( 0x70, 0x000000FF, 0 ) + PWM_GRAD = ( 0x70, 0x0000FF00, 8 ) + PWM_FREQ = ( 0x70, 0x00030000, 16 ) + PWM_AUTOSCALE = ( 0x70, 0x00040000, 18 ) + PWM_AUTOGRAD = ( 0x70, 0x00080000, 19 ) + FREEWHEEL = ( 0x70, 0x00300000, 20 ) + PWM_MEAS_SD_ENABLE = ( 0x70, 0x00400000, 22 ) + PWM_DIS_REG_STST = ( 0x70, 0x00800000, 23 ) + PWM_REG = ( 0x70, 0x0F000000, 24 ) + PWM_LIM = ( 0x70, 0xF0000000, 28 ) + PWM_SCALE_SUM = ( 0x71, 0x000003FF, 0 ) + PWM_SCALE_AUTO = ( 0x71, 0x01FF0000, 16 ) + PWM_OFS_AUTO = ( 0x72, 0x000000FF, 0 ) + PWM_GRAD_AUTO = ( 0x72, 0x00FF0000, 16 ) + SG4_THRS = ( 0x74, 0x000000FF, 0 ) + SG4_FILT_EN = ( 0x74, 0x00000100, 8 ) + SG_ANGLE_OFFSET = ( 0x74, 0x00000200, 9 ) + SG4_RESULT = ( 0x75, 0x000003FF, 0 ) + SG4_IND_0 = ( 0x76, 0x000000FF, 0 ) + SG4_IND_1 = ( 0x76, 0x0000FF00, 8 ) + SG4_IND_2 = ( 0x76, 0x00FF0000, 16 ) + SG4_IND_3 = ( 0x76, 0xFF000000, 24 ) \ No newline at end of file diff --git a/pytrinamic/ic/__init__.py b/pytrinamic/ic/__init__.py index d7e8a836..1a84dec6 100644 --- a/pytrinamic/ic/__init__.py +++ b/pytrinamic/ic/__init__.py @@ -6,6 +6,7 @@ from .TMC2209 import TMC2209 from .TMC2224 import TMC2224 from .TMC2225 import TMC2225 +from .TMC2240 import TMC2240 from .TMC2300 import TMC2300 from .TMC2590 import TMC2590 from .TMC2660 import TMC2660 From fc433836c1fee64b8c09f059228f56ca146c57e9 Mon Sep 17 00:00:00 2001 From: bp Date: Thu, 21 Jul 2022 13:15:02 +0200 Subject: [PATCH 08/51] README: fix issues with links for PyPI --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 240dbcd5..287cbf9c 100644 --- a/README.md +++ b/README.md @@ -19,9 +19,9 @@ pip install pytrinamic Please have a look at the [code examples on GitHub](https://github.com/trinamic/PyTrinamic/tree/feature_feature_hierarchy_v2/examples). -## Migration Guide +## Migration Guide -Version 0.2.0 of PyTrinamic introduces several changes to the API. For those who want to convert code that uses an older version of PyTrinamic, we wrote a short [migration guide](docs/migration_guide.md). +Version 0.2.0 of PyTrinamic introduces several changes to the API. For those who want to convert code that uses an older version of PyTrinamic, we wrote a short [migration guide](https://github.com/trinamic/PyTrinamic/blob/master/docs/migration_guide.md). All previous versions of PyTrinamic will still be available on PyPI and can be installed via: `pip install pytrinamic==0.1.27`. From d0caa68732196d77dfdbe2794f5c934c33e81a80 Mon Sep 17 00:00:00 2001 From: bp Date: Thu, 21 Jul 2022 13:22:37 +0200 Subject: [PATCH 09/51] Bump: the version for a new release --- pytrinamic/version.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pytrinamic/version.py b/pytrinamic/version.py index 744fa00f..2995a62e 100644 --- a/pytrinamic/version.py +++ b/pytrinamic/version.py @@ -1,2 +1,2 @@ -__version__ = "0.2.1-rc.1" +__version__ = "0.2.1" From 27c0ac5baa21720dcced6ec3e1b89023e926adad Mon Sep 17 00:00:00 2001 From: zomberg Date: Wed, 27 Jul 2022 13:42:19 +0200 Subject: [PATCH 10/51] Relative imports instead of absolute imports - Add module TMCM1141 to modules __init.py__ Signed-off-by: zomberg --- pytrinamic/connections/connection_manager.py | 18 +++++++++--------- pytrinamic/connections/dummy_tmcl_interface.py | 2 +- pytrinamic/connections/ixxat_tmcl_interface.py | 2 +- .../connections/kvaser_tmcl_interface.py | 2 +- pytrinamic/connections/pcan_tmcl_interface.py | 2 +- .../connections/serial_tmcl_interface.py | 4 ++-- pytrinamic/connections/slcan_tmcl_interface.py | 2 +- .../connections/socketcan_tmcl_interface.py | 2 +- pytrinamic/connections/tmcl_interface.py | 4 ++-- pytrinamic/connections/usb_tmcl_interface.py | 2 +- pytrinamic/features/abn_encoder_module.py | 2 +- pytrinamic/features/absolute_encoder_module.py | 2 +- pytrinamic/features/coolstep_module.py | 2 +- pytrinamic/features/digital_hall_module.py | 2 +- pytrinamic/features/drive_setting_module.py | 2 +- pytrinamic/features/linear_ramp_module.py | 2 +- pytrinamic/features/motor_control_ic.py | 4 ++-- pytrinamic/features/motor_control_module.py | 2 +- pytrinamic/features/pid_module.py | 2 +- pytrinamic/features/stallguard2_module.py | 2 +- pytrinamic/helpers.py | 2 +- pytrinamic/ic/MAX22216.py | 3 ++- pytrinamic/ic/TMC2100.py | 2 +- pytrinamic/ic/TMC2130.py | 2 +- pytrinamic/ic/TMC2160.py | 2 +- pytrinamic/ic/TMC2208.py | 2 +- pytrinamic/ic/TMC2209.py | 2 +- pytrinamic/ic/TMC2224.py | 2 +- pytrinamic/ic/TMC2225.py | 2 +- pytrinamic/ic/TMC2300.py | 2 +- pytrinamic/ic/TMC2590.py | 2 +- pytrinamic/ic/TMC2660.py | 2 +- pytrinamic/ic/TMC4361.py | 2 +- pytrinamic/ic/TMC4671.py | 4 ++-- pytrinamic/ic/TMC5031.py | 2 +- pytrinamic/ic/TMC5041.py | 2 +- pytrinamic/ic/TMC5062.py | 2 +- pytrinamic/ic/TMC5072.py | 4 ++-- pytrinamic/ic/TMC5130.py | 4 ++-- pytrinamic/ic/TMC5160.py | 2 +- pytrinamic/ic/TMC6100.py | 2 +- pytrinamic/ic/TMC6200.py | 2 +- pytrinamic/ic/TMC6300.py | 2 +- pytrinamic/ic/TMC7300.py | 4 ++-- pytrinamic/modules/TMCC160.py | 6 +++--- pytrinamic/modules/TMCM1140.py | 6 +++--- pytrinamic/modules/TMCM1141.py | 6 +++--- pytrinamic/modules/TMCM1160.py | 6 +++--- pytrinamic/modules/TMCM1161.py | 6 +++--- pytrinamic/modules/TMCM1240.py | 6 +++--- pytrinamic/modules/TMCM1260.py | 6 +++--- pytrinamic/modules/TMCM1270.py | 6 +++--- pytrinamic/modules/TMCM1276.py | 6 +++--- pytrinamic/modules/TMCM1370.py | 6 +++--- pytrinamic/modules/TMCM1617.py | 10 +++++----- pytrinamic/modules/TMCM1630.py | 6 +++--- pytrinamic/modules/TMCM1633.py | 6 +++--- pytrinamic/modules/TMCM1636.py | 6 +++--- pytrinamic/modules/TMCM1637.py | 6 +++--- pytrinamic/modules/TMCM1638.py | 6 +++--- pytrinamic/modules/TMCM1640.py | 6 +++--- pytrinamic/modules/TMCM1670.py | 6 +++--- pytrinamic/modules/TMCM3110.py | 6 +++--- pytrinamic/modules/TMCM3312.py | 6 +++--- pytrinamic/modules/TMCM3351.py | 6 +++--- pytrinamic/modules/TMCM6110.py | 6 +++--- pytrinamic/modules/TMCM6212.py | 6 +++--- pytrinamic/modules/__init__.py | 1 + 68 files changed, 132 insertions(+), 130 deletions(-) diff --git a/pytrinamic/connections/connection_manager.py b/pytrinamic/connections/connection_manager.py index d778c0d3..59b079b4 100644 --- a/pytrinamic/connections/connection_manager.py +++ b/pytrinamic/connections/connection_manager.py @@ -1,15 +1,15 @@ import sys import argparse -from pytrinamic.connections import DummyTmclInterface -from pytrinamic.connections import PcanTmclInterface -from pytrinamic.connections import SocketcanTmclInterface -from pytrinamic.connections import KvaserTmclInterface -from pytrinamic.connections import SerialTmclInterface -from pytrinamic.connections import UartIcInterface -from pytrinamic.connections import UsbTmclInterface -from pytrinamic.connections import SlcanTmclInterface -from pytrinamic.connections import IxxatTmclInterface +from ..connections import DummyTmclInterface +from ..connections import PcanTmclInterface +from ..connections import SocketcanTmclInterface +from ..connections import KvaserTmclInterface +from ..connections import SerialTmclInterface +from ..connections import UartIcInterface +from ..connections import UsbTmclInterface +from ..connections import SlcanTmclInterface +from ..connections import IxxatTmclInterface class ConnectionManager: diff --git a/pytrinamic/connections/dummy_tmcl_interface.py b/pytrinamic/connections/dummy_tmcl_interface.py index 02948ce9..e8736a63 100644 --- a/pytrinamic/connections/dummy_tmcl_interface.py +++ b/pytrinamic/connections/dummy_tmcl_interface.py @@ -1,4 +1,4 @@ -from pytrinamic.connections.tmcl_interface import TmclInterface +from ..connections.tmcl_interface import TmclInterface class DummyTmclInterface(TmclInterface): diff --git a/pytrinamic/connections/ixxat_tmcl_interface.py b/pytrinamic/connections/ixxat_tmcl_interface.py index e178d1ea..534145a5 100644 --- a/pytrinamic/connections/ixxat_tmcl_interface.py +++ b/pytrinamic/connections/ixxat_tmcl_interface.py @@ -1,6 +1,6 @@ import can from can import CanError -from pytrinamic.connections.tmcl_interface import TmclInterface +from ..connections.tmcl_interface import TmclInterface # Providing 5 channels here, this should cover all use cases. # Ixxat USB-to-CAN provides 1, 2 or 4 channels. diff --git a/pytrinamic/connections/kvaser_tmcl_interface.py b/pytrinamic/connections/kvaser_tmcl_interface.py index 40c2e5a8..28b8ab93 100644 --- a/pytrinamic/connections/kvaser_tmcl_interface.py +++ b/pytrinamic/connections/kvaser_tmcl_interface.py @@ -1,6 +1,6 @@ import can from can import CanError -from pytrinamic.connections.tmcl_interface import TmclInterface +from ..connections.tmcl_interface import TmclInterface _CHANNELS = ["0", "1", "2"] diff --git a/pytrinamic/connections/pcan_tmcl_interface.py b/pytrinamic/connections/pcan_tmcl_interface.py index 3b711a1c..d62676e7 100644 --- a/pytrinamic/connections/pcan_tmcl_interface.py +++ b/pytrinamic/connections/pcan_tmcl_interface.py @@ -2,7 +2,7 @@ import can from can import CanError from can.interfaces.pcan.pcan import PcanError -from pytrinamic.connections.tmcl_interface import TmclInterface +from ..connections.tmcl_interface import TmclInterface _CHANNELS = [ "PCAN_USBBUS1", "PCAN_USBBUS2", "PCAN_USBBUS3", "PCAN_USBBUS4", diff --git a/pytrinamic/connections/serial_tmcl_interface.py b/pytrinamic/connections/serial_tmcl_interface.py index 41960b19..c31e6ae3 100644 --- a/pytrinamic/connections/serial_tmcl_interface.py +++ b/pytrinamic/connections/serial_tmcl_interface.py @@ -1,7 +1,7 @@ from serial import Serial, SerialException import serial.tools.list_ports -from pytrinamic.connections.tmcl_interface import TmclInterface -from pytrinamic.tmcl import TMCLReplyChecksumError +from ..connections.tmcl_interface import TmclInterface +from ..tmcl import TMCLReplyChecksumError class SerialTmclInterface(TmclInterface): diff --git a/pytrinamic/connections/slcan_tmcl_interface.py b/pytrinamic/connections/slcan_tmcl_interface.py index f11231bb..e3751b91 100644 --- a/pytrinamic/connections/slcan_tmcl_interface.py +++ b/pytrinamic/connections/slcan_tmcl_interface.py @@ -1,7 +1,7 @@ import can from can import CanError from serial.tools.list_ports import comports -from pytrinamic.connections.tmcl_interface import TmclInterface +from ..connections.tmcl_interface import TmclInterface class SlcanTmclInterface(TmclInterface): diff --git a/pytrinamic/connections/socketcan_tmcl_interface.py b/pytrinamic/connections/socketcan_tmcl_interface.py index 1a3d57fa..9c06b3b6 100644 --- a/pytrinamic/connections/socketcan_tmcl_interface.py +++ b/pytrinamic/connections/socketcan_tmcl_interface.py @@ -1,6 +1,6 @@ import can from can import CanError -from pytrinamic.connections.tmcl_interface import TmclInterface +from ..connections.tmcl_interface import TmclInterface _CHANNELS = ["can0", "can1", "can2", "can3", "can4", "can5", "can6", "can7"] diff --git a/pytrinamic/connections/tmcl_interface.py b/pytrinamic/connections/tmcl_interface.py index 26bf8f5b..dae5c686 100644 --- a/pytrinamic/connections/tmcl_interface.py +++ b/pytrinamic/connections/tmcl_interface.py @@ -1,6 +1,6 @@ from abc import ABC -from pytrinamic.tmcl import TMCL, TMCLRequest, TMCLCommand, TMCLReply, TMCLReplyChecksumError, TMCLReplyStatusError -from pytrinamic.helpers import TMC_helpers +from ..tmcl import TMCL, TMCLRequest, TMCLCommand, TMCLReply, TMCLReplyChecksumError, TMCLReplyStatusError +from ..helpers import TMC_helpers class TmclInterface(ABC): diff --git a/pytrinamic/connections/usb_tmcl_interface.py b/pytrinamic/connections/usb_tmcl_interface.py index b1a5babb..0bc8621e 100644 --- a/pytrinamic/connections/usb_tmcl_interface.py +++ b/pytrinamic/connections/usb_tmcl_interface.py @@ -1,5 +1,5 @@ import serial.tools.list_ports -from pytrinamic.connections.serial_tmcl_interface import SerialTmclInterface +from ..connections.serial_tmcl_interface import SerialTmclInterface class UsbTmclInterface(SerialTmclInterface): diff --git a/pytrinamic/features/abn_encoder_module.py b/pytrinamic/features/abn_encoder_module.py index 73287dd1..718d0555 100644 --- a/pytrinamic/features/abn_encoder_module.py +++ b/pytrinamic/features/abn_encoder_module.py @@ -1,4 +1,4 @@ -from pytrinamic.features.abn_encoder import ABNEncoder +from ..features.abn_encoder import ABNEncoder class ABNEncoderModule(ABNEncoder): diff --git a/pytrinamic/features/absolute_encoder_module.py b/pytrinamic/features/absolute_encoder_module.py index ac669273..2b0a5c55 100644 --- a/pytrinamic/features/absolute_encoder_module.py +++ b/pytrinamic/features/absolute_encoder_module.py @@ -1,4 +1,4 @@ -from pytrinamic.features.absolute_encoder import AbsoluteEncoder +from ..features.absolute_encoder import AbsoluteEncoder class AbsoluteEncoderModule(AbsoluteEncoder): diff --git a/pytrinamic/features/coolstep_module.py b/pytrinamic/features/coolstep_module.py index 3d127dae..bce5aec9 100644 --- a/pytrinamic/features/coolstep_module.py +++ b/pytrinamic/features/coolstep_module.py @@ -1,4 +1,4 @@ -from pytrinamic.features.coolstep import CoolStep +from ..features.coolstep import CoolStep import time diff --git a/pytrinamic/features/digital_hall_module.py b/pytrinamic/features/digital_hall_module.py index f3d40dd9..007221c5 100644 --- a/pytrinamic/features/digital_hall_module.py +++ b/pytrinamic/features/digital_hall_module.py @@ -1,4 +1,4 @@ -from pytrinamic.features.digital_hall import DigitalHall +from ..features.digital_hall import DigitalHall class DigitalHallModule(DigitalHall): diff --git a/pytrinamic/features/drive_setting_module.py b/pytrinamic/features/drive_setting_module.py index 1ff42442..eb1bc3ca 100644 --- a/pytrinamic/features/drive_setting_module.py +++ b/pytrinamic/features/drive_setting_module.py @@ -1,4 +1,4 @@ -from pytrinamic.features.drive_setting import DriveSetting +from ..features.drive_setting import DriveSetting class DriveSettingModule(DriveSetting): diff --git a/pytrinamic/features/linear_ramp_module.py b/pytrinamic/features/linear_ramp_module.py index 831a2c5f..ff2b7f70 100644 --- a/pytrinamic/features/linear_ramp_module.py +++ b/pytrinamic/features/linear_ramp_module.py @@ -1,4 +1,4 @@ -from pytrinamic.features.linear_ramp import LinearRamp +from ..features.linear_ramp import LinearRamp class LinearRampModule(LinearRamp): diff --git a/pytrinamic/features/motor_control_ic.py b/pytrinamic/features/motor_control_ic.py index 46e45c49..6287e263 100644 --- a/pytrinamic/features/motor_control_ic.py +++ b/pytrinamic/features/motor_control_ic.py @@ -1,5 +1,5 @@ -from pytrinamic.features.motor_control import MotorControl -from pytrinamic.helpers import TMC_helpers +from ..features.motor_control import MotorControl +from ..helpers import TMC_helpers class MotorControlIc(MotorControl): diff --git a/pytrinamic/features/motor_control_module.py b/pytrinamic/features/motor_control_module.py index 70a3f718..7852d096 100644 --- a/pytrinamic/features/motor_control_module.py +++ b/pytrinamic/features/motor_control_module.py @@ -1,4 +1,4 @@ -from pytrinamic.features.motor_control import MotorControl +from ..features.motor_control import MotorControl class MotorControlModule(MotorControl): diff --git a/pytrinamic/features/pid_module.py b/pytrinamic/features/pid_module.py index d24232cf..ef066a36 100644 --- a/pytrinamic/features/pid_module.py +++ b/pytrinamic/features/pid_module.py @@ -1,4 +1,4 @@ -from pytrinamic.features.pid import PID +from ..features.pid import PID class PIDModule(PID): diff --git a/pytrinamic/features/stallguard2_module.py b/pytrinamic/features/stallguard2_module.py index 6cce0001..c8712b14 100644 --- a/pytrinamic/features/stallguard2_module.py +++ b/pytrinamic/features/stallguard2_module.py @@ -1,4 +1,4 @@ -from pytrinamic.features.stallguard2 import StallGuard2 +from ..features.stallguard2 import StallGuard2 import time diff --git a/pytrinamic/helpers.py b/pytrinamic/helpers.py index 3ff86661..c61e95e6 100644 --- a/pytrinamic/helpers.py +++ b/pytrinamic/helpers.py @@ -1,4 +1,4 @@ -from pytrinamic import name, desc +from . import name, desc class TMC_helpers(object): diff --git a/pytrinamic/ic/MAX22216.py b/pytrinamic/ic/MAX22216.py index 1020b542..d7d0be6a 100644 --- a/pytrinamic/ic/MAX22216.py +++ b/pytrinamic/ic/MAX22216.py @@ -1,4 +1,5 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc + class MAX22216(TMCIc): """ diff --git a/pytrinamic/ic/TMC2100.py b/pytrinamic/ic/TMC2100.py index 485057cb..c7c34c15 100644 --- a/pytrinamic/ic/TMC2100.py +++ b/pytrinamic/ic/TMC2100.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC2100(TMCIc): diff --git a/pytrinamic/ic/TMC2130.py b/pytrinamic/ic/TMC2130.py index bcec09ae..d854cfe7 100644 --- a/pytrinamic/ic/TMC2130.py +++ b/pytrinamic/ic/TMC2130.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC2130(TMCIc): diff --git a/pytrinamic/ic/TMC2160.py b/pytrinamic/ic/TMC2160.py index 0ab43174..cf0e2b3a 100644 --- a/pytrinamic/ic/TMC2160.py +++ b/pytrinamic/ic/TMC2160.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC2160(TMCIc): diff --git a/pytrinamic/ic/TMC2208.py b/pytrinamic/ic/TMC2208.py index b5a17077..7c39d675 100644 --- a/pytrinamic/ic/TMC2208.py +++ b/pytrinamic/ic/TMC2208.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC2208(TMCIc): diff --git a/pytrinamic/ic/TMC2209.py b/pytrinamic/ic/TMC2209.py index 78dceb70..caf7a603 100644 --- a/pytrinamic/ic/TMC2209.py +++ b/pytrinamic/ic/TMC2209.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC2209(TMCIc): diff --git a/pytrinamic/ic/TMC2224.py b/pytrinamic/ic/TMC2224.py index 451f0a02..cd39241d 100644 --- a/pytrinamic/ic/TMC2224.py +++ b/pytrinamic/ic/TMC2224.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC2224(TMCIc): diff --git a/pytrinamic/ic/TMC2225.py b/pytrinamic/ic/TMC2225.py index f8f51a31..15471b55 100644 --- a/pytrinamic/ic/TMC2225.py +++ b/pytrinamic/ic/TMC2225.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC2225(TMCIc): diff --git a/pytrinamic/ic/TMC2300.py b/pytrinamic/ic/TMC2300.py index a6326e45..cfcee69d 100644 --- a/pytrinamic/ic/TMC2300.py +++ b/pytrinamic/ic/TMC2300.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC2300(TMCIc): diff --git a/pytrinamic/ic/TMC2590.py b/pytrinamic/ic/TMC2590.py index 96c88430..f899b90b 100644 --- a/pytrinamic/ic/TMC2590.py +++ b/pytrinamic/ic/TMC2590.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC2590(TMCIc): diff --git a/pytrinamic/ic/TMC2660.py b/pytrinamic/ic/TMC2660.py index 5c108dff..1393bd05 100644 --- a/pytrinamic/ic/TMC2660.py +++ b/pytrinamic/ic/TMC2660.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC2660(TMCIc): diff --git a/pytrinamic/ic/TMC4361.py b/pytrinamic/ic/TMC4361.py index 262277b2..fe14946c 100644 --- a/pytrinamic/ic/TMC4361.py +++ b/pytrinamic/ic/TMC4361.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC4361(TMCIc): diff --git a/pytrinamic/ic/TMC4671.py b/pytrinamic/ic/TMC4671.py index 8adf5fbf..ac9adb4a 100644 --- a/pytrinamic/ic/TMC4671.py +++ b/pytrinamic/ic/TMC4671.py @@ -1,6 +1,6 @@ import struct -from pytrinamic.ic.tmc_ic import TMCIc -from pytrinamic.helpers import TMC_helpers +from ..ic.tmc_ic import TMCIc +from ..helpers import TMC_helpers DATAGRAM_FORMAT = ">BI" DATAGRAM_LENGTH = 5 diff --git a/pytrinamic/ic/TMC5031.py b/pytrinamic/ic/TMC5031.py index 81958dcd..370bfadd 100644 --- a/pytrinamic/ic/TMC5031.py +++ b/pytrinamic/ic/TMC5031.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC5031(TMCIc): diff --git a/pytrinamic/ic/TMC5041.py b/pytrinamic/ic/TMC5041.py index d47128cf..33624a80 100644 --- a/pytrinamic/ic/TMC5041.py +++ b/pytrinamic/ic/TMC5041.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC5041(TMCIc): diff --git a/pytrinamic/ic/TMC5062.py b/pytrinamic/ic/TMC5062.py index 7d5c5015..6ff6e6c8 100644 --- a/pytrinamic/ic/TMC5062.py +++ b/pytrinamic/ic/TMC5062.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC5062(TMCIc): diff --git a/pytrinamic/ic/TMC5072.py b/pytrinamic/ic/TMC5072.py index 46ac4625..6569bd57 100644 --- a/pytrinamic/ic/TMC5072.py +++ b/pytrinamic/ic/TMC5072.py @@ -1,7 +1,7 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc # features -from pytrinamic.features.motor_control_ic import MotorControlIc +from ..features.motor_control_ic import MotorControlIc # from pytrinamic.features.LinearRampIC import LinearRampIC # from pytrinamic.features.CurrentIC import CurrentIC # from pytrinamic.features.StallGuard2IC import StallGuard2IC diff --git a/pytrinamic/ic/TMC5130.py b/pytrinamic/ic/TMC5130.py index 245e97cd..b7858c3f 100644 --- a/pytrinamic/ic/TMC5130.py +++ b/pytrinamic/ic/TMC5130.py @@ -1,7 +1,7 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc # features -from pytrinamic.features.motor_control_ic import MotorControlIc +from ..features.motor_control_ic import MotorControlIc # from pytrinamic.features.linear_ramp_ic import LinearRampIC # from pytrinamic.features.current_ic import CurrentIC # from pytrinamic.features.stallguard2_ic import StallGuard2IC diff --git a/pytrinamic/ic/TMC5160.py b/pytrinamic/ic/TMC5160.py index 7803d687..0ddffee0 100644 --- a/pytrinamic/ic/TMC5160.py +++ b/pytrinamic/ic/TMC5160.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC5160(TMCIc): diff --git a/pytrinamic/ic/TMC6100.py b/pytrinamic/ic/TMC6100.py index c3d85abb..06a4d9af 100644 --- a/pytrinamic/ic/TMC6100.py +++ b/pytrinamic/ic/TMC6100.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC6100(TMCIc): diff --git a/pytrinamic/ic/TMC6200.py b/pytrinamic/ic/TMC6200.py index ec3e9dd2..7a814c61 100644 --- a/pytrinamic/ic/TMC6200.py +++ b/pytrinamic/ic/TMC6200.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC6200(TMCIc): diff --git a/pytrinamic/ic/TMC6300.py b/pytrinamic/ic/TMC6300.py index 0a2ddc9e..aed57182 100644 --- a/pytrinamic/ic/TMC6300.py +++ b/pytrinamic/ic/TMC6300.py @@ -1,4 +1,4 @@ -from pytrinamic.ic.tmc_ic import TMCIc +from ..ic.tmc_ic import TMCIc class TMC6300(TMCIc): diff --git a/pytrinamic/ic/TMC7300.py b/pytrinamic/ic/TMC7300.py index 804f76cd..6c67d97f 100644 --- a/pytrinamic/ic/TMC7300.py +++ b/pytrinamic/ic/TMC7300.py @@ -1,6 +1,6 @@ import struct -from pytrinamic.ic.tmc_ic import TMCIc -from pytrinamic.helpers import TMC_helpers +from ..ic.tmc_ic import TMCIc +from ..helpers import TMC_helpers DATAGRAM_FORMAT = ">BI" DATAGRAM_LENGTH = 5 diff --git a/pytrinamic/modules/TMCC160.py b/pytrinamic/modules/TMCC160.py index 50df8d96..f38a7c4f 100644 --- a/pytrinamic/modules/TMCC160.py +++ b/pytrinamic/modules/TMCC160.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import ABNEncoderModule, DigitalHallModule, PIDModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import ABNEncoderModule, DigitalHallModule, PIDModule class TMCC160(TMCLModule): diff --git a/pytrinamic/modules/TMCM1140.py b/pytrinamic/modules/TMCM1140.py index 20bbce43..9c7c2775 100644 --- a/pytrinamic/modules/TMCM1140.py +++ b/pytrinamic/modules/TMCM1140.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM1140(TMCLModule): diff --git a/pytrinamic/modules/TMCM1141.py b/pytrinamic/modules/TMCM1141.py index 5202a037..f2af6c50 100644 --- a/pytrinamic/modules/TMCM1141.py +++ b/pytrinamic/modules/TMCM1141.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM1141(TMCLModule): diff --git a/pytrinamic/modules/TMCM1160.py b/pytrinamic/modules/TMCM1160.py index 96c67261..c75b5b2d 100644 --- a/pytrinamic/modules/TMCM1160.py +++ b/pytrinamic/modules/TMCM1160.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM1160(TMCLModule): diff --git a/pytrinamic/modules/TMCM1161.py b/pytrinamic/modules/TMCM1161.py index 8eaa81de..48a7f0b3 100644 --- a/pytrinamic/modules/TMCM1161.py +++ b/pytrinamic/modules/TMCM1161.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM1161(TMCLModule): diff --git a/pytrinamic/modules/TMCM1240.py b/pytrinamic/modules/TMCM1240.py index 70686c6f..dd73b95a 100644 --- a/pytrinamic/modules/TMCM1240.py +++ b/pytrinamic/modules/TMCM1240.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM1240(TMCLModule): diff --git a/pytrinamic/modules/TMCM1260.py b/pytrinamic/modules/TMCM1260.py index 9eda3994..c7710cb3 100644 --- a/pytrinamic/modules/TMCM1260.py +++ b/pytrinamic/modules/TMCM1260.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM1260(TMCLModule): diff --git a/pytrinamic/modules/TMCM1270.py b/pytrinamic/modules/TMCM1270.py index edae6e73..6dc9a91f 100644 --- a/pytrinamic/modules/TMCM1270.py +++ b/pytrinamic/modules/TMCM1270.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM1270(TMCLModule): diff --git a/pytrinamic/modules/TMCM1276.py b/pytrinamic/modules/TMCM1276.py index 0d4f6b5c..6fd29a95 100644 --- a/pytrinamic/modules/TMCM1276.py +++ b/pytrinamic/modules/TMCM1276.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM1276(TMCLModule): diff --git a/pytrinamic/modules/TMCM1370.py b/pytrinamic/modules/TMCM1370.py index 8d9baa60..d3254d21 100644 --- a/pytrinamic/modules/TMCM1370.py +++ b/pytrinamic/modules/TMCM1370.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM1370(TMCLModule): diff --git a/pytrinamic/modules/TMCM1617.py b/pytrinamic/modules/TMCM1617.py index 7f707c34..d75305c1 100644 --- a/pytrinamic/modules/TMCM1617.py +++ b/pytrinamic/modules/TMCM1617.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule -from pytrinamic.ic import TMC4671, TMC6200 -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import ABNEncoderModule, DigitalHallModule, PIDModule -from pytrinamic.helpers import TMC_helpers +from ..modules import TMCLModule +from ..ic import TMC4671, TMC6200 +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import ABNEncoderModule, DigitalHallModule, PIDModule +from ..helpers import TMC_helpers class TMCM1617(TMCLModule): diff --git a/pytrinamic/modules/TMCM1630.py b/pytrinamic/modules/TMCM1630.py index 51089961..11949493 100644 --- a/pytrinamic/modules/TMCM1630.py +++ b/pytrinamic/modules/TMCM1630.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import ABNEncoderModule, DigitalHallModule, PIDModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import ABNEncoderModule, DigitalHallModule, PIDModule class TMCM1630(TMCLModule): diff --git a/pytrinamic/modules/TMCM1633.py b/pytrinamic/modules/TMCM1633.py index 967417d0..e152ac4f 100644 --- a/pytrinamic/modules/TMCM1633.py +++ b/pytrinamic/modules/TMCM1633.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import ABNEncoderModule, DigitalHallModule, PIDModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import ABNEncoderModule, DigitalHallModule, PIDModule class TMCM1633(TMCLModule): diff --git a/pytrinamic/modules/TMCM1636.py b/pytrinamic/modules/TMCM1636.py index 9b990f1c..fec8c47a 100644 --- a/pytrinamic/modules/TMCM1636.py +++ b/pytrinamic/modules/TMCM1636.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule, ABNEncoderModule -from pytrinamic.features import DigitalHallModule, PIDModule, AbsoluteEncoderModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule, ABNEncoderModule +from ..features import DigitalHallModule, PIDModule, AbsoluteEncoderModule class TMCM1636(TMCLModule): diff --git a/pytrinamic/modules/TMCM1637.py b/pytrinamic/modules/TMCM1637.py index 7d707313..47371ac2 100644 --- a/pytrinamic/modules/TMCM1637.py +++ b/pytrinamic/modules/TMCM1637.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule, ABNEncoderModule -from pytrinamic.features import DigitalHallModule, PIDModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule, ABNEncoderModule +from ..features import DigitalHallModule, PIDModule class TMCM1637(TMCLModule): diff --git a/pytrinamic/modules/TMCM1638.py b/pytrinamic/modules/TMCM1638.py index 0858d5d8..9f0ca69e 100644 --- a/pytrinamic/modules/TMCM1638.py +++ b/pytrinamic/modules/TMCM1638.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule, ABNEncoderModule -from pytrinamic.features import DigitalHallModule, PIDModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule, ABNEncoderModule +from ..features import DigitalHallModule, PIDModule class TMCM1638(TMCLModule): diff --git a/pytrinamic/modules/TMCM1640.py b/pytrinamic/modules/TMCM1640.py index c3da0191..41e6a187 100644 --- a/pytrinamic/modules/TMCM1640.py +++ b/pytrinamic/modules/TMCM1640.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import ABNEncoderModule, DigitalHallModule, PIDModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import ABNEncoderModule, DigitalHallModule, PIDModule class TMCM1640(TMCLModule): diff --git a/pytrinamic/modules/TMCM1670.py b/pytrinamic/modules/TMCM1670.py index b2388bf3..cc5a91d9 100644 --- a/pytrinamic/modules/TMCM1670.py +++ b/pytrinamic/modules/TMCM1670.py @@ -1,6 +1,6 @@ -from pytrinamic.modules import TMCLModule -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule, AbsoluteEncoderModule -from pytrinamic.features import PIDModule +from ..modules import TMCLModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule, AbsoluteEncoderModule +from ..features import PIDModule class TMCM1670(TMCLModule): diff --git a/pytrinamic/modules/TMCM3110.py b/pytrinamic/modules/TMCM3110.py index 306d7248..5dc3459f 100644 --- a/pytrinamic/modules/TMCM3110.py +++ b/pytrinamic/modules/TMCM3110.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM3110(TMCLModule): diff --git a/pytrinamic/modules/TMCM3312.py b/pytrinamic/modules/TMCM3312.py index 6b07bc3e..211681e3 100644 --- a/pytrinamic/modules/TMCM3312.py +++ b/pytrinamic/modules/TMCM3312.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM3312(TMCLModule): diff --git a/pytrinamic/modules/TMCM3351.py b/pytrinamic/modules/TMCM3351.py index 978185b8..fd9c8476 100644 --- a/pytrinamic/modules/TMCM3351.py +++ b/pytrinamic/modules/TMCM3351.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM3351(TMCLModule): diff --git a/pytrinamic/modules/TMCM6110.py b/pytrinamic/modules/TMCM6110.py index 509c116f..9566528e 100644 --- a/pytrinamic/modules/TMCM6110.py +++ b/pytrinamic/modules/TMCM6110.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM6110(TMCLModule): diff --git a/pytrinamic/modules/TMCM6212.py b/pytrinamic/modules/TMCM6212.py index 1a608dfc..c6f643a7 100644 --- a/pytrinamic/modules/TMCM6212.py +++ b/pytrinamic/modules/TMCM6212.py @@ -1,8 +1,8 @@ -from pytrinamic.modules import TMCLModule +from ..modules import TMCLModule # features -from pytrinamic.features import MotorControlModule, DriveSettingModule, LinearRampModule -from pytrinamic.features import StallGuard2Module, CoolStepModule +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule class TMCM6212(TMCLModule): diff --git a/pytrinamic/modules/__init__.py b/pytrinamic/modules/__init__.py index 4392c0be..ad68fbcc 100644 --- a/pytrinamic/modules/__init__.py +++ b/pytrinamic/modules/__init__.py @@ -1,6 +1,7 @@ from .tmcl_module import TMCLModule from .TMCC160 import TMCC160 from .TMCM1140 import TMCM1140 +from .TMCM1141 import TMCM1141 from .TMCM1160 import TMCM1160 from .TMCM1161 import TMCM1161 from .TMCM1240 import TMCM1240 From 752bcedddd8efe8d87159d7182ea9eb81e1e83ad Mon Sep 17 00:00:00 2001 From: bp Date: Wed, 27 Jul 2022 15:35:45 +0200 Subject: [PATCH 11/51] Bump: the version for a new release --- pytrinamic/version.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pytrinamic/version.py b/pytrinamic/version.py index 2995a62e..1f4ca897 100644 --- a/pytrinamic/version.py +++ b/pytrinamic/version.py @@ -1,2 +1,2 @@ -__version__ = "0.2.1" +__version__ = "0.2.2" From 71623c032035f98d1b3036032702d6d626ea4e14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Mon, 15 Aug 2022 10:23:00 +0200 Subject: [PATCH 12/51] setup.py: Added explicit encoding selection for reading README.md --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 475a3dbb..4a699d39 100644 --- a/setup.py +++ b/setup.py @@ -2,7 +2,7 @@ import setuptools from pytrinamic.version import __version__ -with open("README.md", "r") as fh: +with open("README.md", "r", encoding="utf8") as fh: long_description = fh.read() setuptools.setup( From 469c028ba15f10bf9bcc72ceb87b82af7347a266 Mon Sep 17 00:00:00 2001 From: ed Date: Wed, 17 Aug 2022 12:49:33 +0200 Subject: [PATCH 13/51] Added TMCL examples for TMCM-1638. --- .../TMCM1638/TMCL/encoder_position_mode.py | 68 +++++++++++++++++ .../TMCM1638/TMCL/hall_position_mode.py | 73 +++++++++++++++++++ pytrinamic/modules/__init__.py | 1 + 3 files changed, 142 insertions(+) create mode 100644 examples/modules/TMCM1638/TMCL/encoder_position_mode.py create mode 100644 examples/modules/TMCM1638/TMCL/hall_position_mode.py diff --git a/examples/modules/TMCM1638/TMCL/encoder_position_mode.py b/examples/modules/TMCM1638/TMCL/encoder_position_mode.py new file mode 100644 index 00000000..b19f2d26 --- /dev/null +++ b/examples/modules/TMCM1638/TMCL/encoder_position_mode.py @@ -0,0 +1,68 @@ +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.modules import TMCM1638 +import time + +pytrinamic.show_info() +# connection_manager = ConnectionManager("--interface serial_tmcl --port COM4 --data-rate 115200") +connection_manager = ConnectionManager("--interface kvaser_tmcl --module-id 1") + +with connection_manager.connect() as my_interface: + module = TMCM1638(my_interface) + motor = module.motors[0] + + # Define motor configuration for the TMCM-1638. + # + # The configuration is based on our standard BLDC motor (QBL4208-61-04-013-1024-AT). + # If you use a different motor be sure you have the right configuration setup otherwise the script may not work. + + # drive configuration + motor.drive_settings.motor_type = motor.ENUM.MOTOR_TYPE_THREE_PHASE_BLDC + motor.drive_settings.pole_pairs = 4 + motor.drive_settings.max_current = 2000 + motor.drive_settings.commutation_mode = motor.ENUM.COMM_MODE_ABN_ENCODER + motor.drive_settings.target_reached_distance = 5 + motor.drive_settings.target_reached_velocity = 500 + motor.drive_settings.open_loop_current = 1000 + print(motor.drive_settings) + + # encoder configuration + motor.abn_encoder.resolution = 4096 # 16384 + motor.abn_encoder.direction = 1 + motor.abn_encoder.init_mode = motor.ENUM.ENCODER_INIT_MODE_0 + print(motor.abn_encoder) + + # motion settings + motor.linear_ramp.max_velocity = 2000 + motor.linear_ramp.max_acceleration = 1000 + motor.linear_ramp.enabled = 1 + print(motor.linear_ramp) + + motor.set_axis_parameter(motor.AP.PositionScaler, motor.abn_encoder.resolution) + + # PI configuration + motor.pid.torque_p = 500 + motor.pid.torque_i = 1000 + motor.pid.velocity_p = 500 + motor.pid.velocity_i = 100 + motor.pid.position_p = 300 + print(motor.pid) + + time.sleep(1.0) + + # clear actual position + motor.actual_position = 0 + + # set target position + motor.move_to(motor.abn_encoder.resolution * 50) + while not(motor.get_position_reached()): + print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + time.sleep(0.2) + + # move back to zero position + motor.move_to(0) + while not(motor.get_position_reached()): + print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + time.sleep(0.2) + +print("\nReady.") diff --git a/examples/modules/TMCM1638/TMCL/hall_position_mode.py b/examples/modules/TMCM1638/TMCL/hall_position_mode.py new file mode 100644 index 00000000..c44b3e33 --- /dev/null +++ b/examples/modules/TMCM1638/TMCL/hall_position_mode.py @@ -0,0 +1,73 @@ +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.modules import TMCM1638 +import time + +pytrinamic.show_info() +# connection_manager = ConnectionManager("--interface serial_tmcl --port COM4 --data-rate 115200") +connection_manager = ConnectionManager("--interface kvaser_tmcl --module-id 1") + +with connection_manager.connect() as my_interface: + module = TMCM1638(my_interface) + motor = module.motors[0] + + # Define motor configuration for the TMCM-1638. + # + # The configuration is based on our standard BLDC motor (QBL4208-61-04-013-1024-AT). + # If you use a different motor be sure you have the right configuration setup otherwise the script may not work. + + # drive configuration + motor.drive_settings.motor_type = motor.ENUM.MOTOR_TYPE_THREE_PHASE_BLDC + motor.drive_settings.pole_pairs = 4 + motor.drive_settings.max_current = 2000 + motor.drive_settings.commutation_mode = motor.ENUM.COMM_MODE_DIGITAL_HALL + motor.drive_settings.target_reached_distance = 5 + motor.drive_settings.target_reached_velocity = 500 + print(motor.drive_settings) + + # hall sensor configuration + motor.digital_hall.direction = 0 + motor.digital_hall.polarity = 1 + motor.digital_hall.offset = 0 + motor.digital_hall.interpolation = 1 + print(motor.digital_hall) + + # ramp settings + motor.linear_ramp.max_velocity = 2000 + motor.linear_ramp.max_acceleration = 1000 + motor.linear_ramp.enabled = 1 + print(motor.linear_ramp) + + motor.set_axis_parameter(motor.AP.PositionScaler, 6*motor.drive_settings.pole_pairs) + + # PI configuration + motor.pid.torque_p = 300 + motor.pid.torque_i = 300 + motor.pid.velocity_p = 300 + motor.pid.velocity_i = 100 + motor.pid.position_p = 200 + print(motor.pid) + + # set position counter to zero + motor.actual_position = 0 + + # move to zero position + motor.move_to(0) + + print("starting positioning") + motor.move_to(4000) + + # wait for position reached + while not(motor.get_position_reached()): + print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + time.sleep(0.2) + + # move back to zero + motor.move_to(0) + + # wait for position reached + while not(motor.get_position_reached()): + print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + time.sleep(0.2) + +print("\nReady.") diff --git a/pytrinamic/modules/__init__.py b/pytrinamic/modules/__init__.py index ad68fbcc..ef54fd69 100644 --- a/pytrinamic/modules/__init__.py +++ b/pytrinamic/modules/__init__.py @@ -14,6 +14,7 @@ from .TMCM1633 import TMCM1633 from .TMCM1636 import TMCM1636 from .TMCM1637 import TMCM1637 +from .TMCM1638 import TMCM1638 from .TMCM1640 import TMCM1640 from .TMCM1670 import TMCM1670 from .TMCM3110 import TMCM3110 From 94f3b36c9964d846802b9ba79e8685ea3d401408 Mon Sep 17 00:00:00 2001 From: ed Date: Wed, 17 Aug 2022 20:24:20 +0200 Subject: [PATCH 14/51] Updated TMCM-1637/TMCM-1638 examples to use str.format. --- .../modules/TMCM1637/TMCL/encoder_position_mode.py | 8 ++++++-- examples/modules/TMCM1637/TMCL/hall_position_mode.py | 11 ++++------- .../modules/TMCM1638/TMCL/encoder_position_mode.py | 8 ++++++-- examples/modules/TMCM1638/TMCL/hall_position_mode.py | 11 ++++------- 4 files changed, 20 insertions(+), 18 deletions(-) diff --git a/examples/modules/TMCM1637/TMCL/encoder_position_mode.py b/examples/modules/TMCM1637/TMCL/encoder_position_mode.py index 9e93ad43..9908f65f 100644 --- a/examples/modules/TMCM1637/TMCL/encoder_position_mode.py +++ b/examples/modules/TMCM1637/TMCL/encoder_position_mode.py @@ -55,14 +55,18 @@ # set target position motor.move_to(motor.abn_encoder.resolution * 50) + + # wait for position reached while not(motor.get_position_reached()): - print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + print("target position: {} actual position: {}".format(motor.target_position, motor.actual_position)) time.sleep(0.2) # move back to zero position motor.move_to(0) + + # wait for position reached while not(motor.get_position_reached()): - print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + print("target position: {} actual position: {}".format(motor.target_position, motor.actual_position)) time.sleep(0.2) print("\nReady.") diff --git a/examples/modules/TMCM1637/TMCL/hall_position_mode.py b/examples/modules/TMCM1637/TMCL/hall_position_mode.py index bb0da373..5a31f156 100644 --- a/examples/modules/TMCM1637/TMCL/hall_position_mode.py +++ b/examples/modules/TMCM1637/TMCL/hall_position_mode.py @@ -48,18 +48,15 @@ motor.pid.position_p = 200 print(motor.pid) - # set position counter to zero + # clear actual position motor.actual_position = 0 - # move to zero position - motor.move_to(0) - - print("starting positioning") + # set target position motor.move_to(4000) # wait for position reached while not(motor.get_position_reached()): - print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + print("target position: {} actual position: {}".format(motor.target_position, motor.actual_position)) time.sleep(0.2) # move back to zero @@ -67,7 +64,7 @@ # wait for position reached while not(motor.get_position_reached()): - print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + print("target position: {} actual position: {}".format(motor.target_position, motor.actual_position)) time.sleep(0.2) print("\nReady.") diff --git a/examples/modules/TMCM1638/TMCL/encoder_position_mode.py b/examples/modules/TMCM1638/TMCL/encoder_position_mode.py index b19f2d26..bb2c701c 100644 --- a/examples/modules/TMCM1638/TMCL/encoder_position_mode.py +++ b/examples/modules/TMCM1638/TMCL/encoder_position_mode.py @@ -55,14 +55,18 @@ # set target position motor.move_to(motor.abn_encoder.resolution * 50) + + # wait for position reached while not(motor.get_position_reached()): - print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + print("target position: {} actual position: {}".format(motor.target_position, motor.actual_position)) time.sleep(0.2) # move back to zero position motor.move_to(0) + + # wait for position reached while not(motor.get_position_reached()): - print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + print("target position: {} actual position: {}".format(motor.target_position, motor.actual_position)) time.sleep(0.2) print("\nReady.") diff --git a/examples/modules/TMCM1638/TMCL/hall_position_mode.py b/examples/modules/TMCM1638/TMCL/hall_position_mode.py index c44b3e33..56af3207 100644 --- a/examples/modules/TMCM1638/TMCL/hall_position_mode.py +++ b/examples/modules/TMCM1638/TMCL/hall_position_mode.py @@ -48,18 +48,15 @@ motor.pid.position_p = 200 print(motor.pid) - # set position counter to zero + # clear actual position motor.actual_position = 0 - # move to zero position - motor.move_to(0) - - print("starting positioning") + # set target position motor.move_to(4000) # wait for position reached while not(motor.get_position_reached()): - print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + print("target position: {} actual position: {}".format(motor.target_position, motor.actual_position)) time.sleep(0.2) # move back to zero @@ -67,7 +64,7 @@ # wait for position reached while not(motor.get_position_reached()): - print("target position: " + str(motor.target_position) + " actual position: " + str(motor.actual_position)) + print("target position: {} actual position: {}".format(motor.target_position, motor.actual_position)) time.sleep(0.2) print("\nReady.") From 2de94b3cbd94d4e001334df449391f3608613a99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Thu, 25 Aug 2022 10:50:05 +0200 Subject: [PATCH 15/51] Updated firmware uploading tool - Fixed breakage from function refactoring - Switched from manual hex file parsing to intelhex package - Added usage of logging package - Switched to argparse for commandline arguments --- examples/tools/FirmwareUpdate.py | 187 ++++++++++--------------------- setup.py | 6 + 2 files changed, 68 insertions(+), 125 deletions(-) diff --git a/examples/tools/FirmwareUpdate.py b/examples/tools/FirmwareUpdate.py index abcdda6e..45b89e88 100644 --- a/examples/tools/FirmwareUpdate.py +++ b/examples/tools/FirmwareUpdate.py @@ -1,8 +1,11 @@ +import argparse import sys import time import math import re import struct +import logging +import intelhex import pytrinamic from pytrinamic.connections.connection_manager import ConnectionManager @@ -11,134 +14,57 @@ # Timeout in seconds for reconnecting to the module after sending the TMCL_BOOT # command. SERIAL_BOOT_TIMEOUT = 100 -# ################################ Preparation ################################## -pytrinamic.show_info() - -if len(sys.argv) < 2: - print("Usage:\n\tFirmwareUpdate.py HexFilePath [connection options]") - exit(1) - -print("Opening hex file (" + sys.argv[1] + ")") - -try: - f = open(sys.argv[1], "rt") -except FileNotFoundError: - print("Error: Hex file not found") - exit(1) - -connectionManager = ConnectionManager(sys.argv) - -# ############################## Hex file parsing ############################### -print("Parsing hex file") - -data = [] -extendedAddress = 0 -segmentAddress = 0 -for lineNumber, line in enumerate(f, 1): - # ## Parse a hex file line - # Check for RECORD MARK - if line[0] != ':': - continue - - # CHECKSUM validation - # All Bytes summed together modulo 256 have to be 0 - checksum = 0 - for i in range(1, len(line)-1, 2): - checksum = checksum + int(line[i:i+2], 16) - if checksum % 256 != 0: - print("Error: Invalid Checksum in line " + str(lineNumber)) - exit(1) - - # Read the fields of the entry - rec_len = int(line[1:3], 16) - rec_address = int(line[3:7], 16) - rec_type = int(line[7:9], 16) - rec_data = line[9:rec_len*2+9] - - # RECLEN validation - # Total characters: - # 1: RECORD MARK - # 2: RECLEN - # 4: LOAD OFFSET - # 2: RECTYPE - # RECLEN*2: DATA / INFO - # 2: CHKSUM - # 1: \n - if 1 + 2 + 4 + 2 + (rec_len*2) + 2 + 1 != len(line): - print("Error: Invalid record length in line " + str(lineNumber)) - - # ## Record type distinction - if rec_type == 0: - # Type: Data Record - address = extendedAddress + segmentAddress + rec_address - if address % 4 != 0: - print("Error: Address is not 4-Byte aligned (Line " + str(lineNumber) + ")") - exit(1) - - data.append((address, rec_len, rec_data)) - - if rec_type == 1: - # Type: End of File Record - break - - if rec_type == 2: - # Type: Extended Segment Address Record - segmentAddress = int(rec_data, 16) * 0x10 +# ################################ Commandline ################################## +parser = argparse.ArgumentParser() - if extendedAddress != 0: - print("Warning: Hex file uses both Type 2 and Type 4 records!") +# Mandatory arguments +parser.add_argument("hex_file", metavar="hex-file", help="Hex file to be uploaded") - if rec_type == 3: - # Type: Start Segment Address Record - # Ignore this record - pass +# Optional arguments +parser.add_argument('-v', '--verbose', action="count", default=0, help="Verbosity level") - if rec_type == 4: - # Type: Extended Linear Address Record - extendedAddress = int(rec_data, 16) * 0x10000 - print("Extended Address: " + hex(extendedAddress)) +# ConnectionManager arguments +ConnectionManager.argparse(parser) - if segmentAddress != 0: - print("Warning: Hex file uses both Type 2 and Type 4 records!") +args = parser.parse_args() - if rec_type == 5: - # Type: Start Linear Address Record - # Ignore this record - pass - -print("Parsed " + str(lineNumber) + " lines containing " + str(len(data)) + " data records") +# ################################ Preparation ################################## +pytrinamic.show_info() -# Make sure that the data is sorted by address -data.sort(key=lambda x: x[0]) +connectionManager = ConnectionManager(sys.argv) -f.close() +if args.verbose == 0: + log_level = logging.ERROR +elif args.verbose == 1: + log_level = logging.WARNING +elif args.verbose == 2: + log_level = logging.INFO +else: + log_level = logging.DEBUG -print() +logging.basicConfig(stream=sys.stdout, level=log_level) +############################### Hex file parsing ############################### +print("Opening hex file (" + args.hex_file + ")") +file = intelhex.IntelHex(args.hex_file) +file.padding = 0x00 # ########################## Binary data preparation ############################ # Get the boundaries and size of the data -start_address = data[0][0] -end_address = data[-1][0] + data[-1][1] +start_address = file.minaddr() +end_address = file.maxaddr() length = end_address - start_address -# Extract the parsed hex data into a bytearray -byteData = bytearray(length) +# Calculate the checksum checksum = 0 -for entry in data: - address = entry[0] - - for i in range(0, entry[1]): - value = int(entry[2][i*2:(i+1)*2], 16) - checksum = (checksum + value) & 0xFFFFFFFF - byteData[address-start_address + i] = value +for addr in file.addresses(): + checksum += file[addr] + checksum &= 0xFFFFFFFF -print("Start address: 0x{0:08X}".format(start_address)) -print("End address: 0x{0:08X}".format(end_address)) -print("Length: 0x{0:08X}".format(length)) -print("Checksum: 0x{0:08X}".format(checksum)) - -print() +logging.info("Start address: 0x{0:08X}".format(start_address)) +logging.info("End address: 0x{0:08X}".format(end_address)) +logging.info("Length: 0x{0:08X}".format(length)) +logging.info("Checksum: 0x{0:08X}".format(checksum)) # ############################# Bootloader entry ################################ # Connect to the evaluation board @@ -166,26 +92,34 @@ print("Error: Timeout when attempting to reconnect to bootloader") exit(1) -myInterface.enableDebug(True) # Retrieve the bootloader version -bootloaderVersion = myInterface.getVersionString(1) +bootloaderVersion = myInterface.get_version_string(1) found = re.search("\d\d\d\dB\d\d\d", bootloaderVersion) if found: pattern = found.group(0)[0:4] + "V\d\d\d" - print("Scanning new firmware data for correct module string (" + found.group(0)[0:4] + "V###)") + logging.info(f"Scanning new firmware data for correct module string ({found.group(0)[0:4]}V###)") else: found = re.search("\d\d\dB\d\.\d\d", bootloaderVersion) if found: pattern = found.group(0)[0:3] + "V\d\.\d\d" - print("Scanning new firmware data for correct module string (" + found.group(0)[0:3] + "V#.##)") + logging.info(f"Scanning new firmware data for correct module string ({found.group(0)[0:3]}V#.##)") else: - print("Error: GetVersion returned invalid answer (" + bootloaderVersion + ")") + logging.error(f"GetVersion returned invalid answer ({bootloaderVersion})") exit(1) # Scan for the module string -firmwareString = str(byteData, encoding="ascii", errors="ignore") -found = re.search(pattern, firmwareString) -if not found: +found = None +for segment in file.segments(): + print(segment) + start = segment[0] + length = segment[1] - segment[0] + firmware_bytes = file.gets(start, length) + firmware_string = str(firmware_bytes, encoding="ascii", errors="ignore") + + found = re.search(pattern, firmware_string) + if found: + break +else: print("Error: No matching version string found in firmware image") exit(1) @@ -202,6 +136,10 @@ reply = myInterface.send(TMCLCommand.BOOT_GET_INFO, 2, 0, 0) mem_size = reply.value +logging.debug(f"Bootloader memory page size: 0x{mem_page_size:08X}") +logging.debug(f"Bootloader Memory start address: 0x{mem_start_address:08X}") +logging.debug(f"Bootloader Memory size: 0x{mem_size:08X}") + # Check if the page size is a power of two if not(((mem_page_size & (mem_page_size - 1)) == 0) and mem_page_size != 0): print("Error: Page size of module is not a power of two") @@ -252,11 +190,10 @@ def write32Bit(address, write_data): myInterface.send(TMCLCommand.BOOT_WRITE_BUFFER, math.floor(offset / 4) % 256, math.floor(math.floor(offset / 4) / 256), write_data) current_page_dirty = True - -for i in range(0, len(byteData), 4): - address = i + start_address - value = struct.unpack("=3" ], + extras_require={ + # Optional: Examples, scripts etc. that require additional libraries + "Extra": [ + "IntelHex>=2.3" + ], + }, classifiers=[ "Programming Language :: Python :: 3", "Development Status :: 4 - Beta", From 10f61d56ecab9ca8ebe4e83a78da28eb3f85f9c6 Mon Sep 17 00:00:00 2001 From: Leonard Kugis Date: Mon, 12 Sep 2022 04:31:52 +0200 Subject: [PATCH 16/51] Updated RAMDebug module for specification. --- pytrinamic/RAMDebug.py | 46 +++++++++++------------------------------- 1 file changed, 12 insertions(+), 34 deletions(-) diff --git a/pytrinamic/RAMDebug.py b/pytrinamic/RAMDebug.py index 5962a7f6..b1f13bc7 100644 --- a/pytrinamic/RAMDebug.py +++ b/pytrinamic/RAMDebug.py @@ -57,36 +57,35 @@ class RAMDebug_State(IntEnum): PRETRIGGER = 4 class Channel(): - def __init__(self, channel_type, value, address = 0, signed = False, mask = 0xFFFF_FFFF, shift = 0, eval_channel = 0): #TODO: add signed + def __init__(self, channel_type, value, address = 0, signed = False, mask = 0xFFFF_FFFF, shift = 0): #TODO: add signed self.value = value self.type = channel_type self.shift = shift self.mask = mask self.address = address self.signed = signed - self.eval_channel = eval_channel @classmethod def axis_parameter(cls, motor, parameter_nr, eval_channel=0): channel_type = RAMDebug_Channel.CHANNEL_AXIS_PARAMETER - value = ((motor << 24) & 0xFF00_0000) | (parameter_nr & 0x0000_00FF) + value = ((motor << 24) & 0xFF00_0000) | ((eval_channel << 16) & 0x0001_0000) | (parameter_nr & 0x0000_00FF) # Error if value is bigger than 8 bits - return cls(channel_type, value, eval_channel=eval_channel) + return cls(channel_type, value) @classmethod def register(cls, motor, address, signed=False, eval_channel=0): channel_type = RAMDebug_Channel.CHANNEL_REGISTER - value = ((motor << 24) & 0xFF00_0000) | (address & 0x0000_FFFF) + value = ((motor << 24) & 0xFF00_0000) | ((eval_channel << 16) & 0x0001_0000) | (address & 0x0000_FFFF) # Error if value is bigger than 8 bits - return cls(channel_type, value, address, signed, eval_channel=eval_channel) + return cls(channel_type, value, address, signed) @classmethod def stacked_register(cls, motor, data_address, selector_address, value, eval_channel=0): channel_type = RAMDebug_Channel.CHANNEL_STACKED_REGISTER - value = ((motor << 24) & 0xFF00_0000) | (value << 16 & 0x00FF_0000) | (selector_address << 8 & 0x0000_FF00) | (data_address & 0x0000_00FF) + value = ((motor << 24) & 0xFF00_0000) | (value << 16 & 0x00FF_0000) | ((eval_channel << 16) & 0x0001_0000) | (selector_address << 8 & 0x0000_FF00) | (data_address & 0x0000_00FF) # Error if value is bigger than 8 bits - return cls(channel_type, value, eval_channel=eval_channel) + return cls(channel_type, value) @classmethod def field(cls, motor, field, signed=False, eval_channel=0): @@ -96,9 +95,9 @@ def field(cls, motor, field, signed=False, eval_channel=0): mask = field[1] shift = field[2] - value = ((motor << 24) & 0xFF00_0000) | (address & 0x0000_FFFF) + value = ((motor << 24) & 0xFF00_0000) | ((eval_channel << 16) & 0x0001_0000) | (address & 0x0000_FFFF) - return cls(channel_type, value, address, signed, mask, shift, eval_channel=eval_channel) + return cls(channel_type, value, address, signed, mask, shift) @classmethod def memory_address(cls, address): @@ -176,10 +175,12 @@ def set_channel(self, channel): def get_channels(self): return self.channels - def start_measurement_module(self): + def start_measurement(self): self._command(RAMDebug_Command.INIT.value, 0, 0) self._command(RAMDebug_Command.SET_SAMPLE_COUNT.value, 0, self.get_total_samples()) + self._command(RAMDebug_Command.SET_PROCESS_FREQUENCY, 0, self._process_frequency) self._command(RAMDebug_Command.SET_PRESCALER.value, 0, self._prescaler) + for channel in self.channels: self._command(RAMDebug_Command.SET_CHANNEL.value, channel.type.value, channel.value) @@ -188,29 +189,6 @@ def start_measurement_module(self): self._command(RAMDebug_Command.SET_TRIGGER_CHANNEL.value, self._trigger_channel.type.value, self._trigger_channel.value) self._command(RAMDebug_Command.ENABLE_TRIGGER.value, self._trigger_type.value, self._trigger_threshold) - def start_measurement_eval(self): - self._command(RAMDebug_Command.INIT.value, 0, 0) - self._command(RAMDebug_Command.SET_SAMPLE_COUNT.value, 0, self.get_total_samples()) - self._command(RAMDebug_Command.SET_PROCESS_FREQUENCY, 0, self._process_frequency) - self._command(RAMDebug_Command.SET_PRESCALER.value, 0, self._prescaler) - for channel in self.channels: - self._command(RAMDebug_Command.SET_EVAL_CHANNEL.value, 0, channel.eval_channel) - self._command(RAMDebug_Command.SET_ADDRESS.value, 0, channel.value) - self._command(RAMDebug_Command.SET_TYPE.value, 0, channel.type.value) - - self._command(RAMDebug_Command.SET_SHIFT_MASK.value, self._trigger_shift, self._trigger_mask) - self._command(RAMDebug_Command.SET_PRETRIGGER_SAMPLE_COUNT.value, 0, self._pretrigger_samples * self.channel_count()) - self._command(RAMDebug_Command.SET_TRIGGER_TYPE.value, 0, self._trigger_channel.type.value) - self._command(RAMDebug_Command.SET_TRIGGER_EVAL_CHANNEL.value, 0, self._trigger_channel.eval_channel) - self._command(RAMDebug_Command.SET_TRIGGER_ADDRESS.value, 0, self._trigger_channel.value) - self._command(RAMDebug_Command.ENABLE_TRIGGER.value, self._trigger_type.value, self._trigger_threshold) - - def start_measurement(self, is_eval=False): - if(is_eval): - self.start_measurement_eval() - else: - self.start_measurement_module() - def is_measurement_done(self): return self.get_state() == RAMDebug_State.COMPLETE.value From ef12c8f4132fd6c92f7df37c8d88c687549e416e Mon Sep 17 00:00:00 2001 From: Leonard Kugis Date: Mon, 12 Sep 2022 04:32:33 +0200 Subject: [PATCH 17/51] MAX22216_EVAL: Updated RAMDebug example script according to new class. --- examples/evalboards/MAX22216/ramdebug.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/evalboards/MAX22216/ramdebug.py b/examples/evalboards/MAX22216/ramdebug.py index b15f454d..9511ffe5 100644 --- a/examples/evalboards/MAX22216/ramdebug.py +++ b/examples/evalboards/MAX22216/ramdebug.py @@ -13,9 +13,9 @@ debug = RAMDebug(my_interface) debug.set_channel(ch) - debug.set_trigger_type(RAMDebug_Trigger.TRIGGER_RISING_EDGE_SIGNED) + debug.set_trigger_type(RAMDebug_Trigger.TRIGGER_UNCONDITIONAL) debug.set_trigger_threshold(50) - debug.start_measurement(is_eval=True) + debug.start_measurement() while(not debug.is_measurement_done()): pass From 4cdaf3c5a05d26e083c39c61bc16edca5ffc2376 Mon Sep 17 00:00:00 2001 From: Leonard Kugis Date: Thu, 15 Sep 2022 10:45:04 +0200 Subject: [PATCH 18/51] MAX22216: Updated fields and registers to rev2. --- pytrinamic/ic/MAX22216.py | 556 +++++++++++++++++++------------------- 1 file changed, 284 insertions(+), 272 deletions(-) diff --git a/pytrinamic/ic/MAX22216.py b/pytrinamic/ic/MAX22216.py index d7d0be6a..e149c22e 100644 --- a/pytrinamic/ic/MAX22216.py +++ b/pytrinamic/ic/MAX22216.py @@ -103,38 +103,42 @@ class REG: CFG_I_3 = 0x40 I_DPM_PEAK_0 = 0x41 I_DPM_VALLEY_0 = 0x42 - REACTION_TIME_0 = 0x43 - I_ADC_0 = 0x44 - I_DC_0 = 0x45 - I_IND_AC_0 = 0x46 - R_0 = 0x47 - PWM_DUTY_0 = 0x48 - I_DPM_PEAK_1 = 0x49 - I_DPM_VALLEY_1 = 0x4A - REACTION_TIME_1 = 0x4B - I_ADC_1 = 0x4C - I_DC_1 = 0x4D - I_IND_AC_1 = 0x4E - R_1 = 0x4F - PWM_DUTY_1 = 0x50 - I_DPM_PEAK_2 = 0x51 - I_DPM_VALLEY_2 = 0x52 - REACTION_TIME_2 = 0x53 - I_ADC_2 = 0x54 - I_DC_2 = 0x55 - I_IND_AC_2 = 0x56 - R_2 = 0x57 - PWM_DUTY_2 = 0x58 - I_DPM_PEAK_3 = 0x59 - I_DPM_VALLEY_3 = 0x5A - REACTION_TIME_3 = 0x5B - I_ADC_3 = 0x5C - I_DC_3 = 0x5D - I_IND_AC_3 = 0x5E - R_3 = 0x5F - PWM_DUTY_3 = 0x60 - FAULT0 = 0x61 - FAULT1 = 0x62 + TRAVEL_TIME_0 = 0x43 + REACTION_TIME_0 = 0x44 + I_ADC_0 = 0x45 + I_DC_0 = 0x46 + I_IND_AC_0 = 0x47 + R_0 = 0x48 + PWM_DUTY_0 = 0x49 + I_DPM_PEAK_1 = 0x4A + I_DPM_VALLEY_1 = 0x4B + TRAVEL_TIME_1 = 0x4C + REACTION_TIME_1 = 0x4D + I_ADC_1 = 0x4E + I_DC_1 = 0x4F + I_IND_AC_1 = 0x50 + R_1 = 0x51 + PWM_DUTY_1 = 0x52 + I_DPM_PEAK_2 = 0x53 + I_DPM_VALLEY_2 = 0x54 + TRAVEL_TIME_2 = 0x55 + REACTION_TIME_2 = 0x56 + I_ADC_2 = 0x57 + I_DC_2 = 0x58 + I_IND_AC_2 = 0x59 + R_2 = 0x5A + PWM_DUTY_2 = 0x5B + I_DPM_PEAK_3 = 0x5C + I_DPM_VALLEY_3 = 0x5D + TRAVEL_TIME_3 = 0x5E + REACTION_TIME_3 = 0x5F + I_ADC_3 = 0x60 + I_DC_3 = 0x61 + I_IND_AC_3 = 0x62 + R_3 = 0x63 + PWM_DUTY_3 = 0x64 + FAULT0 = 0x65 + FAULT1 = 0x66 class FIELD: """ @@ -147,243 +151,251 @@ class FIELD: it. This allows the user to see the corresponding register name of a field without opening this file and searching for the definition. """ - CNTL0 = ( 0x00, 0x00000001, 0 ) - CNTL1 = ( 0x00, 0x00000002, 1 ) - CNTL2 = ( 0x00, 0x00000004, 2 ) - CNTL3 = ( 0x00, 0x00000008, 3 ) - F_PWM_M = ( 0x00, 0x000000F0, 4 ) - CHS = ( 0x01, 0x0000000F, 0 ) - VDR_NDUTY = ( 0x01, 0x00000010, 4 ) - STAT_POL = ( 0x01, 0x00000040, 6 ) - CNTL_POL = ( 0x01, 0x00000080, 7 ) - M_UVM = ( 0x01, 0x00000100, 8 ) - M_COMF = ( 0x01, 0x00000200, 9 ) - M_DPM = ( 0x01, 0x00000400, 10 ) - M_HHF = ( 0x01, 0x00000800, 11 ) - M_OLF = ( 0x01, 0x00001000, 12 ) - M_OCP = ( 0x01, 0x00002000, 13 ) - M_OVT = ( 0x01, 0x00004000, 14 ) - ACTIVE = ( 0x01, 0x00008000, 15 ) - RFU = ( 0x02, 0x00000001, 0 ) - UVM = ( 0x02, 0x00000002, 1 ) - COMER = ( 0x02, 0x00000004, 2 ) - DPM = ( 0x02, 0x00000008, 3 ) - HHF = ( 0x02, 0x00000010, 4 ) - OLF = ( 0x02, 0x00000020, 5 ) - OCP = ( 0x02, 0x00000040, 6 ) - OVT = ( 0x02, 0x00000080, 7 ) - IND = ( 0x02, 0x00000100, 8 ) - RES = ( 0x02, 0x00000200, 9 ) - MIN_T_ON = ( 0x02, 0x00000400, 10 ) - STAT0 = ( 0x02, 0x00000800, 11 ) - STAT1 = ( 0x02, 0x00001000, 12 ) - STAT2 = ( 0x02, 0x00002000, 13 ) - STAT3 = ( 0x02, 0x00004000, 14 ) - STAT_FUN = ( 0x03, 0x00000007, 0 ) - STAT_SEL0 = ( 0x03, 0x00000008, 3 ) - STAT_SEL1 = ( 0x03, 0x00000010, 4 ) - STRETCH_EN = ( 0x03, 0x00000060, 5 ) - EN_LDO = ( 0x03, 0x00000080, 7 ) - V5_NV3 = ( 0x03, 0x00000100, 8 ) - M_UVM_CMP = ( 0x03, 0x00000200, 9 ) - DC_H2L = ( 0x04, 0x0000FFFF, 0 ) - ADC_VM_RAW = ( 0x05, 0x00001FFF, 0 ) - VM_THLD_DOWN = ( 0x06, 0x0000000F, 0 ) - VM_THLD_UP = ( 0x06, 0x000000F0, 4 ) - DELTA_PHI = ( 0x07, 0x00000FFF, 0 ) - U_AC = ( 0x08, 0x00007FFF, 0 ) - DC_L2H = ( 0x09, 0x0000FFFF, 0 ) - DC_H = ( 0x0A, 0x0000FFFF, 0 ) - DC_L = ( 0x0B, 0x0000FFFF, 0 ) - TIME_L2H = ( 0x0C, 0x0000FFFF, 0 ) - RAMP = ( 0x0D, 0x000000FF, 0 ) - RUPE = ( 0x0D, 0x00000100, 8 ) - RMDE = ( 0x0D, 0x00000200, 9 ) - RDWE = ( 0x0D, 0x00000400, 10 ) - H2L_EN = ( 0x0D, 0x00000800, 11 ) - OL_EN = ( 0x0D, 0x00001000, 12 ) - HHF_EN = ( 0x0D, 0x00002000, 13 ) - CTRL_MODE = ( 0x0D, 0x0000C000, 14 ) - FSF = ( 0x0E, 0x00000003, 0 ) - SHUNT_SCALE = ( 0x0E, 0x0000000C, 2 ) - SLEW_RATE = ( 0x0E, 0x00000030, 4 ) - T_BLANKING = ( 0x0E, 0x000000C0, 6 ) - F_PWM = ( 0x0E, 0x00000300, 8 ) - HSNLS = ( 0x0E, 0x00000400, 10 ) - DPM_THLD = ( 0x0F, 0x00000FFF, 0 ) - DPM_MIN_CURRENT = ( 0x10, 0x000000FF, 0 ) - DPM_MIN_NBR = ( 0x10, 0x00000F00, 8 ) - END_HIT_AUTO = ( 0x10, 0x00001000, 12 ) - DPM_EN = ( 0x10, 0x00004000, 14 ) - IDC_THLD = ( 0x11, 0x0000FFFF, 0 ) - RES_THLD = ( 0x12, 0x0000FFFF, 0 ) - L_NBR_CALC = ( 0x13, 0x0000000F, 0 ) - L_MEAS_WCYCLES = ( 0x13, 0x000000F0, 4 ) - L_MEAS_H = ( 0x13, 0x00000100, 8 ) - L_MEAS_L2H = ( 0x13, 0x00000200, 9 ) - L_MEAS_EN = ( 0x13, 0x00000400, 10 ) - DITH_EN = ( 0x13, 0x00000800, 11 ) - IAC_THLD = ( 0x14, 0x00000FFF, 0 ) - CFG_P = ( 0x15, 0x0000FFFF, 0 ) - CFG_I = ( 0x16, 0x0000FFFF, 0 ) - #DC_L2H = ( 0x17, 0x0000FFFF, 0 ) - #DC_H = ( 0x18, 0x0000FFFF, 0 ) - #DC_L = ( 0x19, 0x0000FFFF, 0 ) - #TIME_L2H = ( 0x1A, 0x0000FFFF, 0 ) - #RAMP = ( 0x1B, 0x000000FF, 0 ) - #RUPE = ( 0x1B, 0x00000100, 8 ) - #RMDE = ( 0x1B, 0x00000200, 9 ) - #RDWE = ( 0x1B, 0x00000400, 10 ) - #H2L_EN = ( 0x1B, 0x00000800, 11 ) - #OL_EN = ( 0x1B, 0x00001000, 12 ) - #HHF_EN = ( 0x1B, 0x00002000, 13 ) - #CTRL_MODE = ( 0x1B, 0x0000C000, 14 ) - #FSF = ( 0x1C, 0x00000003, 0 ) - #SHUNT_SCALE = ( 0x1C, 0x0000000C, 2 ) - #SLEW_RATE = ( 0x1C, 0x00000030, 4 ) - #T_BLANKING = ( 0x1C, 0x000000C0, 6 ) - #F_PWM = ( 0x1C, 0x00000300, 8 ) - #HSNLS = ( 0x1C, 0x00000400, 10 ) - #DPM_THLD = ( 0x1D, 0x00000FFF, 0 ) - #DPM_MIN_CURRENT = ( 0x1E, 0x000000FF, 0 ) - #DPM_MIN_NBR = ( 0x1E, 0x00000F00, 8 ) - #END_HIT_AUTO = ( 0x1E, 0x00001000, 12 ) - #DPM_EN = ( 0x1E, 0x00004000, 14 ) - #IDC_THLD = ( 0x1F, 0x0000FFFF, 0 ) - #RES_THLD = ( 0x20, 0x0000FFFF, 0 ) - #L_NBR_CALC = ( 0x21, 0x0000000F, 0 ) - #L_MEAS_WCYCLES = ( 0x21, 0x000000F0, 4 ) - #L_MEAS_H = ( 0x21, 0x00000100, 8 ) - #L_MEAS_L2H = ( 0x21, 0x00000200, 9 ) - #L_MEAS_EN = ( 0x21, 0x00000400, 10 ) - #DITH_EN = ( 0x21, 0x00000800, 11 ) - #IAC_THLD = ( 0x22, 0x00000FFF, 0 ) - #CFG_P = ( 0x23, 0x0000FFFF, 0 ) - #CFG_I = ( 0x24, 0x0000FFFF, 0 ) - #DC_L2H = ( 0x25, 0x0000FFFF, 0 ) - #DC_H = ( 0x26, 0x0000FFFF, 0 ) - #DC_L = ( 0x27, 0x0000FFFF, 0 ) - #TIME_L2H = ( 0x28, 0x0000FFFF, 0 ) - #RAMP = ( 0x29, 0x000000FF, 0 ) - #RUPE = ( 0x29, 0x00000100, 8 ) - #RMDE = ( 0x29, 0x00000200, 9 ) - #RDWE = ( 0x29, 0x00000400, 10 ) - #H2L_EN = ( 0x29, 0x00000800, 11 ) - #OL_EN = ( 0x29, 0x00001000, 12 ) - #HHF_EN = ( 0x29, 0x00002000, 13 ) - #CTRL_MODE = ( 0x29, 0x0000C000, 14 ) - #FSF = ( 0x2A, 0x00000003, 0 ) - #SHUNT_SCALE = ( 0x2A, 0x0000000C, 2 ) - #SLEW_RATE = ( 0x2A, 0x00000030, 4 ) - #T_BLANKING = ( 0x2A, 0x000000C0, 6 ) - #F_PWM = ( 0x2A, 0x00000300, 8 ) - #HSNLS = ( 0x2A, 0x00000400, 10 ) - #DPM_THLD = ( 0x2B, 0x00000FFF, 0 ) - #DPM_MIN_CURRENT = ( 0x2C, 0x000000FF, 0 ) - #DPM_MIN_NBR = ( 0x2C, 0x00000F00, 8 ) - #END_HIT_AUTO = ( 0x2C, 0x00001000, 12 ) - #DPM_EN = ( 0x2C, 0x00004000, 14 ) - #IDC_THLD = ( 0x2D, 0x0000FFFF, 0 ) - #RES_THLD = ( 0x2E, 0x0000FFFF, 0 ) - #L_NBR_CALC = ( 0x2F, 0x0000000F, 0 ) - #L_MEAS_WCYCLES = ( 0x2F, 0x000000F0, 4 ) - #L_MEAS_H = ( 0x2F, 0x00000100, 8 ) - #L_MEAS_L2H = ( 0x2F, 0x00000200, 9 ) - #L_MEAS_EN = ( 0x2F, 0x00000400, 10 ) - #DITH_EN = ( 0x2F, 0x00000800, 11 ) - #IAC_THLD = ( 0x30, 0x00000FFF, 0 ) - #CFG_P = ( 0x31, 0x0000FFFF, 0 ) - #CFG_I = ( 0x32, 0x0000FFFF, 0 ) - #DC_L2H = ( 0x33, 0x0000FFFF, 0 ) - #DC_H = ( 0x34, 0x0000FFFF, 0 ) - #DC_L = ( 0x35, 0x0000FFFF, 0 ) - #TIME_L2H = ( 0x36, 0x0000FFFF, 0 ) - #RAMP = ( 0x37, 0x000000FF, 0 ) - #RUPE = ( 0x37, 0x00000100, 8 ) - #RMDE = ( 0x37, 0x00000200, 9 ) - #RDWE = ( 0x37, 0x00000400, 10 ) - #H2L_EN = ( 0x37, 0x00000800, 11 ) - #OL_EN = ( 0x37, 0x00001000, 12 ) - #HHF_EN = ( 0x37, 0x00002000, 13 ) - #CTRL_MODE = ( 0x37, 0x0000C000, 14 ) - #FSF = ( 0x38, 0x00000003, 0 ) - #SHUNT_SCALE = ( 0x38, 0x0000000C, 2 ) - #SLEW_RATE = ( 0x38, 0x00000030, 4 ) - #T_BLANKING = ( 0x38, 0x000000C0, 6 ) - #F_PWM = ( 0x38, 0x00000300, 8 ) - #HSNLS = ( 0x38, 0x00000400, 10 ) - #DPM_THLD = ( 0x39, 0x00000FFF, 0 ) - #DPM_MIN_CURRENT = ( 0x3A, 0x000000FF, 0 ) - #DPM_MIN_NBR = ( 0x3A, 0x00000F00, 8 ) - #END_HIT_AUTO = ( 0x3A, 0x00001000, 12 ) - #DPM_EN = ( 0x3A, 0x00004000, 14 ) - #IDC_THLD = ( 0x3B, 0x0000FFFF, 0 ) - #RES_THLD = ( 0x3C, 0x0000FFFF, 0 ) - #L_NBR_CALC = ( 0x3D, 0x0000000F, 0 ) - #L_MEAS_WCYCLES = ( 0x3D, 0x000000F0, 4 ) - #L_MEAS_H = ( 0x3D, 0x00000100, 8 ) - #L_MEAS_L2H = ( 0x3D, 0x00000200, 9 ) - #L_MEAS_EN = ( 0x3D, 0x00000400, 10 ) - #DITH_EN = ( 0x3D, 0x00000800, 11 ) - #IAC_THLD = ( 0x3E, 0x00000FFF, 0 ) - #CFG_P = ( 0x3F, 0x0000FFFF, 0 ) - #CFG_I = ( 0x40, 0x0000FFFF, 0 ) - I_DPM_PEAK = ( 0x41, 0x0000FFFF, 0 ) - I_DPM_VALLEY = ( 0x42, 0x0000FFFF, 0 ) - REACTION_TIME = ( 0x43, 0x0000FFFF, 0 ) - I_MONITOR = ( 0x44, 0x0000FFFF, 0 ) - I_DC = ( 0x45, 0x0000FFFF, 0 ) - I_AC = ( 0x46, 0x0000FFFF, 0 ) - R = ( 0x47, 0x0000FFFF, 0 ) - PWM_DUTYCYCLE = ( 0x48, 0x0000FFFF, 0 ) - #I_DPM_PEAK = ( 0x49, 0x0000FFFF, 0 ) - #I_DPM_VALLEY = ( 0x4A, 0x0000FFFF, 0 ) - #REACTION_TIME = ( 0x4B, 0x0000FFFF, 0 ) - #I_MONITOR = ( 0x4C, 0x0000FFFF, 0 ) - #I_DC = ( 0x4D, 0x0000FFFF, 0 ) - #I_AC = ( 0x4E, 0x0000FFFF, 0 ) - #R = ( 0x4F, 0x0000FFFF, 0 ) - #PWM_DUTYCYCLE = ( 0x50, 0x0000FFFF, 0 ) - #I_DPM_PEAK = ( 0x51, 0x0000FFFF, 0 ) - #I_DPM_VALLEY = ( 0x52, 0x0000FFFF, 0 ) - #REACTION_TIME = ( 0x53, 0x0000FFFF, 0 ) - #I_MONITOR = ( 0x54, 0x0000FFFF, 0 ) - #I_DC = ( 0x55, 0x0000FFFF, 0 ) - #I_AC = ( 0x56, 0x0000FFFF, 0 ) - #R = ( 0x57, 0x0000FFFF, 0 ) - #PWM_DUTYCYCLE = ( 0x58, 0x0000FFFF, 0 ) - #I_DPM_PEAK = ( 0x59, 0x0000FFFF, 0 ) - #I_DPM_VALLEY = ( 0x5A, 0x0000FFFF, 0 ) - #REACTION_TIME = ( 0x5B, 0x0000FFFF, 0 ) - #I_MONITOR = ( 0x5C, 0x0000FFFF, 0 ) - #I_DC = ( 0x5D, 0x0000FFFF, 0 ) - #I_AC = ( 0x5E, 0x0000FFFF, 0 ) - #R = ( 0x5F, 0x0000FFFF, 0 ) - #PWM_DUTYCYCLE = ( 0x60, 0x0000FFFF, 0 ) - OCP0 = ( 0x61, 0x00000001, 0 ) - OCP1 = ( 0x61, 0x00000002, 1 ) - OCP2 = ( 0x61, 0x00000004, 2 ) - OCP3 = ( 0x61, 0x00000008, 3 ) - HHF0 = ( 0x61, 0x00000010, 4 ) - HHF1 = ( 0x61, 0x00000020, 5 ) - HHF2 = ( 0x61, 0x00000040, 6 ) - HHF3 = ( 0x61, 0x00000080, 7 ) - OLF0 = ( 0x61, 0x00000100, 8 ) - OLF1 = ( 0x61, 0x00000200, 9 ) - OLF2 = ( 0x61, 0x00000400, 10 ) - OLF3 = ( 0x61, 0x00000800, 11 ) - DPM0 = ( 0x61, 0x00001000, 12 ) - DPM1 = ( 0x61, 0x00002000, 13 ) - DPM2 = ( 0x61, 0x00004000, 14 ) - DPM3 = ( 0x61, 0x00008000, 15 ) - IND0 = ( 0x62, 0x00000001, 0 ) - IND1 = ( 0x62, 0x00000002, 1 ) - IND2 = ( 0x62, 0x00000004, 2 ) - IND3 = ( 0x62, 0x00000008, 3 ) - #UVM = ( 0x62, 0x00000010, 4 ) - #COMER = ( 0x62, 0x00000020, 5 ) - #OVT = ( 0x62, 0x00000040, 6 ) - RES0 = ( 0x62, 0x00000080, 7 ) - RES1 = ( 0x62, 0x00000100, 8 ) - RES2 = ( 0x62, 0x00000200, 9 ) - RES3 = ( 0x62, 0x00000400, 10 ) + CNTL0 = ( 0x00, 0x00000001, 0 ) + CNTL1 = ( 0x00, 0x00000002, 1 ) + CNTL2 = ( 0x00, 0x00000004, 2 ) + CNTL3 = ( 0x00, 0x00000008, 3 ) + F_PWM_M = ( 0x00, 0x000000F0, 4 ) + CHS = ( 0x01, 0x0000000F, 0 ) + VDR_NDUTY = ( 0x01, 0x00000010, 4 ) + STAT_POL = ( 0x01, 0x00000040, 6 ) + CNTL_POL = ( 0x01, 0x00000080, 7 ) + M_UVM = ( 0x01, 0x00000100, 8 ) + M_COMF = ( 0x01, 0x00000200, 9 ) + M_DPM = ( 0x01, 0x00000400, 10 ) + M_HHF = ( 0x01, 0x00000800, 11 ) + M_OLF = ( 0x01, 0x00001000, 12 ) + M_OCP = ( 0x01, 0x00002000, 13 ) + M_OVT = ( 0x01, 0x00004000, 14 ) + ACTIVE = ( 0x01, 0x00008000, 15 ) + RFU = ( 0x02, 0x00000001, 0 ) + UVM = ( 0x02, 0x00000002, 1 ) + COMER = ( 0x02, 0x00000004, 2 ) + DPM = ( 0x02, 0x00000008, 3 ) + HHF = ( 0x02, 0x00000010, 4 ) + OLF = ( 0x02, 0x00000020, 5 ) + OCP = ( 0x02, 0x00000040, 6 ) + OVT = ( 0x02, 0x00000080, 7 ) + IND = ( 0x02, 0x00000100, 8 ) + RES = ( 0x02, 0x00000200, 9 ) + MIN_T_ON = ( 0x02, 0x00000400, 10 ) + STAT0 = ( 0x02, 0x00000800, 11 ) + STAT1 = ( 0x02, 0x00001000, 12 ) + STAT2 = ( 0x02, 0x00002000, 13 ) + STAT3 = ( 0x02, 0x00004000, 14 ) + STAT_FUN = ( 0x03, 0x00000007, 0 ) + STAT_SEL0 = ( 0x03, 0x00000008, 3 ) + STAT_SEL1 = ( 0x03, 0x00000010, 4 ) + STRETCH_EN = ( 0x03, 0x00000060, 5 ) + EN_LDO = ( 0x03, 0x00000080, 7 ) + V5_NV3 = ( 0x03, 0x00000100, 8 ) + M_UVM_CMP = ( 0x03, 0x00000200, 9 ) + DC_H2L = ( 0x04, 0x0000FFFF, 0 ) + ADC_VM_RAW = ( 0x05, 0x00001FFF, 0 ) + VM_THLD_DOWN = ( 0x06, 0x0000000F, 0 ) + VM_THLD_UP = ( 0x06, 0x000000F0, 4 ) + DELTA_PHI = ( 0x07, 0x00000FFF, 0 ) + U_AC = ( 0x08, 0x00007FFF, 0 ) + DC_L2H = ( 0x09, 0x0000FFFF, 0 ) + DC_H = ( 0x0A, 0x0000FFFF, 0 ) + DC_L = ( 0x0B, 0x0000FFFF, 0 ) + TIME_L2H = ( 0x0C, 0x0000FFFF, 0 ) + RAMP = ( 0x0D, 0x000000FF, 0 ) + RUPE = ( 0x0D, 0x00000100, 8 ) + RMDE = ( 0x0D, 0x00000200, 9 ) + RDWE = ( 0x0D, 0x00000400, 10 ) + H2L_EN = ( 0x0D, 0x00000800, 11 ) + OL_EN = ( 0x0D, 0x00001000, 12 ) + HHF_EN = ( 0x0D, 0x00002000, 13 ) + CTRL_MODE = ( 0x0D, 0x0000C000, 14 ) + FSF = ( 0x0E, 0x00000003, 0 ) + SHUNT_SCALE = ( 0x0E, 0x0000000C, 2 ) + SLEW_RATE = ( 0x0E, 0x00000030, 4 ) + T_BLANKING = ( 0x0E, 0x000000C0, 6 ) + F_PWM = ( 0x0E, 0x00000300, 8 ) + HSNLS = ( 0x0E, 0x00000400, 10 ) + DPM_THLD = ( 0x0F, 0x00000FFF, 0 ) + DPM_MIN_CURRENT = ( 0x10, 0x000000FF, 0 ) + DPM_MIN_NBR = ( 0x10, 0x00000F00, 8 ) + END_HIT_AUTO = ( 0x10, 0x00001000, 12 ) + END_HIT_START_HIZ_AUTO = ( 0x10, 0x00002000, 13 ) + DPM_EN = ( 0x10, 0x00004000, 14 ) + IDC_THLD = ( 0x11, 0x0000FFFF, 0 ) + RES_THLD = ( 0x12, 0x0000FFFF, 0 ) + L_NBR_CALC = ( 0x13, 0x0000000F, 0 ) + L_MEAS_WCYCLES = ( 0x13, 0x000000F0, 4 ) + L_MEAS_H = ( 0x13, 0x00000100, 8 ) + L_MEAS_L2H = ( 0x13, 0x00000200, 9 ) + L_MEAS_EN = ( 0x13, 0x00000400, 10 ) + DITH_EN = ( 0x13, 0x00000800, 11 ) + IAC_THLD = ( 0x14, 0x00000FFF, 0 ) + CFG_P = ( 0x15, 0x0000FFFF, 0 ) + CFG_I = ( 0x16, 0x0000FFFF, 0 ) + #DC_L2H = ( 0x17, 0x0000FFFF, 0 ) + #DC_H = ( 0x18, 0x0000FFFF, 0 ) + #DC_L = ( 0x19, 0x0000FFFF, 0 ) + #TIME_L2H = ( 0x1A, 0x0000FFFF, 0 ) + #RAMP = ( 0x1B, 0x000000FF, 0 ) + #RUPE = ( 0x1B, 0x00000100, 8 ) + #RMDE = ( 0x1B, 0x00000200, 9 ) + #RDWE = ( 0x1B, 0x00000400, 10 ) + #H2L_EN = ( 0x1B, 0x00000800, 11 ) + #OL_EN = ( 0x1B, 0x00001000, 12 ) + #HHF_EN = ( 0x1B, 0x00002000, 13 ) + #CTRL_MODE = ( 0x1B, 0x0000C000, 14 ) + #FSF = ( 0x1C, 0x00000003, 0 ) + #SHUNT_SCALE = ( 0x1C, 0x0000000C, 2 ) + #SLEW_RATE = ( 0x1C, 0x00000030, 4 ) + #T_BLANKING = ( 0x1C, 0x000000C0, 6 ) + #F_PWM = ( 0x1C, 0x00000300, 8 ) + #HSNLS = ( 0x1C, 0x00000400, 10 ) + #DPM_THLD = ( 0x1D, 0x00000FFF, 0 ) + #DPM_MIN_CURRENT = ( 0x1E, 0x000000FF, 0 ) + #DPM_MIN_NBR = ( 0x1E, 0x00000F00, 8 ) + #END_HIT_AUTO = ( 0x1E, 0x00001000, 12 ) + #END_HIT_START_HIZ_AUTO = ( 0x1E, 0x00002000, 13 ) + #DPM_EN = ( 0x1E, 0x00004000, 14 ) + #IDC_THLD = ( 0x1F, 0x0000FFFF, 0 ) + #RES_THLD = ( 0x20, 0x0000FFFF, 0 ) + #L_NBR_CALC = ( 0x21, 0x0000000F, 0 ) + #L_MEAS_WCYCLES = ( 0x21, 0x000000F0, 4 ) + #L_MEAS_H = ( 0x21, 0x00000100, 8 ) + #L_MEAS_L2H = ( 0x21, 0x00000200, 9 ) + #L_MEAS_EN = ( 0x21, 0x00000400, 10 ) + #DITH_EN = ( 0x21, 0x00000800, 11 ) + #IAC_THLD = ( 0x22, 0x00000FFF, 0 ) + #CFG_P = ( 0x23, 0x0000FFFF, 0 ) + #CFG_I = ( 0x24, 0x0000FFFF, 0 ) + #DC_L2H = ( 0x25, 0x0000FFFF, 0 ) + #DC_H = ( 0x26, 0x0000FFFF, 0 ) + #DC_L = ( 0x27, 0x0000FFFF, 0 ) + #TIME_L2H = ( 0x28, 0x0000FFFF, 0 ) + #RAMP = ( 0x29, 0x000000FF, 0 ) + #RUPE = ( 0x29, 0x00000100, 8 ) + #RMDE = ( 0x29, 0x00000200, 9 ) + #RDWE = ( 0x29, 0x00000400, 10 ) + #H2L_EN = ( 0x29, 0x00000800, 11 ) + #OL_EN = ( 0x29, 0x00001000, 12 ) + #HHF_EN = ( 0x29, 0x00002000, 13 ) + #CTRL_MODE = ( 0x29, 0x0000C000, 14 ) + #FSF = ( 0x2A, 0x00000003, 0 ) + #SHUNT_SCALE = ( 0x2A, 0x0000000C, 2 ) + #SLEW_RATE = ( 0x2A, 0x00000030, 4 ) + #T_BLANKING = ( 0x2A, 0x000000C0, 6 ) + #F_PWM = ( 0x2A, 0x00000300, 8 ) + #HSNLS = ( 0x2A, 0x00000400, 10 ) + #DPM_THLD = ( 0x2B, 0x00000FFF, 0 ) + #DPM_MIN_CURRENT = ( 0x2C, 0x000000FF, 0 ) + #DPM_MIN_NBR = ( 0x2C, 0x00000F00, 8 ) + #END_HIT_AUTO = ( 0x2C, 0x00001000, 12 ) + #END_HIT_START_HIZ_AUTO = ( 0x2C, 0x00002000, 13 ) + #DPM_EN = ( 0x2C, 0x00004000, 14 ) + #IDC_THLD = ( 0x2D, 0x0000FFFF, 0 ) + #RES_THLD = ( 0x2E, 0x0000FFFF, 0 ) + #L_NBR_CALC = ( 0x2F, 0x0000000F, 0 ) + #L_MEAS_WCYCLES = ( 0x2F, 0x000000F0, 4 ) + #L_MEAS_H = ( 0x2F, 0x00000100, 8 ) + #L_MEAS_L2H = ( 0x2F, 0x00000200, 9 ) + #L_MEAS_EN = ( 0x2F, 0x00000400, 10 ) + #DITH_EN = ( 0x2F, 0x00000800, 11 ) + #IAC_THLD = ( 0x30, 0x00000FFF, 0 ) + #CFG_P = ( 0x31, 0x0000FFFF, 0 ) + #CFG_I = ( 0x32, 0x0000FFFF, 0 ) + #DC_L2H = ( 0x33, 0x0000FFFF, 0 ) + #DC_H = ( 0x34, 0x0000FFFF, 0 ) + #DC_L = ( 0x35, 0x0000FFFF, 0 ) + #TIME_L2H = ( 0x36, 0x0000FFFF, 0 ) + #RAMP = ( 0x37, 0x000000FF, 0 ) + #RUPE = ( 0x37, 0x00000100, 8 ) + #RMDE = ( 0x37, 0x00000200, 9 ) + #RDWE = ( 0x37, 0x00000400, 10 ) + #H2L_EN = ( 0x37, 0x00000800, 11 ) + #OL_EN = ( 0x37, 0x00001000, 12 ) + #HHF_EN = ( 0x37, 0x00002000, 13 ) + #CTRL_MODE = ( 0x37, 0x0000C000, 14 ) + #FSF = ( 0x38, 0x00000003, 0 ) + #SHUNT_SCALE = ( 0x38, 0x0000000C, 2 ) + #SLEW_RATE = ( 0x38, 0x00000030, 4 ) + #T_BLANKING = ( 0x38, 0x000000C0, 6 ) + #F_PWM = ( 0x38, 0x00000300, 8 ) + #HSNLS = ( 0x38, 0x00000400, 10 ) + #DPM_THLD = ( 0x39, 0x00000FFF, 0 ) + #DPM_MIN_CURRENT = ( 0x3A, 0x000000FF, 0 ) + #DPM_MIN_NBR = ( 0x3A, 0x00000F00, 8 ) + #END_HIT_AUTO = ( 0x3A, 0x00001000, 12 ) + #END_HIT_START_HIZ_AUTO = ( 0x3A, 0x00002000, 13 ) + #DPM_EN = ( 0x3A, 0x00004000, 14 ) + #IDC_THLD = ( 0x3B, 0x0000FFFF, 0 ) + #RES_THLD = ( 0x3C, 0x0000FFFF, 0 ) + #L_NBR_CALC = ( 0x3D, 0x0000000F, 0 ) + #L_MEAS_WCYCLES = ( 0x3D, 0x000000F0, 4 ) + #L_MEAS_H = ( 0x3D, 0x00000100, 8 ) + #L_MEAS_L2H = ( 0x3D, 0x00000200, 9 ) + #L_MEAS_EN = ( 0x3D, 0x00000400, 10 ) + #DITH_EN = ( 0x3D, 0x00000800, 11 ) + #IAC_THLD = ( 0x3E, 0x00000FFF, 0 ) + #CFG_P = ( 0x3F, 0x0000FFFF, 0 ) + #CFG_I = ( 0x40, 0x0000FFFF, 0 ) + I_DPM_PEAK = ( 0x41, 0x0000FFFF, 0 ) + I_DPM_VALLEY = ( 0x42, 0x0000FFFF, 0 ) + TRAVEL_TIME = ( 0x43, 0x0000FFFF, 0 ) + REACTION_TIME = ( 0x44, 0x0000FFFF, 0 ) + I_MONITOR = ( 0x45, 0x0000FFFF, 0 ) + I_DC = ( 0x46, 0x0000FFFF, 0 ) + I_AC = ( 0x47, 0x0000FFFF, 0 ) + R = ( 0x48, 0x0000FFFF, 0 ) + PWM_DUTYCYCLE = ( 0x49, 0x0000FFFF, 0 ) + #I_DPM_PEAK = ( 0x4A, 0x0000FFFF, 0 ) + #I_DPM_VALLEY = ( 0x4B, 0x0000FFFF, 0 ) + #TRAVEL_TIME = ( 0x4C, 0x0000FFFF, 0 ) + #REACTION_TIME = ( 0x4D, 0x0000FFFF, 0 ) + #I_MONITOR = ( 0x4E, 0x0000FFFF, 0 ) + #I_DC = ( 0x4F, 0x0000FFFF, 0 ) + #I_AC = ( 0x50, 0x0000FFFF, 0 ) + #R = ( 0x51, 0x0000FFFF, 0 ) + #PWM_DUTYCYCLE = ( 0x52, 0x0000FFFF, 0 ) + #I_DPM_PEAK = ( 0x53, 0x0000FFFF, 0 ) + #I_DPM_VALLEY = ( 0x54, 0x0000FFFF, 0 ) + #TRAVEL_TIME = ( 0x55, 0x0000FFFF, 0 ) + #REACTION_TIME = ( 0x56, 0x0000FFFF, 0 ) + #I_MONITOR = ( 0x57, 0x0000FFFF, 0 ) + #I_DC = ( 0x58, 0x0000FFFF, 0 ) + #I_AC = ( 0x59, 0x0000FFFF, 0 ) + #R = ( 0x5A, 0x0000FFFF, 0 ) + #PWM_DUTYCYCLE = ( 0x5B, 0x0000FFFF, 0 ) + #I_DPM_PEAK = ( 0x5C, 0x0000FFFF, 0 ) + #I_DPM_VALLEY = ( 0x5D, 0x0000FFFF, 0 ) + #TRAVEL_TIME = ( 0x5E, 0x0000FFFF, 0 ) + #REACTION_TIME = ( 0x5F, 0x0000FFFF, 0 ) + #I_MONITOR = ( 0x60, 0x0000FFFF, 0 ) + #I_DC = ( 0x61, 0x0000FFFF, 0 ) + #I_AC = ( 0x62, 0x0000FFFF, 0 ) + #R = ( 0x63, 0x0000FFFF, 0 ) + #PWM_DUTYCYCLE = ( 0x64, 0x0000FFFF, 0 ) + OCP0 = ( 0x65, 0x00000001, 0 ) + OCP1 = ( 0x65, 0x00000002, 1 ) + OCP2 = ( 0x65, 0x00000004, 2 ) + OCP3 = ( 0x65, 0x00000008, 3 ) + HHF0 = ( 0x65, 0x00000010, 4 ) + HHF1 = ( 0x65, 0x00000020, 5 ) + HHF2 = ( 0x65, 0x00000040, 6 ) + HHF3 = ( 0x65, 0x00000080, 7 ) + OLF0 = ( 0x65, 0x00000100, 8 ) + OLF1 = ( 0x65, 0x00000200, 9 ) + OLF2 = ( 0x65, 0x00000400, 10 ) + OLF3 = ( 0x65, 0x00000800, 11 ) + DPM0 = ( 0x65, 0x00001000, 12 ) + DPM1 = ( 0x65, 0x00002000, 13 ) + DPM2 = ( 0x65, 0x00004000, 14 ) + DPM3 = ( 0x65, 0x00008000, 15 ) + IND0 = ( 0x66, 0x00000001, 0 ) + IND1 = ( 0x66, 0x00000002, 1 ) + IND2 = ( 0x66, 0x00000004, 2 ) + IND3 = ( 0x66, 0x00000008, 3 ) + #UVM = ( 0x66, 0x00000010, 4 ) + #COMER = ( 0x66, 0x00000020, 5 ) + #OVT = ( 0x66, 0x00000040, 6 ) + RES0 = ( 0x66, 0x00000080, 7 ) + RES1 = ( 0x66, 0x00000100, 8 ) + RES2 = ( 0x66, 0x00000200, 9 ) + RES3 = ( 0x66, 0x00000400, 10 ) From 32afce872a5bfe221728afbbf97065fa25fa0853 Mon Sep 17 00:00:00 2001 From: Leonard Kugis Date: Thu, 22 Sep 2022 03:31:24 +0200 Subject: [PATCH 19/51] Evalboards: Moved field access functions to tmcl_eval and implemented axis field access function. Field access functions can be abstracted from the specific evalboard, since they are the same for every evalboard (unlike register access, which require distinction between mc and drv channel). Additionally, register axis field access functions have been implemented to handle register fields with same name and function on different axes. This has been generalized from Motor Control IC feature. This way, the determination of target register address, mask and shift is made by definition of ordered lists (by axis) in the field definitions. --- pytrinamic/evalboards/TMC2100_eval.py | 7 ----- pytrinamic/evalboards/TMC2130_eval.py | 7 ----- pytrinamic/evalboards/TMC2160_eval.py | 7 ----- pytrinamic/evalboards/TMC2208_eval.py | 7 ----- pytrinamic/evalboards/TMC2209_eval.py | 7 ----- pytrinamic/evalboards/TMC2224_eval.py | 7 ----- pytrinamic/evalboards/TMC2225_eval.py | 7 ----- pytrinamic/evalboards/TMC2300_eval.py | 7 ----- pytrinamic/evalboards/TMC2590_eval.py | 7 ----- pytrinamic/evalboards/TMC2660_eval.py | 7 ----- pytrinamic/evalboards/TMC4361_eval.py | 7 ----- pytrinamic/evalboards/TMC4671_eval.py | 7 ----- pytrinamic/evalboards/TMC5031_eval.py | 7 ----- pytrinamic/evalboards/TMC5041_eval.py | 7 ----- pytrinamic/evalboards/TMC5062_eval.py | 7 ----- pytrinamic/evalboards/TMC5072_eval.py | 7 ----- pytrinamic/evalboards/TMC5130_eval.py | 7 ----- pytrinamic/evalboards/TMC5160_eval.py | 7 ----- pytrinamic/evalboards/TMC5160_shield.py | 7 ----- pytrinamic/evalboards/TMC6100_eval.py | 7 ----- pytrinamic/evalboards/TMC6200_eval.py | 7 ----- pytrinamic/evalboards/TMC7300_eval.py | 7 ----- pytrinamic/evalboards/tmcl_eval.py | 37 +++++++++++++++++++++++++ 23 files changed, 37 insertions(+), 154 deletions(-) diff --git a/pytrinamic/evalboards/TMC2100_eval.py b/pytrinamic/evalboards/TMC2100_eval.py index c6e9dba1..03b55a19 100644 --- a/pytrinamic/evalboards/TMC2100_eval.py +++ b/pytrinamic/evalboards/TMC2100_eval.py @@ -41,13 +41,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion Control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC2130_eval.py b/pytrinamic/evalboards/TMC2130_eval.py index 95644772..8f782297 100644 --- a/pytrinamic/evalboards/TMC2130_eval.py +++ b/pytrinamic/evalboards/TMC2130_eval.py @@ -40,13 +40,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC2160_eval.py b/pytrinamic/evalboards/TMC2160_eval.py index 4443db67..7451f954 100644 --- a/pytrinamic/evalboards/TMC2160_eval.py +++ b/pytrinamic/evalboards/TMC2160_eval.py @@ -40,13 +40,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC2208_eval.py b/pytrinamic/evalboards/TMC2208_eval.py index 715404ca..6fc48141 100644 --- a/pytrinamic/evalboards/TMC2208_eval.py +++ b/pytrinamic/evalboards/TMC2208_eval.py @@ -40,13 +40,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, axis, value): diff --git a/pytrinamic/evalboards/TMC2209_eval.py b/pytrinamic/evalboards/TMC2209_eval.py index b71a876f..92ec2a4f 100644 --- a/pytrinamic/evalboards/TMC2209_eval.py +++ b/pytrinamic/evalboards/TMC2209_eval.py @@ -40,13 +40,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC2224_eval.py b/pytrinamic/evalboards/TMC2224_eval.py index 65d0cce9..da39a9d7 100644 --- a/pytrinamic/evalboards/TMC2224_eval.py +++ b/pytrinamic/evalboards/TMC2224_eval.py @@ -41,13 +41,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC2225_eval.py b/pytrinamic/evalboards/TMC2225_eval.py index c58c08ee..67cd2e62 100644 --- a/pytrinamic/evalboards/TMC2225_eval.py +++ b/pytrinamic/evalboards/TMC2225_eval.py @@ -41,13 +41,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC2300_eval.py b/pytrinamic/evalboards/TMC2300_eval.py index 25068bac..ea00900d 100644 --- a/pytrinamic/evalboards/TMC2300_eval.py +++ b/pytrinamic/evalboards/TMC2300_eval.py @@ -41,13 +41,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC2590_eval.py b/pytrinamic/evalboards/TMC2590_eval.py index a11979a5..4173e20e 100644 --- a/pytrinamic/evalboards/TMC2590_eval.py +++ b/pytrinamic/evalboards/TMC2590_eval.py @@ -41,13 +41,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC2660_eval.py b/pytrinamic/evalboards/TMC2660_eval.py index 63c43e57..1f10e9f5 100644 --- a/pytrinamic/evalboards/TMC2660_eval.py +++ b/pytrinamic/evalboards/TMC2660_eval.py @@ -41,13 +41,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC4361_eval.py b/pytrinamic/evalboards/TMC4361_eval.py index a76b2e1d..6e535cd7 100644 --- a/pytrinamic/evalboards/TMC4361_eval.py +++ b/pytrinamic/evalboards/TMC4361_eval.py @@ -25,13 +25,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_mc(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC4671_eval.py b/pytrinamic/evalboards/TMC4671_eval.py index 96624b8a..b9a3b313 100644 --- a/pytrinamic/evalboards/TMC4671_eval.py +++ b/pytrinamic/evalboards/TMC4671_eval.py @@ -21,13 +21,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_mc(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - class _MotorTypeA(MotorControlModule): def __init__(self, eval_board, axis): MotorControlModule.__init__(self, eval_board, axis, self.AP) diff --git a/pytrinamic/evalboards/TMC5031_eval.py b/pytrinamic/evalboards/TMC5031_eval.py index c47f26d5..18936f98 100644 --- a/pytrinamic/evalboards/TMC5031_eval.py +++ b/pytrinamic/evalboards/TMC5031_eval.py @@ -25,13 +25,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_mc(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC5041_eval.py b/pytrinamic/evalboards/TMC5041_eval.py index a4305135..ff05843e 100644 --- a/pytrinamic/evalboards/TMC5041_eval.py +++ b/pytrinamic/evalboards/TMC5041_eval.py @@ -21,13 +21,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_mc(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC5062_eval.py b/pytrinamic/evalboards/TMC5062_eval.py index 6ecd07b1..b0166e15 100644 --- a/pytrinamic/evalboards/TMC5062_eval.py +++ b/pytrinamic/evalboards/TMC5062_eval.py @@ -21,13 +21,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_mc(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC5072_eval.py b/pytrinamic/evalboards/TMC5072_eval.py index 34cc6aa2..b3d8f909 100644 --- a/pytrinamic/evalboards/TMC5072_eval.py +++ b/pytrinamic/evalboards/TMC5072_eval.py @@ -34,13 +34,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_mc(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC5130_eval.py b/pytrinamic/evalboards/TMC5130_eval.py index 05789bc4..2f85ff25 100644 --- a/pytrinamic/evalboards/TMC5130_eval.py +++ b/pytrinamic/evalboards/TMC5130_eval.py @@ -34,13 +34,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_mc(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC5160_eval.py b/pytrinamic/evalboards/TMC5160_eval.py index 51ec7167..88f87555 100644 --- a/pytrinamic/evalboards/TMC5160_eval.py +++ b/pytrinamic/evalboards/TMC5160_eval.py @@ -40,13 +40,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_mc(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC5160_shield.py b/pytrinamic/evalboards/TMC5160_shield.py index 82b8a309..ac4f9dc2 100644 --- a/pytrinamic/evalboards/TMC5160_shield.py +++ b/pytrinamic/evalboards/TMC5160_shield.py @@ -47,13 +47,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_register(register_address, TMCLCommand.READ_MC, self.__channel, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/TMC6100_eval.py b/pytrinamic/evalboards/TMC6100_eval.py index f4d3d22e..225db58a 100644 --- a/pytrinamic/evalboards/TMC6100_eval.py +++ b/pytrinamic/evalboards/TMC6100_eval.py @@ -18,10 +18,3 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) diff --git a/pytrinamic/evalboards/TMC6200_eval.py b/pytrinamic/evalboards/TMC6200_eval.py index cbee16c1..3fa1bcb4 100644 --- a/pytrinamic/evalboards/TMC6200_eval.py +++ b/pytrinamic/evalboards/TMC6200_eval.py @@ -18,10 +18,3 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) diff --git a/pytrinamic/evalboards/TMC7300_eval.py b/pytrinamic/evalboards/TMC7300_eval.py index 9931b804..e8f23d99 100644 --- a/pytrinamic/evalboards/TMC7300_eval.py +++ b/pytrinamic/evalboards/TMC7300_eval.py @@ -41,13 +41,6 @@ def write_register(self, register_address, value): def read_register(self, register_address, signed=False): return self._connection.read_drv(register_address, self._module_id, signed) - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) - # Motion control functions def rotate(self, motor, value): diff --git a/pytrinamic/evalboards/tmcl_eval.py b/pytrinamic/evalboards/tmcl_eval.py index cecfe23b..1d3f524e 100644 --- a/pytrinamic/evalboards/tmcl_eval.py +++ b/pytrinamic/evalboards/tmcl_eval.py @@ -1,3 +1,5 @@ +from pytrinamic.helpers import TMC_helpers + class TMCLEval(object): def __init__(self, connection, module_id=1): @@ -41,6 +43,41 @@ def get_axis_parameter(self, ap_type, axis, signed=False): """ return self._connection.get_axis_parameter(ap_type, axis, self._module_id, signed=signed) + def write_register_field(self, field, value): + return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), + field[1], field[2], value)) + + def read_register_field(self, field): + return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) + + def write_axis_field(self, axis, field, value): + """ + Writes the given value to the axis-dependent register field. + On multi-axis ICs, this wraps the process of resolving the actual target + register field to be used for the given axis, when multiple fields with + same meaning for different axes are available. + + Parameters: + field: Base register field for any axis. + value: Value to write to the target register field for this axis. + """ + return self.write_register_field(field[axis] if type(field) == list else field, value) + + def read_axis_field(self, axis, field, signed=False): + """ + Reads the value of the axis-dependent register field. + On multi-axis ICs, this wraps the process of resolving the actual target + register field to be used for the given axis, when multiple fields with + same meaning for different axes are available. + + Parameters: + field: Base register field for any axis. + + Returns: Value of the target register field for the given axis. + """ + value = self.read_register_field(field[axis] if type(field) == list else field) + return TMC_helpers.to_signed_32(value) if signed else value + def __str__(self): return "{} {}".format( self.name, From 3ddaf99649348784df48ae4d57f248d58129862a Mon Sep 17 00:00:00 2001 From: Leonard Kugis Date: Thu, 22 Sep 2022 03:36:06 +0200 Subject: [PATCH 20/51] Implemented Solenoid features and example script for MAX22216-EVAL. --- examples/evalboards/MAX22216/plunger_move.py | 30 ++ pytrinamic/features/solenoid.py | 129 +++++ pytrinamic/features/solenoid_control.py | 23 + pytrinamic/features/solenoid_control_ic.py | 24 + pytrinamic/features/solenoid_ic.py | 256 +++++++++ pytrinamic/ic/MAX22216.py | 517 ++++++++++--------- 6 files changed, 728 insertions(+), 251 deletions(-) create mode 100644 examples/evalboards/MAX22216/plunger_move.py create mode 100644 pytrinamic/features/solenoid.py create mode 100644 pytrinamic/features/solenoid_control.py create mode 100644 pytrinamic/features/solenoid_control_ic.py create mode 100644 pytrinamic/features/solenoid_ic.py diff --git a/examples/evalboards/MAX22216/plunger_move.py b/examples/evalboards/MAX22216/plunger_move.py new file mode 100644 index 00000000..ed576d23 --- /dev/null +++ b/examples/evalboards/MAX22216/plunger_move.py @@ -0,0 +1,30 @@ +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.ic import MAX22216 +from pytrinamic.evalboards import MAX22216_eval +import time + +pytrinamic.show_info() + +with ConnectionManager(debug=True).connect() as my_interface: + print(my_interface) + + eval = MAX22216_eval(my_interface) + ic = eval.ics[0] + solenoid = ic.motors[0] + + solenoid.u_supply = 24.0 # V + solenoid.u_dc_h = 24.0 # V + solenoid.u_dc_l = 0.0 # V + solenoid.u_dc_l2h = 24.0 # V + solenoid.u_dc_h2l = 0.0 # V + solenoid.u_ac = 1.0 # V ampl + solenoid.f_ac = 50.0 # Hz + + solenoid.set_high() + time.sleep(1.0) + solenoid.set_low() + time.sleep(1.0) + + +print("\nDone.") \ No newline at end of file diff --git a/pytrinamic/features/solenoid.py b/pytrinamic/features/solenoid.py new file mode 100644 index 00000000..8b896cca --- /dev/null +++ b/pytrinamic/features/solenoid.py @@ -0,0 +1,129 @@ +from abc import ABC, abstractmethod + +class Solenoid(ABC): + """ + Solenoid feature implementation + """ + def __init__(self, parent, axis): + self._parent = parent + self._axis = axis + + @abstractmethod + def set_voltage_high(self, u_dc_h): + """ + Set high DC voltage. + + Parameters: + u_dc_h: High DC voltage. + """ + raise NotImplementedError + + @abstractmethod + def get_voltage_high(self): + """ + Get high DC voltage. + + Returns: + High DC voltage. + """ + raise NotImplementedError + + @abstractmethod + def set_voltage_low(self, u_dc_l): + """ + Set low DC voltage. + + Parameters: + u_dc_l: Low DC voltage. + """ + raise NotImplementedError + + @abstractmethod + def get_voltage_low(self): + """ + Get low DC voltage. + + Returns: + Low DC voltage. + """ + raise NotImplementedError + + @abstractmethod + def set_voltage_low_high(self, u_dc_l2h): + """ + Set low to high DC voltage. + + Parameters: + u_dc_l2h: Low to high DC voltage. + """ + raise NotImplementedError + + @abstractmethod + def get_voltage_low_high(self): + """ + Get low to high DC voltage. + + Returns: + Low to high DC voltage. + """ + raise NotImplementedError + + @abstractmethod + def set_voltage_high_low(self, u_dc_h2l): + """ + Set high to low DC voltage. + + Parameters: + u_dc_h2l: High to low DC voltage. + """ + raise NotImplementedError + + @abstractmethod + def get_voltage_high_low(self): + """ + Get high to low DC voltage. + + Returns: + High to low DC voltage. + """ + raise NotImplementedError + + @abstractmethod + def set_frequency(self, u_ac_freq): + """ + Set AC frequency. + + Parameters: + u_ac_freq: AC frequency. + """ + raise NotImplementedError + + @abstractmethod + def get_frequency(self): + """ + Get AC frequency. + + Returns: + AC frequency. + """ + raise NotImplementedError + + @abstractmethod + def set_voltage_ac(self, u_ac): + """ + Set AC voltage. + + Parameters: + u_ac: AC voltage. + """ + raise NotImplementedError + + @abstractmethod + def get_voltage_ac(self): + """ + Get AC voltage. + + Returns: + AC voltage. + """ + raise NotImplementedError diff --git a/pytrinamic/features/solenoid_control.py b/pytrinamic/features/solenoid_control.py new file mode 100644 index 00000000..7a5fc2e6 --- /dev/null +++ b/pytrinamic/features/solenoid_control.py @@ -0,0 +1,23 @@ +from abc import ABC, abstractmethod + +class SolenoidControl(ABC): + """ + Solenoid control implementation + """ + def __init__(self, parent, axis): + self._parent = parent + self._axis = axis + + @abstractmethod + def set_high(self): + """ + Apply high voltage. + """ + raise NotImplementedError + + @abstractmethod + def set_low(self): + """ + Apply low voltage. + """ + raise NotImplementedError diff --git a/pytrinamic/features/solenoid_control_ic.py b/pytrinamic/features/solenoid_control_ic.py new file mode 100644 index 00000000..01e677c0 --- /dev/null +++ b/pytrinamic/features/solenoid_control_ic.py @@ -0,0 +1,24 @@ +from pytrinamic.features.solenoid_control import SolenoidControl + +class SolenoidControlIC(SolenoidControl): + """ + Solenoid control implementation for ICs + """ + + def __init__(self, eval_board, ic, axis): + super().__init__(eval_board, axis) + self._ic = ic + + def set_high(self): + """ + Apply high voltage. + This writes 1 to the corresponding CNTL channel field. + """ + self._parent.write_axis_field(self._axis, self._ic.FIELD.CNTL, 1) + + def set_low(self): + """ + Apply low voltage. + This writes 0 to the corresponding CNTL channel field. + """ + self._parent.write_axis_field(self._axis, self._ic.FIELD.CNTL, 0) diff --git a/pytrinamic/features/solenoid_ic.py b/pytrinamic/features/solenoid_ic.py new file mode 100644 index 00000000..904372fe --- /dev/null +++ b/pytrinamic/features/solenoid_ic.py @@ -0,0 +1,256 @@ +from pytrinamic.features.solenoid import Solenoid + +class SolenoidIC(Solenoid): + """ + Solenoid feature implementation for ICs + """ + + __K_CDR = 36700 + + __map_pwm_freq = { + 0: 100E3, + 1: 80E3, + 2: 60E3, + 3: 50E3, + 4: 40E3, + 5: 30E3, + 6: 25E3, + 7: 20E3, + 8: 15E3, + 9: 10E3, + 10: 7.5E3, + 11: 5E3, + 12: 2.5E3 + } + + __map_fsf = { + 0: 1.0, + 1: 2.0 / 3.0, + 2: 1.0 / 3.0 + } + + def __init__(self, eval_board, ic, axis): + super().__init__(eval_board, axis) + self._ic = ic + + def set_voltage_supply(self, u_supply): + """ + Set supply voltage buffer. + This is used to calculate real-world values. + + Parameters: + u_supply: Supply voltage + """ + self.__u_supply = u_supply + + def get_voltage_supply(self): + """ + Get supply voltage buffer. + This is used to calculate real-world values. + + Returns: + Supply voltage + """ + return self.__u_supply + + @staticmethod + def __f_AC(pwm_freq, delta_phi): + return ((pwm_freq * delta_phi) / (2**16)) + + @staticmethod + def __delta_phi(pwm_freq, f_AC): + return ((f_AC * (2**16)) / pwm_freq) + + @staticmethod + def __u_dc_value(u_dc_real, u_supply, vdr, cdr, fsf): + if cdr: + return ((u_dc_real * 0xFFFF) / (SolenoidIC.__K_CDR * 0.000075 * fsf)) + else: + return ((u_dc_real * 0xFFFF) / (36 if vdr else u_supply)) + + @staticmethod + def __u_dc_real(u_dc_value, u_supply, vdr, cdr, fsf): + if cdr: + return ((SolenoidIC.__K_CDR * 0.000075 * fsf * u_dc_value) / 0xFFFF) + else: + return ((u_dc_value * (36 if vdr else u_supply)) / 0xFFFF) + + @staticmethod + def __u_ac_value(u_ac_real, u_supply, vdr): + return ((u_ac_real * 0xFFFF) / (36 if vdr else u_supply)) + + @staticmethod + def __u_ac_real(u_ac_value, u_supply, vdr): + return ((u_ac_value * (36 if vdr else u_supply)) / 0xFFFF) + + def set_voltage_high(self, u_dc_h): + """ + Set high DC voltage. + This value is stored in the DC_H field of the IC. + + Parameters: + u_dc_h: High DC voltage. + """ + vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) + cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) + fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) + self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_H, self.__u_dc_value(u_dc_h, self.__u_supply, vdr, cdr, fsf)) + + def get_voltage_high(self): + """ + Get high DC voltage. + This value is stored in the DC_H field of the IC. + + Returns: + High DC voltage. + """ + vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) + cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) + fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) + return self.__u_dc_real(self._parent.read_axis_field(self._axis, self._ic.FIELD.DC_H), self.__u_supply, vdr, cdr, fsf) + + def set_voltage_low(self, u_dc_l): + """ + Set low DC voltage. + This value is stored in the DC_L field of the IC. + + Parameters: + u_dc_l: Low DC voltage. + """ + vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) + cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) + fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) + self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_L, self.__u_dc_value(u_dc_l, self.__u_supply, vdr, cdr, fsf)) + + def get_voltage_low(self): + """ + Get low DC voltage. + This value is stored in the DC_L field of the IC. + + Returns: + Low DC voltage. + """ + vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) + cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) + fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) + return self.__u_dc_real(self._parent.read_axis_field(self._axis, self._ic.FIELD.DC_L), self.__u_supply, vdr, cdr, fsf) + + def set_voltage_low_high(self, u_dc_l2h): + """ + Set low to high DC voltage. + This value is stored in the DC_L2H field of the IC. + + Parameters: + u_dc_l2h: Low to high DC voltage. + """ + vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) + cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) + fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) + self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_L2H, self.__u_dc_value(u_dc_l2h, self.__u_supply, vdr, cdr, fsf)) + + def get_voltage_low_high(self): + """ + Get low to high DC voltage. + This value is stored in the DC_L2H field of the IC. + + Returns: + Low to high DC voltage. + """ + vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) + cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) + fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) + return self.__u_dc_real(self._parent.read_axis_field(self._axis, self._ic.FIELD.DC_L2H), self.__u_supply, vdr, cdr, fsf) + + def set_voltage_high_low(self, u_dc_h2l): + """ + Set high to low DC voltage. + This value is stored in the DC_H2L field of the IC. + + Parameters: + u_dc_h2l: High to low DC voltage. + """ + vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) + cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) + fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) + self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_H2L, self.__u_dc_value(u_dc_h2l, self.__u_supply, vdr, cdr, fsf)) + + def get_voltage_high_low(self): + """ + Get high to low DC voltage. + This value is stored in the DC_H2L field of the IC. + + Returns: + High to low DC voltage. + """ + vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) + cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) + fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) + return self.__u_dc_real(self._parent.read_axis_field(self._axis, self._ic.FIELD.DC_H2L), self.__u_supply, vdr, cdr, fsf) + + def set_frequency(self, u_ac_freq): + """ + Set AC frequency. + This value is stored in the DELTA_PHI field of the IC. + F_AC = F_PWM_M * (DELTA_PHI/2^16) + + Parameters: + u_ac_freq: AC frequency. + """ + pwm_freq = __map_pwm_freq.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.F_PWM_M), 100E3) + self._parent.write_axis_field(self._axis, self._ic.FIELD.DELTA_PHI, self.__delta_phi(pwm_freq, u_ac_freq)) + + def get_frequency(self): + """ + Get AC frequency. + This value is stored in the DELTA_PHI field of the IC. + F_AC = F_PWM_M * (DELTA_PHI/2^16) + + Returns: + AC frequency. + """ + pwm_freq = __map_pwm_freq.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.F_PWM_M), 100E3) + return self.__f_AC(pwm_freq, self._parent.read_axis_field(self._axis, self._ic.FIELD.DELTA_PHI)) + + def set_voltage_ac(self, u_ac): + """ + Set AC voltage. + This value is stored in the U_AC field of the IC. + + Parameters: + u_ac: AC voltage. + """ + vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) + self._parent.write_axis_field(self._axis, self._ic.FIELD.U_AC, self.__u_ac_value(u_ac, self.__u_supply, vdr)) + + def get_voltage_ac(self): + """ + Get AC voltage. + + Returns: + AC voltage. + """ + vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) + return self.__u_ac_real(self._parent.read_axis_field(self._axis, self._ic.FIELD.U_AC), self.__u_supply, vdr) + + # Properties + u_supply = property(get_voltage_supply, set_voltage_supply) + u_dc_h = property(get_voltage_high, set_voltage_high) + u_dc_l = property(get_voltage_low, set_voltage_low) + u_dc_l2h = property(get_voltage_low_high, set_voltage_low_high) + u_dc_h2l = property(get_voltage_high_low, set_voltage_high_low) + u_ac = property(get_voltage_ac, set_voltage_ac) + f_ac = property(get_frequency, set_frequency) + + def __str__(self): + return "{} {}".format( + "Solenoid", + { + "u_supply": self.u_supply, + "u_dc_h": self.u_dc_h, + "u_dc_l": self.u_dc_l, + "u_dc_l2h": self.u_dc_l2h, + "u_dc_h2l": self.u_dc_h2l, + "u_ac": self.u_ac, + "f_ac": self.f_ac + } + ) diff --git a/pytrinamic/ic/MAX22216.py b/pytrinamic/ic/MAX22216.py index e149c22e..c8585397 100644 --- a/pytrinamic/ic/MAX22216.py +++ b/pytrinamic/ic/MAX22216.py @@ -1,5 +1,7 @@ from ..ic.tmc_ic import TMCIc +from ..features.solenoid_ic import SolenoidIC +from ..features.solenoid_control_ic import SolenoidControlIC class MAX22216(TMCIc): """ @@ -19,14 +21,20 @@ def __init__(self, parent_eval): """ super().__init__("MAX22216", self.__doc__) self._parent = parent_eval - self.motors = [self.MotorTypeA(parent_eval, self, 0)] + self.motors = [ + self._MotorTypeA(parent_eval, self, 0), + self._MotorTypeA(parent_eval, self, 1), + self._MotorTypeA(parent_eval, self, 2), + self._MotorTypeA(parent_eval, self, 3) + ] - class MotorTypeA(object): + class _MotorTypeA(SolenoidIC, SolenoidControlIC): """ Motor class for the generic motor. """ def __init__(self, parent_eval, ic, axis): - pass + SolenoidIC.__init__(self, parent_eval, ic, axis) + SolenoidControlIC.__init__(self, parent_eval, ic, axis) class REG: """ @@ -151,251 +159,258 @@ class FIELD: it. This allows the user to see the corresponding register name of a field without opening this file and searching for the definition. """ - CNTL0 = ( 0x00, 0x00000001, 0 ) - CNTL1 = ( 0x00, 0x00000002, 1 ) - CNTL2 = ( 0x00, 0x00000004, 2 ) - CNTL3 = ( 0x00, 0x00000008, 3 ) - F_PWM_M = ( 0x00, 0x000000F0, 4 ) - CHS = ( 0x01, 0x0000000F, 0 ) - VDR_NDUTY = ( 0x01, 0x00000010, 4 ) - STAT_POL = ( 0x01, 0x00000040, 6 ) - CNTL_POL = ( 0x01, 0x00000080, 7 ) - M_UVM = ( 0x01, 0x00000100, 8 ) - M_COMF = ( 0x01, 0x00000200, 9 ) - M_DPM = ( 0x01, 0x00000400, 10 ) - M_HHF = ( 0x01, 0x00000800, 11 ) - M_OLF = ( 0x01, 0x00001000, 12 ) - M_OCP = ( 0x01, 0x00002000, 13 ) - M_OVT = ( 0x01, 0x00004000, 14 ) - ACTIVE = ( 0x01, 0x00008000, 15 ) - RFU = ( 0x02, 0x00000001, 0 ) - UVM = ( 0x02, 0x00000002, 1 ) - COMER = ( 0x02, 0x00000004, 2 ) - DPM = ( 0x02, 0x00000008, 3 ) - HHF = ( 0x02, 0x00000010, 4 ) - OLF = ( 0x02, 0x00000020, 5 ) - OCP = ( 0x02, 0x00000040, 6 ) - OVT = ( 0x02, 0x00000080, 7 ) - IND = ( 0x02, 0x00000100, 8 ) - RES = ( 0x02, 0x00000200, 9 ) - MIN_T_ON = ( 0x02, 0x00000400, 10 ) - STAT0 = ( 0x02, 0x00000800, 11 ) - STAT1 = ( 0x02, 0x00001000, 12 ) - STAT2 = ( 0x02, 0x00002000, 13 ) - STAT3 = ( 0x02, 0x00004000, 14 ) - STAT_FUN = ( 0x03, 0x00000007, 0 ) - STAT_SEL0 = ( 0x03, 0x00000008, 3 ) - STAT_SEL1 = ( 0x03, 0x00000010, 4 ) - STRETCH_EN = ( 0x03, 0x00000060, 5 ) - EN_LDO = ( 0x03, 0x00000080, 7 ) - V5_NV3 = ( 0x03, 0x00000100, 8 ) - M_UVM_CMP = ( 0x03, 0x00000200, 9 ) - DC_H2L = ( 0x04, 0x0000FFFF, 0 ) - ADC_VM_RAW = ( 0x05, 0x00001FFF, 0 ) - VM_THLD_DOWN = ( 0x06, 0x0000000F, 0 ) - VM_THLD_UP = ( 0x06, 0x000000F0, 4 ) - DELTA_PHI = ( 0x07, 0x00000FFF, 0 ) - U_AC = ( 0x08, 0x00007FFF, 0 ) - DC_L2H = ( 0x09, 0x0000FFFF, 0 ) - DC_H = ( 0x0A, 0x0000FFFF, 0 ) - DC_L = ( 0x0B, 0x0000FFFF, 0 ) - TIME_L2H = ( 0x0C, 0x0000FFFF, 0 ) - RAMP = ( 0x0D, 0x000000FF, 0 ) - RUPE = ( 0x0D, 0x00000100, 8 ) - RMDE = ( 0x0D, 0x00000200, 9 ) - RDWE = ( 0x0D, 0x00000400, 10 ) - H2L_EN = ( 0x0D, 0x00000800, 11 ) - OL_EN = ( 0x0D, 0x00001000, 12 ) - HHF_EN = ( 0x0D, 0x00002000, 13 ) - CTRL_MODE = ( 0x0D, 0x0000C000, 14 ) - FSF = ( 0x0E, 0x00000003, 0 ) - SHUNT_SCALE = ( 0x0E, 0x0000000C, 2 ) - SLEW_RATE = ( 0x0E, 0x00000030, 4 ) - T_BLANKING = ( 0x0E, 0x000000C0, 6 ) - F_PWM = ( 0x0E, 0x00000300, 8 ) - HSNLS = ( 0x0E, 0x00000400, 10 ) - DPM_THLD = ( 0x0F, 0x00000FFF, 0 ) - DPM_MIN_CURRENT = ( 0x10, 0x000000FF, 0 ) - DPM_MIN_NBR = ( 0x10, 0x00000F00, 8 ) - END_HIT_AUTO = ( 0x10, 0x00001000, 12 ) - END_HIT_START_HIZ_AUTO = ( 0x10, 0x00002000, 13 ) - DPM_EN = ( 0x10, 0x00004000, 14 ) - IDC_THLD = ( 0x11, 0x0000FFFF, 0 ) - RES_THLD = ( 0x12, 0x0000FFFF, 0 ) - L_NBR_CALC = ( 0x13, 0x0000000F, 0 ) - L_MEAS_WCYCLES = ( 0x13, 0x000000F0, 4 ) - L_MEAS_H = ( 0x13, 0x00000100, 8 ) - L_MEAS_L2H = ( 0x13, 0x00000200, 9 ) - L_MEAS_EN = ( 0x13, 0x00000400, 10 ) - DITH_EN = ( 0x13, 0x00000800, 11 ) - IAC_THLD = ( 0x14, 0x00000FFF, 0 ) - CFG_P = ( 0x15, 0x0000FFFF, 0 ) - CFG_I = ( 0x16, 0x0000FFFF, 0 ) - #DC_L2H = ( 0x17, 0x0000FFFF, 0 ) - #DC_H = ( 0x18, 0x0000FFFF, 0 ) - #DC_L = ( 0x19, 0x0000FFFF, 0 ) - #TIME_L2H = ( 0x1A, 0x0000FFFF, 0 ) - #RAMP = ( 0x1B, 0x000000FF, 0 ) - #RUPE = ( 0x1B, 0x00000100, 8 ) - #RMDE = ( 0x1B, 0x00000200, 9 ) - #RDWE = ( 0x1B, 0x00000400, 10 ) - #H2L_EN = ( 0x1B, 0x00000800, 11 ) - #OL_EN = ( 0x1B, 0x00001000, 12 ) - #HHF_EN = ( 0x1B, 0x00002000, 13 ) - #CTRL_MODE = ( 0x1B, 0x0000C000, 14 ) - #FSF = ( 0x1C, 0x00000003, 0 ) - #SHUNT_SCALE = ( 0x1C, 0x0000000C, 2 ) - #SLEW_RATE = ( 0x1C, 0x00000030, 4 ) - #T_BLANKING = ( 0x1C, 0x000000C0, 6 ) - #F_PWM = ( 0x1C, 0x00000300, 8 ) - #HSNLS = ( 0x1C, 0x00000400, 10 ) - #DPM_THLD = ( 0x1D, 0x00000FFF, 0 ) - #DPM_MIN_CURRENT = ( 0x1E, 0x000000FF, 0 ) - #DPM_MIN_NBR = ( 0x1E, 0x00000F00, 8 ) - #END_HIT_AUTO = ( 0x1E, 0x00001000, 12 ) - #END_HIT_START_HIZ_AUTO = ( 0x1E, 0x00002000, 13 ) - #DPM_EN = ( 0x1E, 0x00004000, 14 ) - #IDC_THLD = ( 0x1F, 0x0000FFFF, 0 ) - #RES_THLD = ( 0x20, 0x0000FFFF, 0 ) - #L_NBR_CALC = ( 0x21, 0x0000000F, 0 ) - #L_MEAS_WCYCLES = ( 0x21, 0x000000F0, 4 ) - #L_MEAS_H = ( 0x21, 0x00000100, 8 ) - #L_MEAS_L2H = ( 0x21, 0x00000200, 9 ) - #L_MEAS_EN = ( 0x21, 0x00000400, 10 ) - #DITH_EN = ( 0x21, 0x00000800, 11 ) - #IAC_THLD = ( 0x22, 0x00000FFF, 0 ) - #CFG_P = ( 0x23, 0x0000FFFF, 0 ) - #CFG_I = ( 0x24, 0x0000FFFF, 0 ) - #DC_L2H = ( 0x25, 0x0000FFFF, 0 ) - #DC_H = ( 0x26, 0x0000FFFF, 0 ) - #DC_L = ( 0x27, 0x0000FFFF, 0 ) - #TIME_L2H = ( 0x28, 0x0000FFFF, 0 ) - #RAMP = ( 0x29, 0x000000FF, 0 ) - #RUPE = ( 0x29, 0x00000100, 8 ) - #RMDE = ( 0x29, 0x00000200, 9 ) - #RDWE = ( 0x29, 0x00000400, 10 ) - #H2L_EN = ( 0x29, 0x00000800, 11 ) - #OL_EN = ( 0x29, 0x00001000, 12 ) - #HHF_EN = ( 0x29, 0x00002000, 13 ) - #CTRL_MODE = ( 0x29, 0x0000C000, 14 ) - #FSF = ( 0x2A, 0x00000003, 0 ) - #SHUNT_SCALE = ( 0x2A, 0x0000000C, 2 ) - #SLEW_RATE = ( 0x2A, 0x00000030, 4 ) - #T_BLANKING = ( 0x2A, 0x000000C0, 6 ) - #F_PWM = ( 0x2A, 0x00000300, 8 ) - #HSNLS = ( 0x2A, 0x00000400, 10 ) - #DPM_THLD = ( 0x2B, 0x00000FFF, 0 ) - #DPM_MIN_CURRENT = ( 0x2C, 0x000000FF, 0 ) - #DPM_MIN_NBR = ( 0x2C, 0x00000F00, 8 ) - #END_HIT_AUTO = ( 0x2C, 0x00001000, 12 ) - #END_HIT_START_HIZ_AUTO = ( 0x2C, 0x00002000, 13 ) - #DPM_EN = ( 0x2C, 0x00004000, 14 ) - #IDC_THLD = ( 0x2D, 0x0000FFFF, 0 ) - #RES_THLD = ( 0x2E, 0x0000FFFF, 0 ) - #L_NBR_CALC = ( 0x2F, 0x0000000F, 0 ) - #L_MEAS_WCYCLES = ( 0x2F, 0x000000F0, 4 ) - #L_MEAS_H = ( 0x2F, 0x00000100, 8 ) - #L_MEAS_L2H = ( 0x2F, 0x00000200, 9 ) - #L_MEAS_EN = ( 0x2F, 0x00000400, 10 ) - #DITH_EN = ( 0x2F, 0x00000800, 11 ) - #IAC_THLD = ( 0x30, 0x00000FFF, 0 ) - #CFG_P = ( 0x31, 0x0000FFFF, 0 ) - #CFG_I = ( 0x32, 0x0000FFFF, 0 ) - #DC_L2H = ( 0x33, 0x0000FFFF, 0 ) - #DC_H = ( 0x34, 0x0000FFFF, 0 ) - #DC_L = ( 0x35, 0x0000FFFF, 0 ) - #TIME_L2H = ( 0x36, 0x0000FFFF, 0 ) - #RAMP = ( 0x37, 0x000000FF, 0 ) - #RUPE = ( 0x37, 0x00000100, 8 ) - #RMDE = ( 0x37, 0x00000200, 9 ) - #RDWE = ( 0x37, 0x00000400, 10 ) - #H2L_EN = ( 0x37, 0x00000800, 11 ) - #OL_EN = ( 0x37, 0x00001000, 12 ) - #HHF_EN = ( 0x37, 0x00002000, 13 ) - #CTRL_MODE = ( 0x37, 0x0000C000, 14 ) - #FSF = ( 0x38, 0x00000003, 0 ) - #SHUNT_SCALE = ( 0x38, 0x0000000C, 2 ) - #SLEW_RATE = ( 0x38, 0x00000030, 4 ) - #T_BLANKING = ( 0x38, 0x000000C0, 6 ) - #F_PWM = ( 0x38, 0x00000300, 8 ) - #HSNLS = ( 0x38, 0x00000400, 10 ) - #DPM_THLD = ( 0x39, 0x00000FFF, 0 ) - #DPM_MIN_CURRENT = ( 0x3A, 0x000000FF, 0 ) - #DPM_MIN_NBR = ( 0x3A, 0x00000F00, 8 ) - #END_HIT_AUTO = ( 0x3A, 0x00001000, 12 ) - #END_HIT_START_HIZ_AUTO = ( 0x3A, 0x00002000, 13 ) - #DPM_EN = ( 0x3A, 0x00004000, 14 ) - #IDC_THLD = ( 0x3B, 0x0000FFFF, 0 ) - #RES_THLD = ( 0x3C, 0x0000FFFF, 0 ) - #L_NBR_CALC = ( 0x3D, 0x0000000F, 0 ) - #L_MEAS_WCYCLES = ( 0x3D, 0x000000F0, 4 ) - #L_MEAS_H = ( 0x3D, 0x00000100, 8 ) - #L_MEAS_L2H = ( 0x3D, 0x00000200, 9 ) - #L_MEAS_EN = ( 0x3D, 0x00000400, 10 ) - #DITH_EN = ( 0x3D, 0x00000800, 11 ) - #IAC_THLD = ( 0x3E, 0x00000FFF, 0 ) - #CFG_P = ( 0x3F, 0x0000FFFF, 0 ) - #CFG_I = ( 0x40, 0x0000FFFF, 0 ) - I_DPM_PEAK = ( 0x41, 0x0000FFFF, 0 ) - I_DPM_VALLEY = ( 0x42, 0x0000FFFF, 0 ) - TRAVEL_TIME = ( 0x43, 0x0000FFFF, 0 ) - REACTION_TIME = ( 0x44, 0x0000FFFF, 0 ) - I_MONITOR = ( 0x45, 0x0000FFFF, 0 ) - I_DC = ( 0x46, 0x0000FFFF, 0 ) - I_AC = ( 0x47, 0x0000FFFF, 0 ) - R = ( 0x48, 0x0000FFFF, 0 ) - PWM_DUTYCYCLE = ( 0x49, 0x0000FFFF, 0 ) - #I_DPM_PEAK = ( 0x4A, 0x0000FFFF, 0 ) - #I_DPM_VALLEY = ( 0x4B, 0x0000FFFF, 0 ) - #TRAVEL_TIME = ( 0x4C, 0x0000FFFF, 0 ) - #REACTION_TIME = ( 0x4D, 0x0000FFFF, 0 ) - #I_MONITOR = ( 0x4E, 0x0000FFFF, 0 ) - #I_DC = ( 0x4F, 0x0000FFFF, 0 ) - #I_AC = ( 0x50, 0x0000FFFF, 0 ) - #R = ( 0x51, 0x0000FFFF, 0 ) - #PWM_DUTYCYCLE = ( 0x52, 0x0000FFFF, 0 ) - #I_DPM_PEAK = ( 0x53, 0x0000FFFF, 0 ) - #I_DPM_VALLEY = ( 0x54, 0x0000FFFF, 0 ) - #TRAVEL_TIME = ( 0x55, 0x0000FFFF, 0 ) - #REACTION_TIME = ( 0x56, 0x0000FFFF, 0 ) - #I_MONITOR = ( 0x57, 0x0000FFFF, 0 ) - #I_DC = ( 0x58, 0x0000FFFF, 0 ) - #I_AC = ( 0x59, 0x0000FFFF, 0 ) - #R = ( 0x5A, 0x0000FFFF, 0 ) - #PWM_DUTYCYCLE = ( 0x5B, 0x0000FFFF, 0 ) - #I_DPM_PEAK = ( 0x5C, 0x0000FFFF, 0 ) - #I_DPM_VALLEY = ( 0x5D, 0x0000FFFF, 0 ) - #TRAVEL_TIME = ( 0x5E, 0x0000FFFF, 0 ) - #REACTION_TIME = ( 0x5F, 0x0000FFFF, 0 ) - #I_MONITOR = ( 0x60, 0x0000FFFF, 0 ) - #I_DC = ( 0x61, 0x0000FFFF, 0 ) - #I_AC = ( 0x62, 0x0000FFFF, 0 ) - #R = ( 0x63, 0x0000FFFF, 0 ) - #PWM_DUTYCYCLE = ( 0x64, 0x0000FFFF, 0 ) - OCP0 = ( 0x65, 0x00000001, 0 ) - OCP1 = ( 0x65, 0x00000002, 1 ) - OCP2 = ( 0x65, 0x00000004, 2 ) - OCP3 = ( 0x65, 0x00000008, 3 ) - HHF0 = ( 0x65, 0x00000010, 4 ) - HHF1 = ( 0x65, 0x00000020, 5 ) - HHF2 = ( 0x65, 0x00000040, 6 ) - HHF3 = ( 0x65, 0x00000080, 7 ) - OLF0 = ( 0x65, 0x00000100, 8 ) - OLF1 = ( 0x65, 0x00000200, 9 ) - OLF2 = ( 0x65, 0x00000400, 10 ) - OLF3 = ( 0x65, 0x00000800, 11 ) - DPM0 = ( 0x65, 0x00001000, 12 ) - DPM1 = ( 0x65, 0x00002000, 13 ) - DPM2 = ( 0x65, 0x00004000, 14 ) - DPM3 = ( 0x65, 0x00008000, 15 ) - IND0 = ( 0x66, 0x00000001, 0 ) - IND1 = ( 0x66, 0x00000002, 1 ) - IND2 = ( 0x66, 0x00000004, 2 ) - IND3 = ( 0x66, 0x00000008, 3 ) - #UVM = ( 0x66, 0x00000010, 4 ) - #COMER = ( 0x66, 0x00000020, 5 ) - #OVT = ( 0x66, 0x00000040, 6 ) - RES0 = ( 0x66, 0x00000080, 7 ) - RES1 = ( 0x66, 0x00000100, 8 ) - RES2 = ( 0x66, 0x00000200, 9 ) - RES3 = ( 0x66, 0x00000400, 10 ) + CNTL0 = ( 0x00, 0x00000001, 0 ) + CNTL1 = ( 0x00, 0x00000002, 1 ) + CNTL2 = ( 0x00, 0x00000004, 2 ) + CNTL3 = ( 0x00, 0x00000008, 3 ) + F_PWM_M = ( 0x00, 0x000000F0, 4 ) + CHS = ( 0x01, 0x0000000F, 0 ) + VDR_NDUTY = ( 0x01, 0x00000010, 4 ) + STAT_POL = ( 0x01, 0x00000040, 6 ) + CNTL_POL = ( 0x01, 0x00000080, 7 ) + M_UVM = ( 0x01, 0x00000100, 8 ) + M_COMF = ( 0x01, 0x00000200, 9 ) + M_DPM = ( 0x01, 0x00000400, 10 ) + M_HHF = ( 0x01, 0x00000800, 11 ) + M_OLF = ( 0x01, 0x00001000, 12 ) + M_OCP = ( 0x01, 0x00002000, 13 ) + M_OVT = ( 0x01, 0x00004000, 14 ) + ACTIVE = ( 0x01, 0x00008000, 15 ) + RFU = ( 0x02, 0x00000001, 0 ) + UVM = ( 0x02, 0x00000002, 1 ) + COMER = ( 0x02, 0x00000004, 2 ) + DPM = ( 0x02, 0x00000008, 3 ) + HHF = ( 0x02, 0x00000010, 4 ) + OLF = ( 0x02, 0x00000020, 5 ) + OCP = ( 0x02, 0x00000040, 6 ) + OVT = ( 0x02, 0x00000080, 7 ) + IND = ( 0x02, 0x00000100, 8 ) + RES = ( 0x02, 0x00000200, 9 ) + MIN_T_ON = ( 0x02, 0x00000400, 10 ) + STAT0 = ( 0x02, 0x00000800, 11 ) + STAT1 = ( 0x02, 0x00001000, 12 ) + STAT2 = ( 0x02, 0x00002000, 13 ) + STAT3 = ( 0x02, 0x00004000, 14 ) + STAT_FUN = ( 0x03, 0x00000007, 0 ) + STAT_SEL0 = ( 0x03, 0x00000008, 3 ) + STAT_SEL1 = ( 0x03, 0x00000010, 4 ) + STRETCH_EN = ( 0x03, 0x00000060, 5 ) + EN_LDO = ( 0x03, 0x00000080, 7 ) + V5_NV3 = ( 0x03, 0x00000100, 8 ) + M_UVM_CMP = ( 0x03, 0x00000200, 9 ) + DC_H2L = ( 0x04, 0x0000FFFF, 0 ) + ADC_VM_RAW = ( 0x05, 0x00001FFF, 0 ) + VM_THLD_DOWN = ( 0x06, 0x0000000F, 0 ) + VM_THLD_UP = ( 0x06, 0x000000F0, 4 ) + DELTA_PHI = ( 0x07, 0x00000FFF, 0 ) + U_AC = ( 0x08, 0x00007FFF, 0 ) + DC_L2H_0 = ( 0x09, 0x0000FFFF, 0 ) + DC_H_0 = ( 0x0A, 0x0000FFFF, 0 ) + DC_L_0 = ( 0x0B, 0x0000FFFF, 0 ) + TIME_L2H_0 = ( 0x0C, 0x0000FFFF, 0 ) + RAMP_0 = ( 0x0D, 0x000000FF, 0 ) + RUPE_0 = ( 0x0D, 0x00000100, 8 ) + RMDE_0 = ( 0x0D, 0x00000200, 9 ) + RDWE_0 = ( 0x0D, 0x00000400, 10 ) + H2L_EN_0 = ( 0x0D, 0x00000800, 11 ) + OL_EN_0 = ( 0x0D, 0x00001000, 12 ) + HHF_EN_0 = ( 0x0D, 0x00002000, 13 ) + CTRL_MODE_0 = ( 0x0D, 0x0000C000, 14 ) + FSF_0 = ( 0x0E, 0x00000003, 0 ) + SHUNT_SCALE_0 = ( 0x0E, 0x0000000C, 2 ) + SLEW_RATE_0 = ( 0x0E, 0x00000030, 4 ) + T_BLANKING_0 = ( 0x0E, 0x000000C0, 6 ) + F_PWM_0 = ( 0x0E, 0x00000300, 8 ) + HSNLS_0 = ( 0x0E, 0x00000400, 10 ) + DPM_THLD_0 = ( 0x0F, 0x00000FFF, 0 ) + DPM_MIN_CURRENT_0 = ( 0x10, 0x000000FF, 0 ) + DPM_MIN_NBR_0 = ( 0x10, 0x00000F00, 8 ) + END_HIT_AUTO_0 = ( 0x10, 0x00001000, 12 ) + END_HIT_START_HIZ_AUTO_0 = ( 0x10, 0x00002000, 13 ) + DPM_EN_0 = ( 0x10, 0x00004000, 14 ) + IDC_THLD_0 = ( 0x11, 0x0000FFFF, 0 ) + RES_THLD_0 = ( 0x12, 0x0000FFFF, 0 ) + L_NBR_CALC_0 = ( 0x13, 0x0000000F, 0 ) + L_MEAS_WCYCLES_0 = ( 0x13, 0x000000F0, 4 ) + L_MEAS_H_0 = ( 0x13, 0x00000100, 8 ) + L_MEAS_L2H_0 = ( 0x13, 0x00000200, 9 ) + L_MEAS_EN_0 = ( 0x13, 0x00000400, 10 ) + DITH_EN_0 = ( 0x13, 0x00000800, 11 ) + IAC_THLD_0 = ( 0x14, 0x00000FFF, 0 ) + CFG_P_0 = ( 0x15, 0x0000FFFF, 0 ) + CFG_I_0 = ( 0x16, 0x0000FFFF, 0 ) + DC_L2H_1 = ( 0x17, 0x0000FFFF, 0 ) + DC_H_1 = ( 0x18, 0x0000FFFF, 0 ) + DC_L_1 = ( 0x19, 0x0000FFFF, 0 ) + TIME_L2H_1 = ( 0x1A, 0x0000FFFF, 0 ) + RAMP_1 = ( 0x1B, 0x000000FF, 0 ) + RUPE_1 = ( 0x1B, 0x00000100, 8 ) + RMDE_1 = ( 0x1B, 0x00000200, 9 ) + RDWE_1 = ( 0x1B, 0x00000400, 10 ) + H2L_EN_1 = ( 0x1B, 0x00000800, 11 ) + OL_EN_1 = ( 0x1B, 0x00001000, 12 ) + HHF_EN_1 = ( 0x1B, 0x00002000, 13 ) + CTRL_MODE_1 = ( 0x1B, 0x0000C000, 14 ) + FSF_1 = ( 0x1C, 0x00000003, 0 ) + SHUNT_SCALE_1 = ( 0x1C, 0x0000000C, 2 ) + SLEW_RATE_1 = ( 0x1C, 0x00000030, 4 ) + T_BLANKING_1 = ( 0x1C, 0x000000C0, 6 ) + F_PWM_1 = ( 0x1C, 0x00000300, 8 ) + HSNLS_1 = ( 0x1C, 0x00000400, 10 ) + DPM_THLD_1 = ( 0x1D, 0x00000FFF, 0 ) + DPM_MIN_CURRENT_1 = ( 0x1E, 0x000000FF, 0 ) + DPM_MIN_NBR_1 = ( 0x1E, 0x00000F00, 8 ) + END_HIT_AUTO_1 = ( 0x1E, 0x00001000, 12 ) + END_HIT_START_HIZ_AUTO_1 = ( 0x1E, 0x00002000, 13 ) + DPM_EN_1 = ( 0x1E, 0x00004000, 14 ) + IDC_THLD_1 = ( 0x1F, 0x0000FFFF, 0 ) + RES_THLD_1 = ( 0x20, 0x0000FFFF, 0 ) + L_NBR_CALC_1 = ( 0x21, 0x0000000F, 0 ) + L_MEAS_WCYCLES_1 = ( 0x21, 0x000000F0, 4 ) + L_MEAS_H_1 = ( 0x21, 0x00000100, 8 ) + L_MEAS_L2H_1 = ( 0x21, 0x00000200, 9 ) + L_MEAS_EN_1 = ( 0x21, 0x00000400, 10 ) + DITH_EN_1 = ( 0x21, 0x00000800, 11 ) + IAC_THLD_1 = ( 0x22, 0x00000FFF, 0 ) + CFG_P_1 = ( 0x23, 0x0000FFFF, 0 ) + CFG_I_1 = ( 0x24, 0x0000FFFF, 0 ) + DC_L2H_2 = ( 0x25, 0x0000FFFF, 0 ) + DC_H_2 = ( 0x26, 0x0000FFFF, 0 ) + DC_L_2 = ( 0x27, 0x0000FFFF, 0 ) + TIME_L2H_2 = ( 0x28, 0x0000FFFF, 0 ) + RAMP_2 = ( 0x29, 0x000000FF, 0 ) + RUPE_2 = ( 0x29, 0x00000100, 8 ) + RMDE_2 = ( 0x29, 0x00000200, 9 ) + RDWE_2 = ( 0x29, 0x00000400, 10 ) + H2L_EN_2 = ( 0x29, 0x00000800, 11 ) + OL_EN_2 = ( 0x29, 0x00001000, 12 ) + HHF_EN_2 = ( 0x29, 0x00002000, 13 ) + CTRL_MODE_2 = ( 0x29, 0x0000C000, 14 ) + FSF_2 = ( 0x2A, 0x00000003, 0 ) + SHUNT_SCALE_2 = ( 0x2A, 0x0000000C, 2 ) + SLEW_RATE_2 = ( 0x2A, 0x00000030, 4 ) + T_BLANKING_2 = ( 0x2A, 0x000000C0, 6 ) + F_PWM_2 = ( 0x2A, 0x00000300, 8 ) + HSNLS_2 = ( 0x2A, 0x00000400, 10 ) + DPM_THLD_2 = ( 0x2B, 0x00000FFF, 0 ) + DPM_MIN_CURRENT_2 = ( 0x2C, 0x000000FF, 0 ) + DPM_MIN_NBR_2 = ( 0x2C, 0x00000F00, 8 ) + END_HIT_AUTO_2 = ( 0x2C, 0x00001000, 12 ) + END_HIT_START_HIZ_AUTO_2 = ( 0x2C, 0x00002000, 13 ) + DPM_EN_2 = ( 0x2C, 0x00004000, 14 ) + IDC_THLD_2 = ( 0x2D, 0x0000FFFF, 0 ) + RES_THLD_2 = ( 0x2E, 0x0000FFFF, 0 ) + L_NBR_CALC_2 = ( 0x2F, 0x0000000F, 0 ) + L_MEAS_WCYCLES_2 = ( 0x2F, 0x000000F0, 4 ) + L_MEAS_H_2 = ( 0x2F, 0x00000100, 8 ) + L_MEAS_L2H_2 = ( 0x2F, 0x00000200, 9 ) + L_MEAS_EN_2 = ( 0x2F, 0x00000400, 10 ) + DITH_EN_2 = ( 0x2F, 0x00000800, 11 ) + IAC_THLD_2 = ( 0x30, 0x00000FFF, 0 ) + CFG_P_2 = ( 0x31, 0x0000FFFF, 0 ) + CFG_I_2 = ( 0x32, 0x0000FFFF, 0 ) + DC_L2H_3 = ( 0x33, 0x0000FFFF, 0 ) + DC_H_3 = ( 0x34, 0x0000FFFF, 0 ) + DC_L_3 = ( 0x35, 0x0000FFFF, 0 ) + TIME_L2H_3 = ( 0x36, 0x0000FFFF, 0 ) + RAMP_3 = ( 0x37, 0x000000FF, 0 ) + RUPE_3 = ( 0x37, 0x00000100, 8 ) + RMDE_3 = ( 0x37, 0x00000200, 9 ) + RDWE_3 = ( 0x37, 0x00000400, 10 ) + H2L_EN_3 = ( 0x37, 0x00000800, 11 ) + OL_EN_3 = ( 0x37, 0x00001000, 12 ) + HHF_EN_3 = ( 0x37, 0x00002000, 13 ) + CTRL_MODE_3 = ( 0x37, 0x0000C000, 14 ) + FSF_3 = ( 0x38, 0x00000003, 0 ) + SHUNT_SCALE_3 = ( 0x38, 0x0000000C, 2 ) + SLEW_RATE_3 = ( 0x38, 0x00000030, 4 ) + T_BLANKING_3 = ( 0x38, 0x000000C0, 6 ) + F_PWM_3 = ( 0x38, 0x00000300, 8 ) + HSNLS_3 = ( 0x38, 0x00000400, 10 ) + DPM_THLD_3 = ( 0x39, 0x00000FFF, 0 ) + DPM_MIN_CURRENT_3 = ( 0x3A, 0x000000FF, 0 ) + DPM_MIN_NBR_3 = ( 0x3A, 0x00000F00, 8 ) + END_HIT_AUTO_3 = ( 0x3A, 0x00001000, 12 ) + END_HIT_START_HIZ_AUTO_3 = ( 0x3A, 0x00002000, 13 ) + DPM_EN_3 = ( 0x3A, 0x00004000, 14 ) + IDC_THLD_3 = ( 0x3B, 0x0000FFFF, 0 ) + RES_THLD_3 = ( 0x3C, 0x0000FFFF, 0 ) + L_NBR_CALC_3 = ( 0x3D, 0x0000000F, 0 ) + L_MEAS_WCYCLES_3 = ( 0x3D, 0x000000F0, 4 ) + L_MEAS_H_3 = ( 0x3D, 0x00000100, 8 ) + L_MEAS_L2H_3 = ( 0x3D, 0x00000200, 9 ) + L_MEAS_EN_3 = ( 0x3D, 0x00000400, 10 ) + DITH_EN_3 = ( 0x3D, 0x00000800, 11 ) + IAC_THLD_3 = ( 0x3E, 0x00000FFF, 0 ) + CFG_P_3 = ( 0x3F, 0x0000FFFF, 0 ) + CFG_I_3 = ( 0x40, 0x0000FFFF, 0 ) + I_DPM_PEAK_0 = ( 0x41, 0x0000FFFF, 0 ) + I_DPM_VALLEY_0 = ( 0x42, 0x0000FFFF, 0 ) + REACTION_TIME_0 = ( 0x43, 0x0000FFFF, 0 ) + #REACTION_TIME_0 = ( 0x44, 0x0000FFFF, 0 ) + I_MONITOR_0 = ( 0x45, 0x0000FFFF, 0 ) + I_DC_0 = ( 0x46, 0x0000FFFF, 0 ) + I_AC_0 = ( 0x47, 0x0000FFFF, 0 ) + R_0 = ( 0x48, 0x0000FFFF, 0 ) + PWM_DUTYCYCLE_0 = ( 0x49, 0x0000FFFF, 0 ) + I_DPM_PEAK_1 = ( 0x4A, 0x0000FFFF, 0 ) + I_DPM_VALLEY_1 = ( 0x4B, 0x0000FFFF, 0 ) + REACTION_TIME_1 = ( 0x4C, 0x0000FFFF, 0 ) + #REACTION_TIME_1 = ( 0x4D, 0x0000FFFF, 0 ) + I_MONITOR_1 = ( 0x4E, 0x0000FFFF, 0 ) + I_DC_1 = ( 0x4F, 0x0000FFFF, 0 ) + I_AC_1 = ( 0x50, 0x0000FFFF, 0 ) + R_1 = ( 0x51, 0x0000FFFF, 0 ) + PWM_DUTYCYCLE_1 = ( 0x52, 0x0000FFFF, 0 ) + I_DPM_PEAK_2 = ( 0x53, 0x0000FFFF, 0 ) + I_DPM_VALLEY_2 = ( 0x54, 0x0000FFFF, 0 ) + REACTION_TIME_2 = ( 0x55, 0x0000FFFF, 0 ) + #REACTION_TIME_2 = ( 0x56, 0x0000FFFF, 0 ) + I_MONITOR_2 = ( 0x57, 0x0000FFFF, 0 ) + I_DC_2 = ( 0x58, 0x0000FFFF, 0 ) + I_AC_2 = ( 0x59, 0x0000FFFF, 0 ) + R_2 = ( 0x5A, 0x0000FFFF, 0 ) + PWM_DUTYCYCLE_2 = ( 0x5B, 0x0000FFFF, 0 ) + I_DPM_PEAK_3 = ( 0x5C, 0x0000FFFF, 0 ) + I_DPM_VALLEY_3 = ( 0x5D, 0x0000FFFF, 0 ) + REACTION_TIME_3 = ( 0x5E, 0x0000FFFF, 0 ) + #REACTION_TIME_3 = ( 0x5F, 0x0000FFFF, 0 ) + I_MONITOR_3 = ( 0x60, 0x0000FFFF, 0 ) + I_DC_3 = ( 0x61, 0x0000FFFF, 0 ) + I_AC_3 = ( 0x62, 0x0000FFFF, 0 ) + R_3 = ( 0x63, 0x0000FFFF, 0 ) + PWM_DUTYCYCLE_3 = ( 0x64, 0x0000FFFF, 0 ) + OCP0 = ( 0x65, 0x00000001, 0 ) + OCP1 = ( 0x65, 0x00000002, 1 ) + OCP2 = ( 0x65, 0x00000004, 2 ) + OCP3 = ( 0x65, 0x00000008, 3 ) + HHF0 = ( 0x65, 0x00000010, 4 ) + HHF1 = ( 0x65, 0x00000020, 5 ) + HHF2 = ( 0x65, 0x00000040, 6 ) + HHF3 = ( 0x65, 0x00000080, 7 ) + OLF0 = ( 0x65, 0x00000100, 8 ) + OLF1 = ( 0x65, 0x00000200, 9 ) + OLF2 = ( 0x65, 0x00000400, 10 ) + OLF3 = ( 0x65, 0x00000800, 11 ) + DPM0 = ( 0x65, 0x00001000, 12 ) + DPM1 = ( 0x65, 0x00002000, 13 ) + DPM2 = ( 0x65, 0x00004000, 14 ) + DPM3 = ( 0x65, 0x00008000, 15 ) + IND0 = ( 0x66, 0x00000001, 0 ) + IND1 = ( 0x66, 0x00000002, 1 ) + IND2 = ( 0x66, 0x00000004, 2 ) + IND3 = ( 0x66, 0x00000008, 3 ) + #UVM = ( 0x66, 0x00000010, 4 ) + #COMER = ( 0x66, 0x00000020, 5 ) + #OVT = ( 0x66, 0x00000040, 6 ) + RES0 = ( 0x66, 0x00000080, 7 ) + RES1 = ( 0x66, 0x00000100, 8 ) + RES2 = ( 0x66, 0x00000200, 9 ) + RES3 = ( 0x66, 0x00000400, 10 ) + + DC_H = [ DC_H_0, DC_H_1, DC_H_2, DC_H_3 ] + DC_L = [ DC_L_0, DC_L_1, DC_L_2, DC_L_3 ] + DC_L2H = [ DC_L2H_0, DC_L2H_1, DC_L2H_2, DC_L2H_3 ] + FSF = [ FSF_0, FSF_1, FSF_2, FSF_3 ] + CTRL_MODE = [ CTRL_MODE_0, CTRL_MODE_1, CTRL_MODE_2, CTRL_MODE_3 ] + CNTL = [ CNTL0, CNTL1, CNTL2, CNTL3 ] From 43608fb99f412c63602eb7edf2a6b2cc5c85cf5e Mon Sep 17 00:00:00 2001 From: Leonard Kugis Date: Tue, 27 Sep 2022 17:58:15 +0200 Subject: [PATCH 21/51] Solenoid IC feature: Corrected roundings. --- examples/evalboards/MAX22216/plunger_move.py | 4 ++-- pytrinamic/evalboards/MAX22216_eval.py | 18 ++++++++---------- pytrinamic/features/solenoid_ic.py | 17 +++++++++-------- 3 files changed, 19 insertions(+), 20 deletions(-) diff --git a/examples/evalboards/MAX22216/plunger_move.py b/examples/evalboards/MAX22216/plunger_move.py index ed576d23..68cb2ed2 100644 --- a/examples/evalboards/MAX22216/plunger_move.py +++ b/examples/evalboards/MAX22216/plunger_move.py @@ -14,9 +14,9 @@ solenoid = ic.motors[0] solenoid.u_supply = 24.0 # V - solenoid.u_dc_h = 24.0 # V + solenoid.u_dc_h = 10.0 # V solenoid.u_dc_l = 0.0 # V - solenoid.u_dc_l2h = 24.0 # V + solenoid.u_dc_l2h = 10.0 # V solenoid.u_dc_h2l = 0.0 # V solenoid.u_ac = 1.0 # V ampl solenoid.f_ac = 50.0 # Hz diff --git a/pytrinamic/evalboards/MAX22216_eval.py b/pytrinamic/evalboards/MAX22216_eval.py index 17c767a1..9e1bf822 100644 --- a/pytrinamic/evalboards/MAX22216_eval.py +++ b/pytrinamic/evalboards/MAX22216_eval.py @@ -17,23 +17,21 @@ def __init__(self, connection, module_id=1): values have to be configured with the module first. """ TMCLEval.__init__(self, connection, module_id) - self.motors = [self._MotorTypeA(self, 0)] + self.motors = [ + self._MotorTypeA(self, 0), + self._MotorTypeA(self, 1), + self._MotorTypeA(self, 2), + self._MotorTypeA(self, 3) + ] self.ics = [MAX22216(self)] # Use the driver controller functions for register access def write_register(self, register_address, value): - return self._connection.write_mc(register_address, value, self._module_id) + return self._connection.write_drv(register_address, value, self._module_id) def read_register(self, register_address, signed=False): - return self._connection.read_mc(register_address, self._module_id, signed) - - def write_register_field(self, field, value): - return self.write_register(field[0], TMC_helpers.field_set(self.read_register(field[0]), - field[1], field[2], value)) - - def read_register_field(self, field): - return TMC_helpers.field_get(self.read_register(field[0]), field[1], field[2]) + return self._connection.read_drv(register_address, self._module_id, signed) class _MotorTypeA(object): """ diff --git a/pytrinamic/features/solenoid_ic.py b/pytrinamic/features/solenoid_ic.py index 904372fe..d4074b6b 100644 --- a/pytrinamic/features/solenoid_ic.py +++ b/pytrinamic/features/solenoid_ic.py @@ -94,7 +94,8 @@ def set_voltage_high(self, u_dc_h): vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) - self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_H, self.__u_dc_value(u_dc_h, self.__u_supply, vdr, cdr, fsf)) + print("U_DC_H {}".format(round(self.__u_dc_value(u_dc_h, self.__u_supply, vdr, cdr, fsf)))) + self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_H, round(self.__u_dc_value(u_dc_h, self.__u_supply, vdr, cdr, fsf))) def get_voltage_high(self): """ @@ -120,7 +121,7 @@ def set_voltage_low(self, u_dc_l): vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) - self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_L, self.__u_dc_value(u_dc_l, self.__u_supply, vdr, cdr, fsf)) + self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_L, round(self.__u_dc_value(u_dc_l, self.__u_supply, vdr, cdr, fsf))) def get_voltage_low(self): """ @@ -146,7 +147,7 @@ def set_voltage_low_high(self, u_dc_l2h): vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) - self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_L2H, self.__u_dc_value(u_dc_l2h, self.__u_supply, vdr, cdr, fsf)) + self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_L2H, round(self.__u_dc_value(u_dc_l2h, self.__u_supply, vdr, cdr, fsf))) def get_voltage_low_high(self): """ @@ -172,7 +173,7 @@ def set_voltage_high_low(self, u_dc_h2l): vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) cdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE) == 1) fsf = self.__map_fsf.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.CTRL_MODE), 1.0) - self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_H2L, self.__u_dc_value(u_dc_h2l, self.__u_supply, vdr, cdr, fsf)) + self._parent.write_axis_field(self._axis, self._ic.FIELD.DC_H2L, round(self.__u_dc_value(u_dc_h2l, self.__u_supply, vdr, cdr, fsf))) def get_voltage_high_low(self): """ @@ -196,8 +197,8 @@ def set_frequency(self, u_ac_freq): Parameters: u_ac_freq: AC frequency. """ - pwm_freq = __map_pwm_freq.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.F_PWM_M), 100E3) - self._parent.write_axis_field(self._axis, self._ic.FIELD.DELTA_PHI, self.__delta_phi(pwm_freq, u_ac_freq)) + pwm_freq = self.__map_pwm_freq.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.F_PWM_M), 100E3) + self._parent.write_axis_field(self._axis, self._ic.FIELD.DELTA_PHI, round(self.__delta_phi(pwm_freq, u_ac_freq))) def get_frequency(self): """ @@ -208,7 +209,7 @@ def get_frequency(self): Returns: AC frequency. """ - pwm_freq = __map_pwm_freq.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.F_PWM_M), 100E3) + pwm_freq = self.__map_pwm_freq.get(self._parent.read_axis_field(self._axis, self._ic.FIELD.F_PWM_M), 100E3) return self.__f_AC(pwm_freq, self._parent.read_axis_field(self._axis, self._ic.FIELD.DELTA_PHI)) def set_voltage_ac(self, u_ac): @@ -220,7 +221,7 @@ def set_voltage_ac(self, u_ac): u_ac: AC voltage. """ vdr = (self._parent.read_axis_field(self._axis, self._ic.FIELD.VDR_NDUTY) == 1) - self._parent.write_axis_field(self._axis, self._ic.FIELD.U_AC, self.__u_ac_value(u_ac, self.__u_supply, vdr)) + self._parent.write_axis_field(self._axis, self._ic.FIELD.U_AC, round(self.__u_ac_value(u_ac, self.__u_supply, vdr))) def get_voltage_ac(self): """ From 972e035bdec573acf85e3fd0e8cab515cc686665 Mon Sep 17 00:00:00 2001 From: bp Date: Wed, 5 Oct 2022 14:28:50 +0200 Subject: [PATCH 22/51] Connections: remove duplicated code in the can adapter classes * .. by putting the duplicated code in a generic CAN tmcl interface class * introduce a test for the CAN adapters --- pytrinamic/connections/can_tmcl_interface.py | 84 ++++++++++++ .../connections/ixxat_tmcl_interface.py | 110 ++-------------- .../connections/kvaser_tmcl_interface.py | 89 ++----------- pytrinamic/connections/pcan_tmcl_interface.py | 124 ++++-------------- .../connections/slcan_tmcl_interface.py | 82 +----------- .../connections/socketcan_tmcl_interface.py | 86 ++---------- tests/test_can_adapters.py | 81 ++++++++++++ 7 files changed, 231 insertions(+), 425 deletions(-) create mode 100644 pytrinamic/connections/can_tmcl_interface.py create mode 100644 tests/test_can_adapters.py diff --git a/pytrinamic/connections/can_tmcl_interface.py b/pytrinamic/connections/can_tmcl_interface.py new file mode 100644 index 00000000..a0f9f3e3 --- /dev/null +++ b/pytrinamic/connections/can_tmcl_interface.py @@ -0,0 +1,84 @@ + +import can +from ..connections.tmcl_interface import TmclInterface + + +class CanTmclInterface(TmclInterface): + """Generic CAN interface class for the CAN adapters.""" + + def __init__(self, host_id=2, default_module_id=1, debug=False, timeout_s=5): + + TmclInterface.__init__(self, host_id, default_module_id, debug) + self._connection = None + self._channel = None + self._bitrate = None + if timeout_s == 0: + self._timeout_s = None + else: + self._timeout_s = timeout_s + + def __enter__(self): + return self + + def __exit__(self, exit_type, value, traceback): + """ + Close the connection at the end of a with-statement block. + """ + del exit_type, value, traceback + self.close() + + def close(self): + if self._debug: + print(f"Closing {self.__class__.__name__} (channel {str(self._channel)})") + + self._connection.shutdown() + + def _send(self, host_id, module_id, data): + """ + Send the bytearray parameter [data]. + + This is a required override function for using the tmcl_interface class. + """ + del host_id + + msg = can.Message(arbitration_id=module_id, is_extended_id=False, data=data[1:]) + + try: + self._connection.send(msg) + except can.CanError as e: + raise ConnectionError( + f"Failed to send a TMCL message on {self.__class__.__name__} (channel {str(self._channel)})" + ) from e + + def _recv(self, host_id, module_id): + """ + Read 9 bytes and return them as a bytearray. + + This is a required override function for using the tmcl_interface class. + """ + del module_id + + try: + msg = self._connection.recv(timeout=self._timeout_s) + except can.CanError as e: + raise ConnectionError( + f"Failed to receive a TMCL message from {self.__class__.__name__} (channel {str(self._channel)})" + ) from e + + if not msg: + raise ConnectionError(f"Recv timed out ({self.__class__.__name__}, on channel {str(self._channel)})") + + if msg.arbitration_id != host_id: + # The filter shouldn't let wrong messages through. + # This is just a sanity check + if self._debug: + print(f"Received wrong ID ({self.__class__.__name__}, on channel {str(self._channel)})") + + return bytearray([msg.arbitration_id]) + msg.data + + @staticmethod + def supports_tmcl(): + return True + + def __str__(self): + return f"Connection: Type = {self.__class__.__name__}, Channel = {self._channel}, Bitrate = {self._bitrate}" diff --git a/pytrinamic/connections/ixxat_tmcl_interface.py b/pytrinamic/connections/ixxat_tmcl_interface.py index 534145a5..0128613f 100644 --- a/pytrinamic/connections/ixxat_tmcl_interface.py +++ b/pytrinamic/connections/ixxat_tmcl_interface.py @@ -1,13 +1,8 @@ import can -from can import CanError -from ..connections.tmcl_interface import TmclInterface +from ..connections.can_tmcl_interface import CanTmclInterface -# Providing 5 channels here, this should cover all use cases. -# Ixxat USB-to-CAN provides 1, 2 or 4 channels. -_CHANNELS = ["0", "1", "2", "3", "4"] - -class IxxatTmclInterface(TmclInterface): +class IxxatTmclInterface(CanTmclInterface): """ This class implements a TMCL connection for IXXAT USB-to-CAN adapter. Backend is provided by the IXXAT Virtual CAN Interface V3 SDK. @@ -27,20 +22,20 @@ class IxxatTmclInterface(TmclInterface): print("Found IXXAT adapter with hardware id '%s'." % hwid) """ + # Providing 5 channels here, this should cover all use cases. + # Ixxat USB-to-CAN provides 1, 2 or 4 channels. + _CHANNELS = ["0", "1", "2", "3", "4"] + def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): if not isinstance(port, str): raise TypeError - if port not in _CHANNELS: + if port not in self._CHANNELS: raise ValueError("Invalid port!") - TmclInterface.__init__(self, host_id, module_id, debug) + CanTmclInterface.__init__(self, host_id, module_id, debug, timeout_s) self._channel = port self._bitrate = datarate - if timeout_s == 0: - self._timeout_s = None - else: - self._timeout_s = timeout_s try: if self._debug: @@ -52,7 +47,7 @@ def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=Fal bitrate=self._bitrate, can_filters=[{"can_id": host_id, "can_mask": 0x7F}], ) - except CanError as e: + except can.CanError as e: self._connection = None raise ConnectionError( f"Failed to connect to {self.__class__.__name__} on channel {str(self._channel)}" @@ -61,93 +56,12 @@ def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=Fal if self._debug: print(f"Opened {self.__class__.__name__} on channel {str(self._channel)}") - def __enter__(self): - return self - - def __exit__(self, exit_type, value, traceback): - """ - Close the connection at the end of a with-statement block. - """ - del exit_type, value, traceback - self.close() - - def close(self): - if self._debug: - print(f"Closing {self.__class__.__name__} (channel {str(self._channel)})") - - self._connection.shutdown() - - def _send(self, host_id, module_id, data): - """ - Send the bytearray parameter [data]. - - This is a required override function for using the tmcl_interface class. - """ - del host_id - - msg = can.Message(arbitration_id=module_id, is_extended_id=False, data=data[1:]) - - try: - self._connection.send(msg) - except CanError as e: - raise ConnectionError( - f"Failed to send a TMCL message on {self.__class__.__name__} (channel {str(self._channel)})" - ) from e - - def _recv(self, host_id, module_id): - """ - Read 9 bytes and return them as a bytearray. - - This is a required override function for using the tmcl_interface - class. - """ - del module_id - - try: - msg = self._connection.recv(timeout=self._timeout_s) - except CanError as e: - raise ConnectionError( - f"Failed to receive a TMCL message from {self.__class__.__name__} (channel {str(self._channel)})" - ) from e - - if not msg: - # Todo: Timeout retry mechanism - raise ConnectionError(f"Recv timed out ({self.__class__.__name__}, on channel {str(self._channel)})") - - if msg.arbitration_id != host_id: - # The filter shouldn't let wrong messages through. - # This is just a sanity check - if self._debug: - print(f"Received wrong ID ({self.__class__.__name__}, on channel {str(self._channel)})") - - return bytearray([msg.arbitration_id]) + msg.data - - @staticmethod - def supports_tmcl(): - return True - - @staticmethod - def list(): + @classmethod + def list(cls): """ Return a list of available connection ports as a list of strings. This function is required for using this interface with the connection manager. """ - return _CHANNELS - - def __str__(self): - return f"Connection: Type = {self.__class__.__name__}, Channel = {self._channel}, Bitrate = {self._bitrate}" - - -""" -# Snippet to test connection between one (or two) ports of a single IXXAT USB-to-CAN and TMCL device(s). -if __name__ == "__main__": - CAN1 = IxxatTmclInterface(port="0") - # CAN2 = IxxatTmclInterface(port="1") - print(CAN1) - # print(CAN2) - - print(CAN1.get_version_string()) - # print(CAN2.get_version_string()) -""" + return cls._CHANNELS diff --git a/pytrinamic/connections/kvaser_tmcl_interface.py b/pytrinamic/connections/kvaser_tmcl_interface.py index 28b8ab93..abc09033 100644 --- a/pytrinamic/connections/kvaser_tmcl_interface.py +++ b/pytrinamic/connections/kvaser_tmcl_interface.py @@ -1,29 +1,24 @@ import can -from can import CanError -from ..connections.tmcl_interface import TmclInterface +from ..connections.can_tmcl_interface import CanTmclInterface -_CHANNELS = ["0", "1", "2"] - -class KvaserTmclInterface(TmclInterface): +class KvaserTmclInterface(CanTmclInterface): """ This class implements a TMCL connection for Kvaser adapter using CANLIB. Try 0 as default channel. """ + _CHANNELS = ["0", "1", "2"] + def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): if not isinstance(port, str): raise TypeError - if port not in _CHANNELS: + if port not in self._CHANNELS: raise ValueError("Invalid port!") - TmclInterface.__init__(self, host_id, module_id, debug) + CanTmclInterface.__init__(self, host_id, module_id, debug, timeout_s) self._channel = port self._bitrate = datarate - if timeout_s == 0: - self._timeout_s = None - else: - self._timeout_s = timeout_s try: if self._debug: @@ -31,83 +26,19 @@ def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=Fal else: self._connection = can.Bus(interface="kvaser", channel=self._channel, bitrate=self._bitrate, can_filters=[{"can_id": host_id, "can_mask": 0x7F}]) - except CanError as e: + except can.CanError as e: self._connection = None raise ConnectionError("Failed to connect to Kvaser CAN bus") from e if self._debug: print("Opened bus on channel " + str(self._channel)) - def __enter__(self): - return self - - def __exit__(self, exit_type, value, traceback): - """ - Close the connection at the end of a with-statement block. - """ - del exit_type, value, traceback - self.close() - - def close(self): - if self._debug: - print("Closing CAN bus") - - self._connection.shutdown() - - def _send(self, host_id, module_id, data): - """ - Send the bytearray parameter [data]. - - This is a required override function for using the tmcl_interface class. - """ - del host_id - - msg = can.Message(arbitration_id=module_id, is_extended_id=False, data=data[1:]) - - try: - self._connection.send(msg) - except CanError as e: - raise ConnectionError("Failed to send a TMCL message") from e - - def _recv(self, host_id, module_id): - """ - Read 9 bytes and return them as a bytearray. - - This is a required override function for using the tmcl_interface - class. - """ - del module_id - - try: - msg = self._connection.recv(timeout=self._timeout_s) - except CanError as e: - raise ConnectionError("Failed to receive a TMCL message") from e - - if not msg: - # Todo: Timeout retry mechanism - raise ConnectionError("Recv timed out") - - if msg.arbitration_id != host_id: - # The filter shouldn't let wrong messages through. - # This is just a sanity check - if self._debug: - print("Received wrong ID") - - return bytearray([msg.arbitration_id]) + msg.data - - @staticmethod - def supports_tmcl(): - return True - - @staticmethod - def list(): + @classmethod + def list(cls): """ Return a list of available connection ports as a list of strings. This function is required for using this interface with the connection manager. """ - return _CHANNELS - - def __str__(self): - return "Connection: type={} channel={} bitrate={}".format(type(self).__name__, self._channel, self._bitrate) + return cls._CHANNELS diff --git a/pytrinamic/connections/pcan_tmcl_interface.py b/pytrinamic/connections/pcan_tmcl_interface.py index d62676e7..669d6dca 100644 --- a/pytrinamic/connections/pcan_tmcl_interface.py +++ b/pytrinamic/connections/pcan_tmcl_interface.py @@ -1,52 +1,47 @@ import can -from can import CanError from can.interfaces.pcan.pcan import PcanError -from ..connections.tmcl_interface import TmclInterface +from ..connections.can_tmcl_interface import CanTmclInterface -_CHANNELS = [ - "PCAN_USBBUS1", "PCAN_USBBUS2", "PCAN_USBBUS3", "PCAN_USBBUS4", - "PCAN_USBBUS5", "PCAN_USBBUS6", "PCAN_USBBUS7", "PCAN_USBBUS8", - "PCAN_USBBUS9", "PCAN_USBBUS10", "PCAN_USBBUS11", "PCAN_USBBUS12", - "PCAN_USBBUS13", "PCAN_USBBUS14", "PCAN_USBBUS15", "PCAN_USBBUS16", - "PCAN_ISABUS1", "PCAN_ISABUS2", "PCAN_ISABUS3", "PCAN_ISABUS4", - "PCAN_ISABUS5", "PCAN_ISABUS6", "PCAN_ISABUS7", "PCAN_ISABUS8", +class PcanTmclInterface(CanTmclInterface): + """ + This class implements a TMCL connection over a PCAN adapter. + """ + _CHANNELS = [ + "PCAN_USBBUS1", "PCAN_USBBUS2", "PCAN_USBBUS3", "PCAN_USBBUS4", + "PCAN_USBBUS5", "PCAN_USBBUS6", "PCAN_USBBUS7", "PCAN_USBBUS8", + "PCAN_USBBUS9", "PCAN_USBBUS10", "PCAN_USBBUS11", "PCAN_USBBUS12", + "PCAN_USBBUS13", "PCAN_USBBUS14", "PCAN_USBBUS15", "PCAN_USBBUS16", - "PCAN_DNGBUS1", + "PCAN_ISABUS1", "PCAN_ISABUS2", "PCAN_ISABUS3", "PCAN_ISABUS4", + "PCAN_ISABUS5", "PCAN_ISABUS6", "PCAN_ISABUS7", "PCAN_ISABUS8", - "PCAN_PCIBUS1", "PCAN_PCIBUS2", "PCAN_PCIBUS3", "PCAN_PCIBUS4", - "PCAN_PCIBUS5", "PCAN_PCIBUS6", "PCAN_PCIBUS7", "PCAN_PCIBUS8", - "PCAN_PCIBUS9", "PCAN_PCIBUS10", "PCAN_PCIBUS11", "PCAN_PCIBUS12", - "PCAN_PCIBUS13", "PCAN_PCIBUS14", "PCAN_PCIBUS15", "PCAN_PCIBUS16", + "PCAN_DNGBUS1", - "PCAN_PCCBUS1", "PCAN_PCCBUS2", + "PCAN_PCIBUS1", "PCAN_PCIBUS2", "PCAN_PCIBUS3", "PCAN_PCIBUS4", + "PCAN_PCIBUS5", "PCAN_PCIBUS6", "PCAN_PCIBUS7", "PCAN_PCIBUS8", + "PCAN_PCIBUS9", "PCAN_PCIBUS10", "PCAN_PCIBUS11", "PCAN_PCIBUS12", + "PCAN_PCIBUS13", "PCAN_PCIBUS14", "PCAN_PCIBUS15", "PCAN_PCIBUS16", - "PCAN_LANBUS1", "PCAN_LANBUS2", "PCAN_LANBUS3", "PCAN_LANBUS4", - "PCAN_LANBUS5", "PCAN_LANBUS6", "PCAN_LANBUS7", "PCAN_LANBUS8", - "PCAN_LANBUS9", "PCAN_LANBUS10", "PCAN_LANBUS11", "PCAN_LANBUS12", - "PCAN_LANBUS13", "PCAN_LANBUS14", "PCAN_LANBUS15", "PCAN_LANBUS16" - ] + "PCAN_PCCBUS1", "PCAN_PCCBUS2", + "PCAN_LANBUS1", "PCAN_LANBUS2", "PCAN_LANBUS3", "PCAN_LANBUS4", + "PCAN_LANBUS5", "PCAN_LANBUS6", "PCAN_LANBUS7", "PCAN_LANBUS8", + "PCAN_LANBUS9", "PCAN_LANBUS10", "PCAN_LANBUS11", "PCAN_LANBUS12", + "PCAN_LANBUS13", "PCAN_LANBUS14", "PCAN_LANBUS15", "PCAN_LANBUS16" + ] -class PcanTmclInterface(TmclInterface): - """ - This class implements a TMCL connection over a PCAN adapter. - """ def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): if not isinstance(port, str): raise TypeError - if port not in _CHANNELS: + if port not in self._CHANNELS: raise ValueError("Invalid port!") - TmclInterface.__init__(self, host_id, module_id, debug) + CanTmclInterface.__init__(self, host_id, module_id, debug, timeout_s) self._channel = port self._bitrate = datarate - if timeout_s == 0: - self._timeout_s = None - else: - self._timeout_s = timeout_s try: self._connection = can.Bus(interface="pcan", channel=self._channel, bitrate=self._bitrate) @@ -58,75 +53,12 @@ def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, if self._debug: print("Opened bus on channel " + self._channel) - def __enter__(self): - return self - - def __exit__(self, exit_type, value, traceback): - """ - Close the connection at the end of a with-statement block. - """ - del exit_type, value, traceback - self.close() - - def close(self): - if self._debug: - print("Closing PCAN bus") - - self._connection.shutdown() - - def _send(self, host_id, module_id, data): - """ - Send the bytearray parameter [data]. - - This is a required override function for using the tmcl_interface class. - """ - del host_id - - msg = can.Message(arbitration_id=module_id, is_extended_id=False, data=data[1:]) - - try: - self._connection.send(msg) - except CanError as e: - raise ConnectionError("Failed to send a TMCL message") from e - - def _recv(self, host_id, module_id): - """ - Read 9 bytes and return them as a bytearray. - - This is a required override function for using the tmcl_interface - class. - """ - del module_id - - try: - msg = self._connection.recv(timeout=self._timeout_s) - except CanError as e: - raise ConnectionError("Failed to receive a TMCL message") from e - - if not msg: - # Todo: Timeout retry mechanism - raise ConnectionError("Recv timed out") - - if msg.arbitration_id != host_id: - # The filter shouldn't let wrong messages through. - # This is just a sanity check - raise ConnectionError("Received wrong ID") - - return bytearray([msg.arbitration_id]) + msg.data - - @staticmethod - def supports_tmcl(): - return True - - @staticmethod - def list(): + @classmethod + def list(cls): """ Return a list of available connection ports as a list of strings. This function is required for using this interface with the connection manager. """ - return _CHANNELS - - def __str__(self): - return "Connection: type={} channel={} bitrate={}".format(type(self).__name__, self._channel, self._bitrate) + return cls._CHANNELS diff --git a/pytrinamic/connections/slcan_tmcl_interface.py b/pytrinamic/connections/slcan_tmcl_interface.py index e3751b91..34cbfb7a 100644 --- a/pytrinamic/connections/slcan_tmcl_interface.py +++ b/pytrinamic/connections/slcan_tmcl_interface.py @@ -1,10 +1,10 @@ import can from can import CanError from serial.tools.list_ports import comports -from ..connections.tmcl_interface import TmclInterface +from ..connections.can_tmcl_interface import CanTmclInterface -class SlcanTmclInterface(TmclInterface): +class SlcanTmclInterface(CanTmclInterface): """ This class implements a TMCL connection for CAN over Serial / SLCAN. Compatible with CANable running slcan firmware and similar. @@ -16,89 +16,24 @@ def __init__(self, com_port, datarate=1000000, host_id=2, module_id=1, debug=Tru if not isinstance(com_port, str): raise TypeError - TmclInterface.__init__(self, host_id, module_id, debug) + CanTmclInterface.__init__(self, host_id, module_id, debug, timeout_s) self._bitrate = datarate self._port = com_port - if timeout_s == 0: - self._timeout_s = None - else: - self._timeout_s = timeout_s self._serial_baudrate = serial_baudrate try: self._connection = can.Bus(interface='slcan', channel=self._port, bitrate=self._bitrate, - ttyBaudrate=self._serialBaudrate) + ttyBaudrate=self._serial_baudrate) self._connection.set_filters([{"can_id": host_id, "can_mask": 0x7F}]) except CanError as e: - self.__connection = None + self._connection = None raise ConnectionError("Failed to connect to CAN bus") from e if self._debug: print("Opened slcan bus on channel " + self._port) - def __enter__(self): - return self - - def __exit__(self, exit_type, value, traceback): - """ - Close the connection at the end of a with-statement block. - """ - del exit_type, value, traceback - self.close() - - def close(self): - if self._debug: - print("Closing CAN bus") - - self._connection.shutdown() - - def _send(self, host_id, module_id, data): - """ - Send the bytearray parameter [data]. - - This is a required override function for using the tmcl_interface - class. - """ - del host_id - - msg = can.Message(arbitration_id=module_id, is_extended_id=False, data=data[1:]) - - try: - self.__connection.send(msg) - except CanError as e: - raise ConnectionError("Failed to send a TMCL message") from e - - def _recv(self, host_id, module_id): - """ - Read 9 bytes and return them as a bytearray. - - This is a required override function for using the tmcl_interface - class. - """ - del module_id - - try: - msg = self._connection.recv(timeout=self._timeout_s) - except CanError as e: - raise ConnectionError("Failed to receive a TMCL message") from e - - if not msg: - # Todo: Timeout retry mechanism - raise ConnectionError("Recv timed out") - - if msg.arbitration_id != host_id: - # The filter shouldn't let wrong messages through. - # This is just a sanity check - raise ConnectionError("Received wrong ID") - - return bytearray([msg.arbitration_id]) + msg.data - - @staticmethod - def supports_tmcl(): - return True - - @staticmethod - def list(): + @classmethod + def list(cls): """ Return a list of available connection ports as a list of strings. @@ -110,6 +45,3 @@ def list(): connected.append(element.device) return connected - - def __str__(self): - return "Connection: type={} channel={} bitrate={}".format(type(self).__name__, self._port, self._bitrate) diff --git a/pytrinamic/connections/socketcan_tmcl_interface.py b/pytrinamic/connections/socketcan_tmcl_interface.py index 9c06b3b6..d6453b66 100644 --- a/pytrinamic/connections/socketcan_tmcl_interface.py +++ b/pytrinamic/connections/socketcan_tmcl_interface.py @@ -1,31 +1,27 @@ import can from can import CanError -from ..connections.tmcl_interface import TmclInterface +from ..connections.can_tmcl_interface import CanTmclInterface -_CHANNELS = ["can0", "can1", "can2", "can3", "can4", "can5", "can6", "can7"] - -class SocketcanTmclInterface(TmclInterface): +class SocketcanTmclInterface(CanTmclInterface): """ This class implements a TMCL connection over a SocketCAN adapter. Use following command under linux to activate can socket sudo ip link set can0 down type can bitrate 1000000 """ + _CHANNELS = ["can0", "can1", "can2", "can3", "can4", "can5", "can6", "can7"] + def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): if not isinstance(port, str): raise TypeError - if port not in _CHANNELS: + if port not in self._CHANNELS: raise ValueError("Invalid port") - TmclInterface.__init__(self, host_id, module_id, debug) + CanTmclInterface.__init__(self, host_id, module_id, debug, timeout_s) self._channel = port self._bitrate = datarate - if timeout_s == 0: - self._timeout_s = None - else: - self._timeout_s = timeout_s try: self._connection = can.Bus(interface="socketcan", channel=self._channel, bitrate=self._bitrate) @@ -38,76 +34,12 @@ def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, if self._debug: print("Opened bus on channel " + self._channel) - def __enter__(self): - return self - - def __exit__(self, exit_type, value, traceback): - """ - Close the connection at the end of a with-statement block. - """ - del exit_type, value, traceback - self.close() - - def close(self): - if self._debug: - print("Closing socketcan bus") - - self._connection.shutdown() - - def _send(self, host_id, module_id, data): - """ - Send the bytearray parameter [data]. - - This is a required override function for using the tmcl_interface - class. - """ - del host_id - - msg = can.Message(arbitration_id=module_id, is_extended_id=False, data=data[1:]) - - try: - self._connection.send(msg) - except CanError as e: - raise ConnectionError("Failed to send a TMCL message") from e - - def _recv(self, host_id, module_id): - """ - Read 9 bytes and return them as a bytearray. - - This is a required override function for using the tmcl_interface - class. - """ - del module_id - - try: - msg = self._connection.recv(timeout=self._timeout_s) - except CanError as e: - raise ConnectionError("Failed to receive a TMCL message") from e - - if not msg: - # Todo: Timeout retry mechanism - raise ConnectionError("Recv timed out") - - if msg.arbitration_id != host_id: - # The filter shouldn't let wrong messages through. - # This is just a sanity check - raise ConnectionError("Received wrong ID") - - return bytearray([msg.arbitration_id]) + msg.data - - @staticmethod - def supports_tmcl(): - return True - - @staticmethod - def list(): + @classmethod + def list(cls): """ Return a list of available connection ports as a list of strings. This function is required for using this interface with the connection manager. """ - return _CHANNELS - - def __str__(self): - return "Connection: type={} channel={} bitrate={}".format(type(self).__name__, self._channel, self._bitrate) + return cls._CHANNELS diff --git a/tests/test_can_adapters.py b/tests/test_can_adapters.py new file mode 100644 index 00000000..9bc6aff5 --- /dev/null +++ b/tests/test_can_adapters.py @@ -0,0 +1,81 @@ +"""Testing the all CAN Adapters in a combined Network + +To recreate the tests create a CAN network by combining: + * Kvaser Leaf Light v2 + * PEAK PCAN + * Ixxat USB-to-CAN + * CANable (Original) + * 3x TMCM-1241 with CAN-ID 3,4 and 5 +""" + +import pytest + +from pytrinamic.modules import TMCLModule + +from pytrinamic.connections import ConnectionManager + +from pytrinamic.connections import KvaserTmclInterface +from pytrinamic.connections import PcanTmclInterface +from pytrinamic.connections import SlcanTmclInterface +from pytrinamic.connections import IxxatTmclInterface + +slcan_com_port = 'COM15' + +ap = { + 'Maximum current': 6, + 'Microstep resolution': 140, +} + + +@pytest.fixture(scope='function', params=[ + KvaserTmclInterface, + PcanTmclInterface, + SlcanTmclInterface, + IxxatTmclInterface, +]) +def can_adapter(request): + can_tmcl_interface_class = request.param + if can_tmcl_interface_class == PcanTmclInterface: + ports = PcanTmclInterface.list() + adptr = can_tmcl_interface_class(port=ports[0]) + elif can_tmcl_interface_class == SlcanTmclInterface: + adptr = can_tmcl_interface_class(com_port=slcan_com_port) + else: + adptr = can_tmcl_interface_class() + yield adptr + adptr.close() + + +def test_adapter_classes(can_adapter): + tmcm1241s = [TMCLModule(can_adapter, module_id=mid) for mid in range(3, 6)] + for tmcm1241 in tmcm1241s: + assert tmcm1241.get_global_parameter(71, 0) == tmcm1241.module_id + assert tmcm1241.get_axis_parameter(ap['Microstep resolution'], 0) == 8 + for tmcm1241 in tmcm1241s: + tmcm1241.set_axis_parameter(ap['Maximum current'], 0, 10+tmcm1241.module_id) + assert tmcm1241.get_axis_parameter(ap['Maximum current'], 0) == 10+tmcm1241.module_id + for tmcm1241 in tmcm1241s: + tmcm1241.set_axis_parameter(ap['Maximum current'], 0, 20+tmcm1241.module_id) + assert tmcm1241.get_axis_parameter(ap['Maximum current'], 0) == 20+tmcm1241.module_id + + +@pytest.mark.parametrize('cm_call', [ + f"--interface ixxat_tmcl", + f"--interface kvaser_tmcl", + f"--interface pcan_tmcl", + f"--interface slcan_tmcl --port {slcan_com_port}", +]) +def test_connection_manager(cm_call): + cm = ConnectionManager(cm_call) + with cm.connect() as interface: + tmcm1241s = [TMCLModule(interface, module_id=mid) for mid in range(3, 6)] + for tmcm1241 in tmcm1241s: + assert tmcm1241.get_global_parameter(71, 0) == tmcm1241.module_id + assert tmcm1241.get_axis_parameter(ap['Microstep resolution'], 0) == 8 + for tmcm1241 in tmcm1241s: + tmcm1241.set_axis_parameter(ap['Maximum current'], 0, 10+tmcm1241.module_id) + assert tmcm1241.get_axis_parameter(ap['Maximum current'], 0) == 10+tmcm1241.module_id + for tmcm1241 in tmcm1241s: + tmcm1241.set_axis_parameter(ap['Maximum current'], 0, 20+tmcm1241.module_id) + assert tmcm1241.get_axis_parameter(ap['Maximum current'], 0) == 20+tmcm1241.module_id + From cb25288fe7cacaf3b56931fcb350ceadd37f2413 Mon Sep 17 00:00:00 2001 From: bp Date: Wed, 5 Oct 2022 15:07:13 +0200 Subject: [PATCH 23/51] Connections: move the CanTmcl derived classes in a sub-directory --- pytrinamic/connections/__init__.py | 10 +++++----- pytrinamic/connections/can_tmcl/__init__.py | 0 .../connections/{ => can_tmcl}/ixxat_tmcl_interface.py | 2 +- .../{ => can_tmcl}/kvaser_tmcl_interface.py | 2 +- .../connections/{ => can_tmcl}/pcan_tmcl_interface.py | 2 +- .../connections/{ => can_tmcl}/slcan_tmcl_interface.py | 2 +- .../{ => can_tmcl}/socketcan_tmcl_interface.py | 2 +- 7 files changed, 10 insertions(+), 10 deletions(-) create mode 100644 pytrinamic/connections/can_tmcl/__init__.py rename pytrinamic/connections/{ => can_tmcl}/ixxat_tmcl_interface.py (97%) rename pytrinamic/connections/{ => can_tmcl}/kvaser_tmcl_interface.py (95%) rename pytrinamic/connections/{ => can_tmcl}/pcan_tmcl_interface.py (97%) rename pytrinamic/connections/{ => can_tmcl}/slcan_tmcl_interface.py (96%) rename pytrinamic/connections/{ => can_tmcl}/socketcan_tmcl_interface.py (95%) diff --git a/pytrinamic/connections/__init__.py b/pytrinamic/connections/__init__.py index ddae80e1..6f98e0eb 100644 --- a/pytrinamic/connections/__init__.py +++ b/pytrinamic/connections/__init__.py @@ -1,10 +1,10 @@ from .dummy_tmcl_interface import DummyTmclInterface -from .pcan_tmcl_interface import PcanTmclInterface -from .socketcan_tmcl_interface import SocketcanTmclInterface -from .kvaser_tmcl_interface import KvaserTmclInterface +from .can_tmcl.pcan_tmcl_interface import PcanTmclInterface +from .can_tmcl.socketcan_tmcl_interface import SocketcanTmclInterface +from .can_tmcl.kvaser_tmcl_interface import KvaserTmclInterface from .serial_tmcl_interface import SerialTmclInterface from .uart_ic_interface import UartIcInterface from .usb_tmcl_interface import UsbTmclInterface -from .slcan_tmcl_interface import SlcanTmclInterface -from .ixxat_tmcl_interface import IxxatTmclInterface +from .can_tmcl.slcan_tmcl_interface import SlcanTmclInterface +from .can_tmcl.ixxat_tmcl_interface import IxxatTmclInterface from .connection_manager import ConnectionManager diff --git a/pytrinamic/connections/can_tmcl/__init__.py b/pytrinamic/connections/can_tmcl/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/pytrinamic/connections/ixxat_tmcl_interface.py b/pytrinamic/connections/can_tmcl/ixxat_tmcl_interface.py similarity index 97% rename from pytrinamic/connections/ixxat_tmcl_interface.py rename to pytrinamic/connections/can_tmcl/ixxat_tmcl_interface.py index 0128613f..198f39ea 100644 --- a/pytrinamic/connections/ixxat_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl/ixxat_tmcl_interface.py @@ -1,5 +1,5 @@ import can -from ..connections.can_tmcl_interface import CanTmclInterface +from ...connections.can_tmcl_interface import CanTmclInterface class IxxatTmclInterface(CanTmclInterface): diff --git a/pytrinamic/connections/kvaser_tmcl_interface.py b/pytrinamic/connections/can_tmcl/kvaser_tmcl_interface.py similarity index 95% rename from pytrinamic/connections/kvaser_tmcl_interface.py rename to pytrinamic/connections/can_tmcl/kvaser_tmcl_interface.py index abc09033..be5e37ec 100644 --- a/pytrinamic/connections/kvaser_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl/kvaser_tmcl_interface.py @@ -1,5 +1,5 @@ import can -from ..connections.can_tmcl_interface import CanTmclInterface +from ...connections.can_tmcl_interface import CanTmclInterface class KvaserTmclInterface(CanTmclInterface): diff --git a/pytrinamic/connections/pcan_tmcl_interface.py b/pytrinamic/connections/can_tmcl/pcan_tmcl_interface.py similarity index 97% rename from pytrinamic/connections/pcan_tmcl_interface.py rename to pytrinamic/connections/can_tmcl/pcan_tmcl_interface.py index 669d6dca..9d20e303 100644 --- a/pytrinamic/connections/pcan_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl/pcan_tmcl_interface.py @@ -1,7 +1,7 @@ import can from can.interfaces.pcan.pcan import PcanError -from ..connections.can_tmcl_interface import CanTmclInterface +from ...connections.can_tmcl_interface import CanTmclInterface class PcanTmclInterface(CanTmclInterface): diff --git a/pytrinamic/connections/slcan_tmcl_interface.py b/pytrinamic/connections/can_tmcl/slcan_tmcl_interface.py similarity index 96% rename from pytrinamic/connections/slcan_tmcl_interface.py rename to pytrinamic/connections/can_tmcl/slcan_tmcl_interface.py index 34cbfb7a..864a96ca 100644 --- a/pytrinamic/connections/slcan_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl/slcan_tmcl_interface.py @@ -1,7 +1,7 @@ import can from can import CanError from serial.tools.list_ports import comports -from ..connections.can_tmcl_interface import CanTmclInterface +from ...connections.can_tmcl_interface import CanTmclInterface class SlcanTmclInterface(CanTmclInterface): diff --git a/pytrinamic/connections/socketcan_tmcl_interface.py b/pytrinamic/connections/can_tmcl/socketcan_tmcl_interface.py similarity index 95% rename from pytrinamic/connections/socketcan_tmcl_interface.py rename to pytrinamic/connections/can_tmcl/socketcan_tmcl_interface.py index d6453b66..50d69377 100644 --- a/pytrinamic/connections/socketcan_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl/socketcan_tmcl_interface.py @@ -1,6 +1,6 @@ import can from can import CanError -from ..connections.can_tmcl_interface import CanTmclInterface +from ...connections.can_tmcl_interface import CanTmclInterface class SocketcanTmclInterface(CanTmclInterface): From a8fb8c7a19951eb70f84e6acb2bddecc115ab693 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Wed, 2 Nov 2022 10:47:08 +0100 Subject: [PATCH 24/51] RAMDebug: Fixed optional SET_PROCESS_FREQUENCY usage The SET_PROCESS_FREQUENCY subcommand is (currently) only used in the TMC-Evalsystem RAMDebug implementation. For other implementations, skip the "Command type not supported" warning that this subcommand causes. --- pytrinamic/RAMDebug.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/pytrinamic/RAMDebug.py b/pytrinamic/RAMDebug.py index b1f13bc7..1fcec6e2 100644 --- a/pytrinamic/RAMDebug.py +++ b/pytrinamic/RAMDebug.py @@ -1,4 +1,4 @@ -from pytrinamic.tmcl import TMCLCommand +from pytrinamic.tmcl import TMCLCommand, TMCLReplyStatusError, TMCLStatus from enum import IntEnum class RAMDebug_Command(IntEnum): @@ -178,9 +178,18 @@ def get_channels(self): def start_measurement(self): self._command(RAMDebug_Command.INIT.value, 0, 0) self._command(RAMDebug_Command.SET_SAMPLE_COUNT.value, 0, self.get_total_samples()) - self._command(RAMDebug_Command.SET_PROCESS_FREQUENCY, 0, self._process_frequency) self._command(RAMDebug_Command.SET_PRESCALER.value, 0, self._prescaler) + try: + self._command(RAMDebug_Command.SET_PROCESS_FREQUENCY, 0, self._process_frequency) + except TMCLReplyStatusError as e: + if e.status_code == TMCLStatus.WRONG_TYPE: + # SET_PROCESS_FREQUENCY not supported -> skip exception + pass + else: + # A different error occurred -> reraise exception + raise e + for channel in self.channels: self._command(RAMDebug_Command.SET_CHANNEL.value, channel.type.value, channel.value) From b05016004db94caa0c4db9006836cfbffc003a7b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Wed, 2 Nov 2022 10:53:03 +0100 Subject: [PATCH 25/51] RAMDebug: get_state() now returns a RAMDebug_State enum instead of int --- pytrinamic/RAMDebug.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/pytrinamic/RAMDebug.py b/pytrinamic/RAMDebug.py index 1fcec6e2..dfb57ba5 100644 --- a/pytrinamic/RAMDebug.py +++ b/pytrinamic/RAMDebug.py @@ -56,6 +56,12 @@ class RAMDebug_State(IntEnum): COMPLETE = 3 PRETRIGGER = 4 + UNKNOWN_STATUS = -1 # Placeholder value in case invalid state values were returned + + @classmethod + def _missing_(cls, value): + return cls.UNKNOWN_STATUS + class Channel(): def __init__(self, channel_type, value, address = 0, signed = False, mask = 0xFFFF_FFFF, shift = 0): #TODO: add signed self.value = value @@ -199,7 +205,7 @@ def start_measurement(self): self._command(RAMDebug_Command.ENABLE_TRIGGER.value, self._trigger_type.value, self._trigger_threshold) def is_measurement_done(self): - return self.get_state() == RAMDebug_State.COMPLETE.value + return self.get_state() == RAMDebug_State.COMPLETE def get_samples(self): i = 0 @@ -251,7 +257,10 @@ def channel_count(self): return len(self.channels) def get_state(self): - return self._command(RAMDebug_Command.GET_STATE.value, 0, 0).value + """ + Returns the state of this measurement as a RAMDebug_State enum + """ + return RAMDebug_State(self._command(RAMDebug_Command.GET_STATE.value, 0, 0).value) def __str__(self): text = f"RAMDebug handler for connection {self._connection}\n" From fc313b6c3a112a6bc1f28fd8c5064c6a2afc2ced Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Wed, 2 Nov 2022 10:53:42 +0100 Subject: [PATCH 26/51] RAMDebug: Cache downloaded samples instead of restarting the download --- pytrinamic/RAMDebug.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/pytrinamic/RAMDebug.py b/pytrinamic/RAMDebug.py index dfb57ba5..7ddc5fde 100644 --- a/pytrinamic/RAMDebug.py +++ b/pytrinamic/RAMDebug.py @@ -138,7 +138,7 @@ def __init__(self, connection): self._trigger_mask = 0x0000_0000 self._trigger_shift = 0x0000_0000 self.channels = [] - self.samples = [] + self.samples = None def get_sample_count(self): return self._sample_count @@ -208,6 +208,10 @@ def is_measurement_done(self): return self.get_state() == RAMDebug_State.COMPLETE def get_samples(self): + # If the samples were already downloaded, just return them + if self.samples: + return self.samples + i = 0 data = [] From 4a55d1d94c707b7f8eefa585c5af9a2261adae53 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Wed, 2 Nov 2022 10:54:27 +0100 Subject: [PATCH 27/51] RAMDebug: Added set_divider() function This function has the same purpose as set_prescaler, but should be less ambiguous regarding off-by-one errors. In practise, these errors have occurred a lot. --- pytrinamic/RAMDebug.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/pytrinamic/RAMDebug.py b/pytrinamic/RAMDebug.py index 7ddc5fde..774195ea 100644 --- a/pytrinamic/RAMDebug.py +++ b/pytrinamic/RAMDebug.py @@ -153,8 +153,22 @@ def set_process_frequency(self, process_frequency): self._process_frequency = process_frequency def set_prescaler(self, prescaler): + """ + Set the capture prescaler to divide the capture frequency. + The actual capture frequency is MAX_FREQUENCY/(prescaler+1). + """ self._prescaler = prescaler + def set_divider(self, divider): + """ + Set the capture prescaler to divide the capture frequency. + The actual capture frequency is MAX_FREQUENCY/divider. + """ + if not (1 <= divider <= 0xFFFF_FFFF): + raise ValueError("Invalid divider value. Possible divider values are [1; 2^32-1]") + + self._prescaler = divider-1 + def set_trigger_type(self, trigger_type): if isinstance(trigger_type, RAMDebug_Trigger): self._trigger_type = trigger_type From 6af2535c46dc8a4814ea99f6e990a16462fd1004 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Wed, 2 Nov 2022 10:56:17 +0100 Subject: [PATCH 28/51] RAMDebug: Fixed set_trigger_channel() not working --- pytrinamic/RAMDebug.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pytrinamic/RAMDebug.py b/pytrinamic/RAMDebug.py index 774195ea..31961331 100644 --- a/pytrinamic/RAMDebug.py +++ b/pytrinamic/RAMDebug.py @@ -179,8 +179,8 @@ def set_trigger_threshold(self, trigger_threshold): def set_trigger_channel(self, channel): if isinstance(channel, Channel): self._trigger_channel = channel - self._trigger_mask = channel._mask - self._trigger_shift = channel._shift + self._trigger_mask = channel.mask + self._trigger_shift = channel.shift def set_pretrigger_samples(self, pretrigger_samples): From 1b784f5796b06a9622d86c07381297c4fb614f57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Wed, 2 Nov 2022 10:57:34 +0100 Subject: [PATCH 29/51] RAMDebug: Added argument checks to multiple functions --- pytrinamic/RAMDebug.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/pytrinamic/RAMDebug.py b/pytrinamic/RAMDebug.py index 31961331..b791c139 100644 --- a/pytrinamic/RAMDebug.py +++ b/pytrinamic/RAMDebug.py @@ -170,23 +170,30 @@ def set_divider(self, divider): self._prescaler = divider-1 def set_trigger_type(self, trigger_type): - if isinstance(trigger_type, RAMDebug_Trigger): - self._trigger_type = trigger_type + if not isinstance(trigger_type, RAMDebug_Trigger): + raise ValueError("Invalid trigger type - you must pass a RAMDebug_Trigger object") + + self._trigger_type = trigger_type def set_trigger_threshold(self, trigger_threshold): self._trigger_threshold = trigger_threshold def set_trigger_channel(self, channel): - if isinstance(channel, Channel): - self._trigger_channel = channel - self._trigger_mask = channel.mask - self._trigger_shift = channel.shift + if not isinstance(channel, Channel): + raise ValueError("Invalid channel - you must pass a Channel object") + + self._trigger_channel = channel + self._trigger_mask = channel.mask + self._trigger_shift = channel.shift def set_pretrigger_samples(self, pretrigger_samples): self._pretrigger_samples = pretrigger_samples def set_channel(self, channel): + if not isinstance(channel, Channel): + raise ValueError("Invalid channel - you must pass a Channel object") + if self.channel_count() >= self.MAX_CHANNELS: raise RuntimeError("Out of channels!") From e18bfe11ddc8bd46075dafe0d3be038adb9d0349 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Wed, 2 Nov 2022 11:01:34 +0100 Subject: [PATCH 30/51] RAMDebug: Added helper function to fully configure the trigger --- pytrinamic/RAMDebug.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/pytrinamic/RAMDebug.py b/pytrinamic/RAMDebug.py index b791c139..92bd1439 100644 --- a/pytrinamic/RAMDebug.py +++ b/pytrinamic/RAMDebug.py @@ -186,6 +186,13 @@ def set_trigger_channel(self, channel): self._trigger_mask = channel.mask self._trigger_shift = channel.shift + def set_trigger(self, trigger_channel, trigger_type, trigger_threshold): + """ + Fully configure the RAMDebug trigger + """ + self.set_trigger_type(trigger_type) + self.set_trigger_threshold(trigger_threshold) + self.set_trigger_channel(trigger_channel) def set_pretrigger_samples(self, pretrigger_samples): self._pretrigger_samples = pretrigger_samples From 425b3c376332343b33353d1ce666a60cd4b67c90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Thu, 3 Nov 2022 13:46:18 +0100 Subject: [PATCH 31/51] RAMDebug: Added is_pretriggering() function --- pytrinamic/RAMDebug.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/pytrinamic/RAMDebug.py b/pytrinamic/RAMDebug.py index 92bd1439..34d16b95 100644 --- a/pytrinamic/RAMDebug.py +++ b/pytrinamic/RAMDebug.py @@ -232,6 +232,9 @@ def start_measurement(self): self._command(RAMDebug_Command.SET_TRIGGER_CHANNEL.value, self._trigger_channel.type.value, self._trigger_channel.value) self._command(RAMDebug_Command.ENABLE_TRIGGER.value, self._trigger_type.value, self._trigger_threshold) + def is_pretriggering(self): + return self.get_state() == RAMDebug_State.PRETRIGGER + def is_measurement_done(self): return self.get_state() == RAMDebug_State.COMPLETE From 487e32a4c9d0430546c560f18791e87096fdb27e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Thu, 3 Nov 2022 13:47:11 +0100 Subject: [PATCH 32/51] RAMDebug: Added strict option to start_measurement() --- pytrinamic/RAMDebug.py | 32 +++++++++++++++++++++++++++++--- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/pytrinamic/RAMDebug.py b/pytrinamic/RAMDebug.py index 34d16b95..50a34f4c 100644 --- a/pytrinamic/RAMDebug.py +++ b/pytrinamic/RAMDebug.py @@ -209,9 +209,35 @@ def set_channel(self, channel): def get_channels(self): return self.channels - def start_measurement(self): + def start_measurement(self, *, strict=True): + """ + Start the measurement. + If you are waiting for a trigger, wait until is_pretriggering() returns false before causing + your trigger event. + + Arguments: + - strict: + When set to True, reject invalid sample counts. + When set to False, automatically adjust too high sample counts. + """ + samples = self.get_total_samples() + if self.get_total_samples() > self.MAX_ELEMENTS: + if strict: + raise RuntimeError(f"Too many samples requested! Requested {self.get_total_samples()} ({self._sample_count} for {self.channel_count()} channels). Maximum available samples: {self.MAX_ELEMENTS}. Either adjust your sample count or pass strict=False to this function to let RAMDebug reduce sample count automatically.") + else: + # Non-strict mode: Limit the sample count + samples = self.MAX_ELEMENTS - (self.MAX_ELEMENTS % self.channel_count()) + + pretrigger_samples = self._pretrigger_samples * self.channel_count() + if pretrigger_samples > samples: + if strict: + raise RuntimeError(f"Too many pretrigger samples requested! Requested {pretrigger_samples} pretrigger samples, but only capturing {samples} samples.") + else: + # Non-strict mode: Limit the pretrigger sample count + pretrigger_samples = samples + self._command(RAMDebug_Command.INIT.value, 0, 0) - self._command(RAMDebug_Command.SET_SAMPLE_COUNT.value, 0, self.get_total_samples()) + self._command(RAMDebug_Command.SET_SAMPLE_COUNT.value, 0, samples) self._command(RAMDebug_Command.SET_PRESCALER.value, 0, self._prescaler) try: @@ -246,7 +272,7 @@ def get_samples(self): i = 0 data = [] - while i < self.get_total_samples(): + while i < min(self.get_total_samples(), self.MAX_ELEMENTS): reply = self._command(RAMDebug_Command.GET_SAMPLE.value, 0, i) done = reply.status != 0x64 if done: From b6b99cfd775e7f72e2d8c33ade63659936ac3d8b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Fri, 2 Dec 2022 14:27:06 +0100 Subject: [PATCH 33/51] FirmwareUpload: Don't send boot command when board is already in bootloader --- examples/tools/FirmwareUpdate.py | 42 +++++++++++++++++--------------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/examples/tools/FirmwareUpdate.py b/examples/tools/FirmwareUpdate.py index 45b89e88..6d100ebb 100644 --- a/examples/tools/FirmwareUpdate.py +++ b/examples/tools/FirmwareUpdate.py @@ -71,26 +71,30 @@ print("Connecting") myInterface = connectionManager.connect() -# Send the boot command -print("Switching to bootloader mode") -myInterface.send_boot(1) -myInterface.close() - -# Reconnect after a small delay -print("Reconnecting") -timestamp = time.time() -while (time.time() - timestamp) < SERIAL_BOOT_TIMEOUT: - try: - # Attempt to connect - myInterface = connectionManager.connect() - # If no exception occurred, exit the retry loop - break - except (ConnectionError, TypeError): - myInterface = None +# If not already in bootloader, enter it +if not "B" in myInterface.get_version_string().upper(): + # Send the boot command + print("Switching to bootloader mode") + myInterface.send_boot(1) + myInterface.close() + + # Reconnect after a small delay + print("Reconnecting") + timestamp = time.time() + while (time.time() - timestamp) < SERIAL_BOOT_TIMEOUT: + try: + # Attempt to connect + myInterface = connectionManager.connect() + # If no exception occurred, exit the retry loop + break + except (ConnectionError, TypeError): + myInterface = None + + if not myInterface: + print("Error: Timeout when attempting to reconnect to bootloader") + exit(1) -if not myInterface: - print("Error: Timeout when attempting to reconnect to bootloader") - exit(1) +time.sleep(1) # Retrieve the bootloader version bootloaderVersion = myInterface.get_version_string(1) From 0c18d840c39ae86abff4f858883d1e49a1607e50 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lenard=20He=C3=9F?= Date: Fri, 2 Dec 2022 14:27:23 +0100 Subject: [PATCH 34/51] FirmwareUpload: Fixed firmware upload --- examples/tools/FirmwareUpdate.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/examples/tools/FirmwareUpdate.py b/examples/tools/FirmwareUpdate.py index 6d100ebb..64747e30 100644 --- a/examples/tools/FirmwareUpdate.py +++ b/examples/tools/FirmwareUpdate.py @@ -127,6 +127,9 @@ print("Error: No matching version string found in firmware image") exit(1) +start = file.minaddr() +length = file.maxaddr() - start + print("Bootloader version: " + bootloaderVersion) print("Firmware version: " + found.group(0)) From ad920d4f2467e78f31107f1c85c17be2fa500269 Mon Sep 17 00:00:00 2001 From: Sundas Date: Mon, 5 Dec 2022 13:37:53 +0100 Subject: [PATCH 35/51] Adding another example for TMCM1276 --- .../modules/TMCM1276/TMCL/stop_switch_demo.py | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 examples/modules/TMCM1276/TMCL/stop_switch_demo.py diff --git a/examples/modules/TMCM1276/TMCL/stop_switch_demo.py b/examples/modules/TMCM1276/TMCL/stop_switch_demo.py new file mode 100644 index 00000000..a9c2fe47 --- /dev/null +++ b/examples/modules/TMCM1276/TMCL/stop_switch_demo.py @@ -0,0 +1,32 @@ +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.modules import TMCM1276 +import time + +pytrinamic.show_info() + +# This example is using PCAN, if you want to use another connection please change the next line. +connectionManager = ConnectionManager("--interface pcan_tmcl") + +with connectionManager.connect() as myInterface: + module = TMCM1276(myInterface) + motor = module.motors[0] + + print("Preparing parameters") + # preparing linear ramp settings + motor.max_acceleration = 20000 + + while 1: + if motor.get_axis_parameter(motor.AP.RightEndstop): + motor.stop() + time.sleep(5) + print("Rotating in opposite direction") + motor.rotate(-50000) + time.sleep(5) + motor.stop() + break + else: + print("Rotating") + motor.rotate(50000) + time.sleep(5) + From ac42858ae966d63cf56b55a07492071632a27e91 Mon Sep 17 00:00:00 2001 From: Sundas Date: Tue, 6 Dec 2022 13:31:38 +0100 Subject: [PATCH 36/51] Adding six-point-ramp example for TMCM1276 --- .../TMCM1276/TMCL/six_point_ramp_demo.py | 77 +++++++++++++++++++ 1 file changed, 77 insertions(+) create mode 100644 examples/modules/TMCM1276/TMCL/six_point_ramp_demo.py diff --git a/examples/modules/TMCM1276/TMCL/six_point_ramp_demo.py b/examples/modules/TMCM1276/TMCL/six_point_ramp_demo.py new file mode 100644 index 00000000..da5b0d49 --- /dev/null +++ b/examples/modules/TMCM1276/TMCL/six_point_ramp_demo.py @@ -0,0 +1,77 @@ +import dataclasses + +import matplotlib.pyplot as plt +import numpy as np +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.modules import TMCM1276 +import time +import pandas as pd + +import matplotlib + + +@dataclasses.dataclass +class Sample: + timestamp: float + position: int + velocity: int + + +pytrinamic.show_info() + +# This example is using PCAN, if you want to use another connection please change the next line. +connectionManager = ConnectionManager("--interface pcan_tmcl") + +with connectionManager.connect() as myInterface: + module = TMCM1276(myInterface) + motor = module.motors[0] + + # Setting axis parameters for configuring SixPoint ramp + motor.set_axis_parameter(motor.AP.MaxVelocity, 40000) + motor.set_axis_parameter(motor.AP.MaxAcceleration, 30000) + motor.set_axis_parameter(motor.AP.A1, 5000) + motor.set_axis_parameter(motor.AP.V1, 10000) + motor.set_axis_parameter(motor.AP.MaxDeceleration, 20000) + motor.set_axis_parameter(motor.AP.D1, 5000) + motor.set_axis_parameter(motor.AP.StartVelocity, 5000) + motor.set_axis_parameter(motor.AP.StopVelocity, 5000) + motor.set_axis_parameter(motor.AP.RampWaitTime, 31250) + + # Setting initial position to zero + motor.actual_position = 0 + + samples = [] + motor.move_to(100000) + while not (motor.get_position_reached()): + samples.append(Sample(time.perf_counter(), format(motor.actual_position), format(motor.actual_velocity))) + + motor.move_to(0) + while not (motor.get_position_reached()): + samples.append(Sample(time.perf_counter(), format(motor.actual_position), format(motor.actual_velocity))) + + fig, ax = plt.subplots(2) + t = [float(s.timestamp - samples[0].timestamp) for s in samples] + pos = [float(s.position) for s in samples] + vel = [float(s.velocity) for s in samples] + + ax[0].plot(t, pos, label='Position') + ax[0].set_title('Pos vs Time') + ax[0].set_xlabel('Time') + ax[0].set_ylabel('Pos') + ax[0].legend() + ax[0].grid() + + ax[1].plot(t, vel, label='Velocity') + ax[1].set_title('Vel vs Time') + ax[1].set_xlabel('Time') + ax[1].set_ylabel('Vel') + ax[1].legend() + ax[1].grid() + plt.show() + + + + + + From f8922e1088719463baf13e0d3730f9c7e5d06b8a Mon Sep 17 00:00:00 2001 From: Sundas Date: Tue, 6 Dec 2022 14:24:08 +0100 Subject: [PATCH 37/51] removed unnecessary imports --- examples/modules/TMCM1276/TMCL/six_point_ramp_demo.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/examples/modules/TMCM1276/TMCL/six_point_ramp_demo.py b/examples/modules/TMCM1276/TMCL/six_point_ramp_demo.py index da5b0d49..48dbc954 100644 --- a/examples/modules/TMCM1276/TMCL/six_point_ramp_demo.py +++ b/examples/modules/TMCM1276/TMCL/six_point_ramp_demo.py @@ -1,14 +1,10 @@ import dataclasses import matplotlib.pyplot as plt -import numpy as np import pytrinamic from pytrinamic.connections import ConnectionManager from pytrinamic.modules import TMCM1276 import time -import pandas as pd - -import matplotlib @dataclasses.dataclass @@ -43,11 +39,11 @@ class Sample: samples = [] motor.move_to(100000) - while not (motor.get_position_reached()): + while not motor.get_position_reached(): samples.append(Sample(time.perf_counter(), format(motor.actual_position), format(motor.actual_velocity))) motor.move_to(0) - while not (motor.get_position_reached()): + while not motor.get_position_reached(): samples.append(Sample(time.perf_counter(), format(motor.actual_position), format(motor.actual_velocity))) fig, ax = plt.subplots(2) From 4e4e3153fa01b3bf894a20ff119a0b6d0af3c760 Mon Sep 17 00:00:00 2001 From: Sundas Date: Wed, 7 Dec 2022 11:37:21 +0100 Subject: [PATCH 38/51] added new module TMCM6214 --- examples/modules/TMCM6214/TMCL/rotate_demo.py | 50 ++++ pytrinamic/modules/TMCM6214.py | 256 ++++++++++++++++++ pytrinamic/modules/__init__.py | 2 + 3 files changed, 308 insertions(+) create mode 100644 examples/modules/TMCM6214/TMCL/rotate_demo.py create mode 100644 pytrinamic/modules/TMCM6214.py diff --git a/examples/modules/TMCM6214/TMCL/rotate_demo.py b/examples/modules/TMCM6214/TMCL/rotate_demo.py new file mode 100644 index 00000000..d19b8dbf --- /dev/null +++ b/examples/modules/TMCM6214/TMCL/rotate_demo.py @@ -0,0 +1,50 @@ +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.modules import TMCM6214 +import time + +pytrinamic.show_info() + +# This example uses PCAN, if you want to use another connection please change this line. +connectionManager = ConnectionManager("--interface pcan_tmcl") +myInterface = connectionManager.connect() +module = TMCM6214(myInterface) +motor = module.motors[0] + +print("Preparing parameters") +motor.max_acceleration = 20000 + +print("Rotating") +motor.rotate(50000) + +time.sleep(5) + +print("Stopping") +motor.stop() + +print("ActualPostion = {}".format(motor.actual_position)) + +time.sleep(5) + +print("Doubling moved distance") +motor.move_by(motor.actual_position, 50000) +while not(motor.get_position_reached()): + pass + +print("Furthest point reached") +print("ActualPostion = {}".format(motor.actual_position)) + +time.sleep(5) + +print("Moving back to 0") +motor.move_to(0, 100000) + +# Wait until position 0 is reached +while not(motor.get_position_reached()): + pass + +print("Reached Position 0") + +print() + +myInterface.close() diff --git a/pytrinamic/modules/TMCM6214.py b/pytrinamic/modules/TMCM6214.py new file mode 100644 index 00000000..e9ed8ff3 --- /dev/null +++ b/pytrinamic/modules/TMCM6214.py @@ -0,0 +1,256 @@ +from ..modules import TMCLModule + +# features +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule + + +class TMCM6214(TMCLModule): + """ + The TMCM-6214 is a six axis controller/driver module. Supply voltage is 24V. + """ + def __init__(self, connection, module_id=1): + super().__init__(connection, module_id) + self.name = "TMCM-6214" + self.desc = self.__doc__ + self.motors = [self._MotorTypeA(self, 0)] + + def rotate(self, axis, velocity): + """ + Rotates the motor on the given axis with the given velocity. + + Parameters: + axis: Axis index. + velocity: Target velocity to rotate the motor with. Units are module specific. + + Returns: None + """ + self.connection.rotate(axis, velocity, self.module_id) + + def stop(self, axis): + """ + Stops the motor on the given axis. + + Parameters: + axis: Axis index. + + Returns: None + """ + self.connection.stop(axis, self.module_id) + + def move_to(self, axis, position, velocity=None): + """ + Moves the motor on the given axis to the given target position. + + Parameters: + axis: Axis index. + position: Target position to move the motor to. Units are module specific. + velocity: Maximum position velocity to position the motor. Units are module specific. + If no velocity is given, the previously configured maximum positioning velocity (AP 4) + will be used. + + Returns: None + """ + if velocity: + self.motors[axis].linear_ramp.max_velocity = velocity + self.connection.move_to(axis, position, self.module_id) + + def move_by(self, axis, difference, velocity=None): + """ + Moves the motor on the given axis by the given position difference. + + Parameters: + axis: Axis index. + difference: Position difference to move the motor by. Units are module specific. + velocity: Maximum position velocity to position the motor. Units are module specific. + If no velocity is given, the previously configured maximum positioning velocity (AP 4) + will be used. + + Returns: None + """ + if velocity: + self.motors[axis].linear_ramp.max_velocity = velocity + self.connection.move_by(axis, difference, self.module_id) + + class _MotorTypeA(MotorControlModule): + """ + Motor class for the motor on axis 0. + """ + def __init__(self, module, axis): + MotorControlModule.__init__(self, module, axis, self.AP) + self.drive_settings = DriveSettingModule(module, axis, self.AP) + self.linear_ramp = LinearRampModule(module, axis, self.AP) + self.stallguard2 = StallGuard2Module(module, axis, self.AP) + self.coolstep = CoolStepModule(module, axis, self.AP, self.stallguard2) + + def get_position_reached(self): + """ + Indicates whether a positioning task has been completed. + + Returns: + 1, if target position has been reached. + 0, if target position has not been reached. + """ + return self.get_axis_parameter(self.AP.PositionReachedFlag) + + class AP: + # Axis parameter map for this axis. + TargetPosition = 0 + ActualPosition = 1 + TargetVelocity = 2 + ActualVelocity = 3 + MaxVelocity = 4 + MaxAcceleration = 5 + RunCurrent = 6 + StandbyCurrent = 7 + PositionReachedFlag = 8 + HomeSwitch = 9 + RightEndstop = 10 + LeftEndstop = 11 + RightLimitSwitchDisable = 12 + LeftLimitSwitchDisable = 13 + SwapLimitSwitches = 14 + A1 = 15 + V1 = 16 + MaxDeceleration = 17 + D1 = 18 + StartVelocity = 19 + StopVelocity = 20 + RampWaitTime = 21 + HighSpeedTheshold = 22 + MinDcStepSpeed = 23 + RightSwitchPolarity = 24 + LeftSwitchPolarity = 25 + Softstop = 26 + HighSpeedChopperMode = 27 + HighSpeedFullstepMode = 28 + MeasuredSpeed = 29 + PowerDownRamp = 31 + DcStepTime = 32 + DcStepStallGuard = 33 + PositionCompareStart = 40 + PositionCompareDistance = 41 + PositionCompareOutput = 42 + PositionCompareOutputPulseLength = 43 + RelativePositioningOptionCode = 127 + MicrostepResolution = 140 + ChopperBlankTime = 162 + ConstantTOffMode = 163 + DisableFastDecayComparator = 164 + ChopperHysteresisEnd = 165 + ChopperHysteresisStart = 166 + TOff = 167 + SEIMIN = 168 + SECDS = 169 + SmartEnergyHysteresis = 170 + SECUS = 171 + SmartEnergyHysteresisStart = 172 + SG2FilterEnable = 173 + SG2Threshold = 174 + ShortToGroundProtection = 177 + VSense = 179 + SmartEnergyActualCurrent = 180 + SmartEnergyStallVelocity = 181 + SmartEnergyThresholdSpeed = 182 + RandomTOffMode = 184 + ChopperSynchronization = 185 + PWMThresholdSpeed = 186 + PWMGrad = 187 + PWMAmplitude = 188 + PWMScale = 189 + PWMMode = 190 + PWMFrequency = 191 + PWMAutoscale = 192 + ReferenceSearchMode = 193 + ReferenceSearchSpeed = 194 + RefSwitchSpeed = 195 + RightLimitSwitchPosition = 196 + LastReferencePosition = 197 + LatchedActualPosition = 198 + LatchedEncoderPosition = 199 + EncoderMode = 201 + MotorFullStepResolution = 202 + FreewheelingMode = 204 + LoadValue = 206 + ErrorFlags = 207 + StatusFlags = 208 + EncoderPosition = 209 + EncoderResolution = 210 + EncoderDeviationMax = 212 + GroupIndex = 213 + PowerDownDelay = 214 + DeviationAction = 240 + ReverseShaft = 251 + UnitMode = 255 + + class ENUM: + """ + Constant enums for parameters of this module. + """ + MicrostepResolutionFullstep = 0 + MicrostepResolutionHalfstep = 1 + MicrostepResolution4Microsteps = 2 + MicrostepResolution8Microsteps = 3 + MicrostepResolution16Microsteps = 4 + MicrostepResolution32Microsteps = 5 + MicrostepResolution64Microsteps = 6 + MicrostepResolution128Microsteps = 7 + MicrostepResolution256Microsteps = 8 + + class GP0: + """ + Global parameter map for this module. + """ + RS485Baudrate = 65 + SerialAddress = 66 + SerialHeartbeat = 68 + CANBitrate = 69 + CANSendId = 70 + CANReceiveId = 71 + CANSecondaryId = 72 #not in datasheet + TelegramPauseTime = 75 + SerialHostAddress = 76 + AutoStartMode = 77 + ProtectionMode = 81 + CANHeartbeat = 82 + CANSecondaryAddress = 83 + EepromCoordinateStore = 84 + ZeroUserVariables = 85 + SerialSecondaryAddress = 86 + ApplicationStatus = 128 + DownloadMode = 129 + ProgramCounter = 130 + TickTimer = 132 + RandomNumber = 133 + SuppressReply = 255 + + class GP3: + Timer_0 = 0 + Timer_1 = 1 + Timer_2 = 2 + StopLeft_0 = 27 + StopRight_0 = 28 + StopLeft_1 = 29 + StopRight_1 = 30 + StopLeft_2 = 31 + StopRight_2 = 32 + StopLeft_3 = 33 + StopRight_3 = 34 + StopLeft_4 = 35 + StopRight_4 = 36 + StopLeft_5 = 37 + StopRight_5 = 38 + Input_0 = 39 + Input_1 = 40 + Input_2 = 41 + Input_3 = 42 + Input_4 = 43 + Input_5 = 44 + Input_6 = 45 + Input_7 = 46 + + + class IO: + IN0 = 0 + IN1 = 1 + IN2 = 2 diff --git a/pytrinamic/modules/__init__.py b/pytrinamic/modules/__init__.py index ef54fd69..b3f7fa93 100644 --- a/pytrinamic/modules/__init__.py +++ b/pytrinamic/modules/__init__.py @@ -22,3 +22,5 @@ from .TMCM3351 import TMCM3351 from .TMCM6110 import TMCM6110 from .TMCM6212 import TMCM6212 +from .TMCM6214 import TMCM6214 + From cafd6cdd12c94b14a8ea72e88a7b06c2a72d07f5 Mon Sep 17 00:00:00 2001 From: Sundas Date: Wed, 7 Dec 2022 13:40:34 +0100 Subject: [PATCH 39/51] added missing IO variables in class IO --- examples/modules/TMCM6214/TMCL/rotate_demo.py | 55 +++++++++---------- pytrinamic/modules/TMCM6214.py | 25 +++++++-- 2 files changed, 47 insertions(+), 33 deletions(-) diff --git a/examples/modules/TMCM6214/TMCL/rotate_demo.py b/examples/modules/TMCM6214/TMCL/rotate_demo.py index d19b8dbf..0802b1b5 100644 --- a/examples/modules/TMCM6214/TMCL/rotate_demo.py +++ b/examples/modules/TMCM6214/TMCL/rotate_demo.py @@ -4,47 +4,44 @@ import time pytrinamic.show_info() +connectionManager = ConnectionManager() -# This example uses PCAN, if you want to use another connection please change this line. -connectionManager = ConnectionManager("--interface pcan_tmcl") -myInterface = connectionManager.connect() -module = TMCM6214(myInterface) -motor = module.motors[0] +with connectionManager.connect() as myInterface: + module = TMCM6214(myInterface) + motor_0 = module.motors[0] -print("Preparing parameters") -motor.max_acceleration = 20000 + print("Preparing parameters") + motor_0.max_acceleration = 20000 -print("Rotating") -motor.rotate(50000) + print("Rotating") + motor_0.rotate(50000) -time.sleep(5) + time.sleep(5) -print("Stopping") -motor.stop() + print("Stopping") + motor_0.stop() -print("ActualPostion = {}".format(motor.actual_position)) + print("ActualPostion = {}".format(motor_0.actual_position)) -time.sleep(5) + time.sleep(5) -print("Doubling moved distance") -motor.move_by(motor.actual_position, 50000) -while not(motor.get_position_reached()): - pass + print("Doubling moved distance") + motor_0.move_by(motor_0.actual_position, 50000) + while not(motor_0.get_position_reached()): + pass -print("Furthest point reached") -print("ActualPostion = {}".format(motor.actual_position)) + print("Furthest point reached") + print("ActualPostion = {}".format(motor_0.actual_position)) -time.sleep(5) + time.sleep(5) -print("Moving back to 0") -motor.move_to(0, 100000) + print("Moving back to 0") + motor_0.move_to(0, 100000) -# Wait until position 0 is reached -while not(motor.get_position_reached()): - pass + # Wait until position 0 is reached + while not(motor_0.get_position_reached()): + pass -print("Reached Position 0") + print("Reached Position 0") -print() -myInterface.close() diff --git a/pytrinamic/modules/TMCM6214.py b/pytrinamic/modules/TMCM6214.py index e9ed8ff3..72f29fec 100644 --- a/pytrinamic/modules/TMCM6214.py +++ b/pytrinamic/modules/TMCM6214.py @@ -13,7 +13,8 @@ def __init__(self, connection, module_id=1): super().__init__(connection, module_id) self.name = "TMCM-6214" self.desc = self.__doc__ - self.motors = [self._MotorTypeA(self, 0)] + self.motors = [self._MotorTypeA(self, 0), self._MotorTypeA(self, 1), self._MotorTypeA(self, 2), + self._MotorTypeA(self, 3), self._MotorTypeA(self, 4), self._MotorTypeA(self, 5)] def rotate(self, axis, velocity): """ @@ -251,6 +252,22 @@ class GP3: class IO: - IN0 = 0 - IN1 = 1 - IN2 = 2 + OUT0 = 0 + OUT1 = 1 + OUT2 = 2 + OUT3 = 3 + OUT4 = 4 + OUT5 = 5 + OUT6 = 6 + OUT7 = 7 + AIN0 = 0 + IN1 = 1 + IN2 = 2 + IN3 = 3 + AIN4 = 4 + IN5 = 5 + IN6 = 6 + IN7 = 7 + STO = 10 + STO1 = 13 + STO2 = 14 From 263617c5bb67907baa8fb1278d7531bfe5456ed8 Mon Sep 17 00:00:00 2001 From: Sundas Date: Wed, 7 Dec 2022 13:51:22 +0100 Subject: [PATCH 40/51] added new example for TMCM6214 --- .../TMCM6214/TMCL/six_point_ramp_demo.py | 66 +++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 examples/modules/TMCM6214/TMCL/six_point_ramp_demo.py diff --git a/examples/modules/TMCM6214/TMCL/six_point_ramp_demo.py b/examples/modules/TMCM6214/TMCL/six_point_ramp_demo.py new file mode 100644 index 00000000..13c34d27 --- /dev/null +++ b/examples/modules/TMCM6214/TMCL/six_point_ramp_demo.py @@ -0,0 +1,66 @@ +import dataclasses +import matplotlib.pyplot as plt +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.modules import TMCM6214 +import time + + +@dataclasses.dataclass +class Sample: + timestamp: float + position: int + velocity: int + + +pytrinamic.show_info() +# This example is using USB. +connectionManager = ConnectionManager() + +with connectionManager.connect() as myInterface: + module = TMCM6214(myInterface) + motor = module.motors[0] + + # Setting axis parameters for configuring SixPoint ramp + motor.set_axis_parameter(motor.AP.MaxVelocity, 40000) + motor.set_axis_parameter(motor.AP.MaxAcceleration, 30000) + motor.set_axis_parameter(motor.AP.A1, 5000) + motor.set_axis_parameter(motor.AP.V1, 10000) + motor.set_axis_parameter(motor.AP.MaxDeceleration, 20000) + motor.set_axis_parameter(motor.AP.D1, 5000) + motor.set_axis_parameter(motor.AP.StartVelocity, 5000) + motor.set_axis_parameter(motor.AP.StopVelocity, 5000) + motor.set_axis_parameter(motor.AP.RampWaitTime, 31250) + + # Setting initial position to zero + motor.actual_position = 0 + + samples = [] + motor.move_to(100000) + while not motor.get_position_reached(): + samples.append(Sample(time.perf_counter(), format(motor.actual_position), format(motor.actual_velocity))) + + motor.move_to(0) + while not motor.get_position_reached(): + samples.append(Sample(time.perf_counter(), format(motor.actual_position), format(motor.actual_velocity))) + + fig, ax = plt.subplots(2) + t = [float(s.timestamp - samples[0].timestamp) for s in samples] + pos = [float(s.position) for s in samples] + vel = [float(s.velocity) for s in samples] + + ax[0].plot(t, pos, label='Position') + ax[0].set_title('Pos vs Time') + ax[0].set_xlabel('Time') + ax[0].set_ylabel('Pos') + ax[0].legend() + ax[0].grid() + + ax[1].plot(t, vel, label='Velocity') + ax[1].set_title('Vel vs Time') + ax[1].set_xlabel('Time') + ax[1].set_ylabel('Vel') + ax[1].legend() + ax[1].grid() + plt.show() + From 48fd2ac73807239e16606f739b4cb4abad420af0 Mon Sep 17 00:00:00 2001 From: Sundas Date: Wed, 7 Dec 2022 15:34:12 +0100 Subject: [PATCH 41/51] added new module TMCM1231 --- examples/modules/TMCM1231/TMCL/rotate_demo.py | 68 +++++++ pytrinamic/modules/TMCM1231.py | 178 ++++++++++++++++++ pytrinamic/modules/__init__.py | 2 + 3 files changed, 248 insertions(+) create mode 100644 examples/modules/TMCM1231/TMCL/rotate_demo.py create mode 100644 pytrinamic/modules/TMCM1231.py diff --git a/examples/modules/TMCM1231/TMCL/rotate_demo.py b/examples/modules/TMCM1231/TMCL/rotate_demo.py new file mode 100644 index 00000000..c4eeed1f --- /dev/null +++ b/examples/modules/TMCM1231/TMCL/rotate_demo.py @@ -0,0 +1,68 @@ +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.modules import TMCM1231 +import time + +pytrinamic.show_info() + +# This example is using PCAN, if you want to use another connection please change the next line. +connectionManager = ConnectionManager("--interface pcan_tmcl") +with connectionManager.connect() as myInterface: + module = TMCM1231(myInterface) + motor = module.motors[0] + + # Please be sure not to use a too high current setting for your motor. + + print("Preparing parameters") + # preparing drive settings + motor.drive_settings.max_current = 128 + motor.drive_settings.standby_current = 0 + motor.drive_settings.boost_current = 0 + motor.drive_settings.microstep_resolution = motor.ENUM.microstep_resolution_256_microsteps + print(motor.drive_settings) + + + # preparing linear ramp settings + motor.max_acceleration = 51200 + motor.max_velocity = 51200 + + # reset actual position + motor.actual_position = 0 + + print(motor.linear_ramp) + + # start rotating motor in different directions + print("Rotating") + motor.rotate(51200) + time.sleep(5) + + # stop rotating motor + print("Stopping") + motor.stop() + + # read actual position + print("ActualPostion = {}".format(motor.actual_position)) + time.sleep(2) + + # doubling moved distance + print("Doubling moved distance") + motor.move_by(motor.actual_position) + + # wait till position_reached + while not(motor.get_position_reached()): + print("target position motor: " + str(motor.target_position) + " actual position motor: " + str(motor.actual_position)) + + time.sleep(0.2) + print("Furthest point reached") + print("ActualPostion motor = {}".format(motor.actual_position)) + + # short delay and move back to start + time.sleep(2) + print("Moving back to 0") + motor.move_to(0) + + # wait until position 0 is reached + while not(motor.get_position_reached()): + print("target position motor: " + str(motor.target_position) + " actual position motor: " + str(motor.actual_position)) + + print("Reached Position 0") diff --git a/pytrinamic/modules/TMCM1231.py b/pytrinamic/modules/TMCM1231.py new file mode 100644 index 00000000..7890be97 --- /dev/null +++ b/pytrinamic/modules/TMCM1231.py @@ -0,0 +1,178 @@ +from ..modules import TMCLModule + +# features +from ..features import MotorControlModule, DriveSettingModule, LinearRampModule +from ..features import StallGuard2Module, CoolStepModule + + +class TMCM1231(TMCLModule): + """ + The TMCM-1231 is a single axis controller/driver module for 2-phase bipolar stepper motors. + """ + def __init__(self, connection, module_id=1): + super().__init__(connection, module_id) + self.name = "TMCM-1231" + self.desc = self.__doc__ + self.motors = [self._MotorTypeA(self, 0)] + + def rotate(self, axis, velocity): + self.connection.rotate(axis, velocity, self.module_id) + + def stop(self, axis): + self.connection.stop(axis, self.module_id) + + def move_to(self, axis, position, velocity=None): + if velocity: + self.motors[axis].linear_ramp.max_velocity = velocity + self.connection.move_to(axis, position, self.module_id) + + def move_by(self, axis, difference, velocity=None): + if velocity: + self.motors[axis].linear_ramp.max_velocity = velocity + self.connection.move_by(axis, difference, self.module_id) + + class _MotorTypeA(MotorControlModule): + + def __init__(self, module, axis): + MotorControlModule.__init__(self, module, axis, self.AP) + self.drive_settings = DriveSettingModule(module, axis, self.AP) + self.linear_ramp = LinearRampModule(module, axis, self.AP) + self.stallguard2 = StallGuard2Module(module, axis, self.AP) + self.coolstep = CoolStepModule(module, axis, self.AP, self.stallguard2) + + def get_position_reached(self): + return self.get_axis_parameter(self.AP.PositionReachedFlag) + + class AP: + TargetPosition = 0 + ActualPosition = 1 + TargetVelocity = 2 + ActualVelocity = 3 + MaxVelocity = 4 + MaxAcceleration = 5 + RunCurrent = 6 + StandbyCurrent = 7 + PositionReachedFlag = 8 + HomeSwitch = 9 + RightEndstop = 10 + LeftEndstop = 11 + RightLimit = 12 + LeftLimit = 13 + RampType = 14 + StartVelocity = 15 + StartAcceleration = 16 + MaxDeceleration = 17 + BreakVelocity = 18 + FinalDeceleration = 19 + StopVelocity = 20 + StopDeceleration = 21 + Bow1 = 22 + Bow2 = 23 + Bow3 = 24 + Bow4 = 25 + VirtualStopLeft = 26 + VirtualStopRight = 27 + VirtualStopEnable = 28 + VirtualStopMode = 29 + SwapStopSwitches = 33 + EnableSoftStop = 34 + RelativePositioningOption = 127 + MicrostepResolution = 140 + ChopperBlankTime = 162 + ConstantTOffMode = 163 + DisableFastDecayComparator = 164 + ChopperHysteresisEnd = 165 + ChopperHysteresisStart = 166 + TOff = 167 + SEIMIN = 168 + SECDS = 169 + SmartEnergyHysteresis = 170 + SECUS = 171 + SmartEnergyHysteresisStart = 172 + SG2FilterEnable = 173 + SG2Threshold = 174 + SmartEnergyActualCurrent = 180 + SmartEnergyStallVelocity = 181 + SmartEnergyThresholdSpeed = 182 + RandomTOffMode = 184 + ChopperSynchronization = 185 + PWMThresholdSpeed = 186 + PWMGrad = 187 + PWMAmplitude = 188 + PWMScale = 189 + PWMMode = 190 + PWMFrequency = 191 + PWMAutoscale = 192 + ReferenceSearchMode = 193 + ReferenceSearchSpeed = 194 + RefSwitchSpeed = 195 + EndSwitchDistance = 196 + LastReferencePosition = 197 + LatchedActualPosition = 198 + LatchedEncoderPosition = 199 + BoostCurrent = 200 + EncoderMode = 201 + MotorFullStepResolution = 202 + FreewheelingMode = 204 + LoadValue = 206 + ExtendedErrorFlags = 207 + DriverErrorFlags = 208 + EncoderPosition = 209 + EncoderResolution = 210 + MaxPositionEncoderDeviation = 212 + MaxVelocityEncoderDeviation = 213 + PowerDownDelay = 214 + ReverseShaft = 251 + StepDirectionMode = 254 + + class ENUM: + microstep_resolution_fullstep = 0 + microstep_resolution_halfstep = 1 + microstep_resolution_4_microsteps = 2 + microstep_resolution_8_microsteps = 3 + microstep_resolution_16_microsteps = 4 + microstep_resolution_32_microsteps = 5 + microstep_resolution_64_microsteps = 6 + microstep_resolution_128_microsteps = 7 + microstep_resolution_256_microsteps = 8 + + class GP0: + SerialBaudRate = 65 + SerialAddress = 66 + SerialHearbeat = 68 + CANBitRate = 69 + CANsendID = 70 + CANreceiveID = 71 + TelegramPauseTime = 75 + SerialHostAddress = 76 + AutoStartMode = 77 + TMCLCodeProtection = 81 + CANHeartbeat = 82 + CANSecondaryAddress = 83 + eepromCoordinateStore = 84 + zeroUserVariables = 85 + serialSecondaryAddress = 87 + ApplicationStatus = 128 + DownloadMode = 129 + ProgramCounter = 130 + TickTimer = 132 + RandomNumber = 133 + SuppressReply = 255 + + class GP3: + timer_0 = 0 + timer_1 = 1 + timer_2 = 2 + stopLeft_0 = 27 + stopRight_0 = 28 + input_0 = 39 + input_1 = 40 + input_2 = 41 + input_3 = 42 + + class IO: + GPO0 = 0 + AIN0 = 0 + GPI0 = 2 + GPI1 = 3 + diff --git a/pytrinamic/modules/__init__.py b/pytrinamic/modules/__init__.py index ef54fd69..9868824a 100644 --- a/pytrinamic/modules/__init__.py +++ b/pytrinamic/modules/__init__.py @@ -22,3 +22,5 @@ from .TMCM3351 import TMCM3351 from .TMCM6110 import TMCM6110 from .TMCM6212 import TMCM6212 +from .TMCM1231 import TMCM1231 + From f7a0051280955ba90757d07a7537bbe3208074a6 Mon Sep 17 00:00:00 2001 From: Sundas Date: Fri, 9 Dec 2022 18:22:27 +0100 Subject: [PATCH 42/51] added new example stallGuard2 demo --- .../modules/TMCM6214/TMCL/StallGuard2_demo.py | 70 +++++++++++++++++++ pytrinamic/modules/TMCM6214.py | 3 +- 2 files changed, 72 insertions(+), 1 deletion(-) create mode 100644 examples/modules/TMCM6214/TMCL/StallGuard2_demo.py diff --git a/examples/modules/TMCM6214/TMCL/StallGuard2_demo.py b/examples/modules/TMCM6214/TMCL/StallGuard2_demo.py new file mode 100644 index 00000000..3eea8e12 --- /dev/null +++ b/examples/modules/TMCM6214/TMCL/StallGuard2_demo.py @@ -0,0 +1,70 @@ +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.modules import TMCM6214 +import time +import numpy as np + +def stallguard2_init(init_velocity): + motor.stallguard2.set_threshold(0) + motor.stallguard2.stop_velocity = 0 + print("Rotating...") + motor.rotate(init_velocity) + sgthresh = 0 + sgt = 0 + array = [] + while (sgt == 0) and (sgthresh < 64): + array = [] + motor.stallguard2.set_threshold(sgthresh) + time.sleep(0.2) + sgthresh += 1 + for i in range(50): + array.append(motor.stallguard2.get_load_value()) + print(sgthresh) + print(array) + if not np.any(array): + sgt = 0 + else: + sgt = np.max(array) + motor.stallguard2.set_threshold(sgthresh - 1) + while 1: + array = [] + for i in range(50): + array.append(motor.stallguard2.get_load_value()) + print(array) + if 0 in array or np.max(array) > 450: + motor.drive_settings.max_current = motor.drive_settings.max_current - 8 + else: + break + + motor.stallguard2.stop_velocity = motor.get_actual_velocity() - 1 + + +pytrinamic.show_info() + +connectionManager = ConnectionManager() +with connectionManager.connect() as myInterface: + module = TMCM6214(myInterface) + motor = module.motors[0] + + + print("Preparing parameters") + # preparing drive settings + motor.drive_settings.max_current = 128 + motor.drive_settings.standby_current = 8 + motor.drive_settings.boost_current = 0 + motor.drive_settings.microstep_resolution = motor.ENUM.MicrostepResolution256Microsteps + print(motor.drive_settings) + + print(motor.linear_ramp) + + time.sleep(1.0) + + # clear position counter + motor.actual_position = 0 + + # set up StallGuard2 + print("Initial StallGuard2 values:") + print(motor.stallguard2) + stallguard2_init(10000) + + diff --git a/pytrinamic/modules/TMCM6214.py b/pytrinamic/modules/TMCM6214.py index 72f29fec..016c7a91 100644 --- a/pytrinamic/modules/TMCM6214.py +++ b/pytrinamic/modules/TMCM6214.py @@ -102,7 +102,8 @@ class AP: ActualVelocity = 3 MaxVelocity = 4 MaxAcceleration = 5 - RunCurrent = 6 + MaxCurrent = 6 + RunCurrent = MaxCurrent StandbyCurrent = 7 PositionReachedFlag = 8 HomeSwitch = 9 From 42cfc4b7d560d483c7e0da4040cfa8ee6fcf4b79 Mon Sep 17 00:00:00 2001 From: Sundas Date: Mon, 12 Dec 2022 14:56:25 +0100 Subject: [PATCH 43/51] added new example StallGuard2 for TMCM1231 --- .../modules/TMCM1231/TMCL/StallGuard2_demo.py | 69 +++++++++++++++++++ pytrinamic/modules/TMCM1231.py | 3 +- 2 files changed, 71 insertions(+), 1 deletion(-) create mode 100644 examples/modules/TMCM1231/TMCL/StallGuard2_demo.py diff --git a/examples/modules/TMCM1231/TMCL/StallGuard2_demo.py b/examples/modules/TMCM1231/TMCL/StallGuard2_demo.py new file mode 100644 index 00000000..d923739f --- /dev/null +++ b/examples/modules/TMCM1231/TMCL/StallGuard2_demo.py @@ -0,0 +1,69 @@ +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.modules import TMCM1231 +import time +import numpy as np + +def stallguard2_init(init_velocity): + motor.stallguard2.set_threshold(0) + motor.stallguard2.stop_velocity = 0 + print("Rotating...") + motor.rotate(init_velocity) + sgthresh = 0 + sgt = 0 + array = [] + while (sgt == 0) and (sgthresh < 64): + array = [] + motor.stallguard2.set_threshold(sgthresh) + time.sleep(0.2) + sgthresh += 1 + for i in range(50): + array.append(motor.stallguard2.get_load_value()) + print(sgthresh) + print(array) + if not np.any(array): + sgt = 0 + else: + sgt = np.max(array) + motor.stallguard2.set_threshold(sgthresh - 1) + while 1: + array = [] + for i in range(50): + array.append(motor.stallguard2.get_load_value()) + print(array) + if 0 in array: + motor.drive_settings.max_current = motor.drive_settings.max_current - 1 + else: + break + + motor.stallguard2.stop_velocity = motor.get_actual_velocity() - 1 + + +pytrinamic.show_info() + +# This example is using PCAN, if you want to use another connection please change the next line. +connectionManager = ConnectionManager("--interface pcan_tmcl") +with connectionManager.connect() as myInterface: + module = TMCM1231(myInterface) + motor = module.motors[0] + + + print("Preparing parameters") + # preparing drive settings + motor.drive_settings.max_current = 20 + motor.drive_settings.standby_current = 8 + motor.drive_settings.boost_current = 0 + motor.drive_settings.microstep_resolution = motor.ENUM.microstep_resolution_256_microsteps + print(motor.drive_settings) + + print(motor.linear_ramp) + + time.sleep(1.0) + + # clear position counter + motor.actual_position = 0 + + # set up StallGuard2 + print("Initial StallGuard2 values:") + print(motor.stallguard2) + stallguard2_init(10000) diff --git a/pytrinamic/modules/TMCM1231.py b/pytrinamic/modules/TMCM1231.py index 7890be97..68042100 100644 --- a/pytrinamic/modules/TMCM1231.py +++ b/pytrinamic/modules/TMCM1231.py @@ -50,7 +50,8 @@ class AP: ActualVelocity = 3 MaxVelocity = 4 MaxAcceleration = 5 - RunCurrent = 6 + MaxCurrent = 6 + RunCurrent = MaxCurrent StandbyCurrent = 7 PositionReachedFlag = 8 HomeSwitch = 9 From 6e36e0af0ce5e5ce5b75a95a5293f403f312b12f Mon Sep 17 00:00:00 2001 From: Sundas Date: Mon, 12 Dec 2022 15:21:39 +0100 Subject: [PATCH 44/51] added new example for StallGuard2 feature --- .../modules/TMCM1240/TMCL/StallGuard2_demo.py | 68 +++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 examples/modules/TMCM1240/TMCL/StallGuard2_demo.py diff --git a/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py b/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py new file mode 100644 index 00000000..1053e311 --- /dev/null +++ b/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py @@ -0,0 +1,68 @@ +import pytrinamic +from pytrinamic.connections import ConnectionManager +from pytrinamic.modules import TMCM1240 +import time +import numpy as np + +def stallguard2_init(init_velocity): + motor.stallguard2.set_threshold(0) + motor.stallguard2.stop_velocity = 0 + print("Rotating...") + motor.rotate(init_velocity) + sgthresh = 0 + sgt = 0 + array = [] + while (sgt == 0) and (sgthresh < 64): + array = [] + motor.stallguard2.set_threshold(sgthresh) + time.sleep(0.2) + sgthresh += 1 + for i in range(50): + array.append(motor.stallguard2.get_load_value()) + print(sgthresh) + print(array) + if not np.any(array): + sgt = 0 + else: + sgt = np.max(array) + motor.stallguard2.set_threshold(sgthresh - 1) + while 1: + array = [] + for i in range(50): + array.append(motor.stallguard2.get_load_value()) + print(array) + if 0 in array: + motor.drive_settings.max_current = motor.drive_settings.max_current - 8 + else: + break + + motor.stallguard2.stop_velocity = motor.get_actual_velocity() - 1 + + +pytrinamic.show_info() + +connectionManager = ConnectionManager() +with connectionManager.connect() as myInterface: + module = TMCM1240(myInterface) + motor = module.motors[0] + + + print("Preparing parameters") + # preparing drive settings + motor.drive_settings.max_current = 60 + motor.drive_settings.standby_current = 8 + motor.drive_settings.boost_current = 0 + motor.drive_settings.microstep_resolution = motor.ENUM.MicrostepResolution256Microsteps + print(motor.drive_settings) + + print(motor.linear_ramp) + + time.sleep(1.0) + + # clear position counter + motor.actual_position = 0 + + # set up StallGuard2 + print("Initial StallGuard2 values:") + print(motor.stallguard2) + stallguard2_init(10000) From a7a2c50ec3b75dcc026e3d06cab558081c1b4bfc Mon Sep 17 00:00:00 2001 From: Sundas Date: Tue, 13 Dec 2022 14:23:41 +0100 Subject: [PATCH 45/51] committing final version of StallGuard2_demo.py --- .../modules/TMCM1240/TMCL/StallGuard2_demo.py | 83 +++++++++++-------- 1 file changed, 49 insertions(+), 34 deletions(-) diff --git a/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py b/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py index 1053e311..52f83002 100644 --- a/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py +++ b/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py @@ -1,68 +1,83 @@ +""" +Sets the StallGuard2 threshold such that the stall guard value (i.e SG value) is zero +when the motor comes close to stall and also sets the stop on stall velocity to a value +one less than the actual velocity of the moto +:param motor: TMCL motor object +:param init_velocity: initial velocity to rotate the motor. +""" + import pytrinamic from pytrinamic.connections import ConnectionManager from pytrinamic.modules import TMCM1240 import time -import numpy as np -def stallguard2_init(init_velocity): +def stallguard2_init(motor, init_velocity): + # Resetting SG2 threshold and stop on stall velocity to zero motor.stallguard2.set_threshold(0) motor.stallguard2.stop_velocity = 0 + print("Initial StallGuard2 values:") + print(motor.stallguard2) print("Rotating...") motor.rotate(init_velocity) sgthresh = 0 sgt = 0 - array = [] + load_samples = [] while (sgt == 0) and (sgthresh < 64): - array = [] + load_samples = [] motor.stallguard2.set_threshold(sgthresh) time.sleep(0.2) sgthresh += 1 for i in range(50): - array.append(motor.stallguard2.get_load_value()) - print(sgthresh) - print(array) - if not np.any(array): + load_samples.append(motor.stallguard2.get_load_value()) + + if not any(load_samples): sgt = 0 else: - sgt = np.max(array) - motor.stallguard2.set_threshold(sgthresh - 1) + sgt = max(load_samples) while 1: - array = [] + load_samples = [] for i in range(50): - array.append(motor.stallguard2.get_load_value()) - print(array) - if 0 in array: + load_samples.append(motor.stallguard2.get_load_value()) + if 0 in load_samples: motor.drive_settings.max_current = motor.drive_settings.max_current - 8 else: break motor.stallguard2.stop_velocity = motor.get_actual_velocity() - 1 + print("Configured StallGuard2 parameters:") + print(motor.stallguard2) +def main(): + pytrinamic.show_info() -pytrinamic.show_info() + connection_manager = ConnectionManager() + with connection_manager.connect() as my_interface: + module = TMCM1240(my_interface) + motor = module.motors[0] -connectionManager = ConnectionManager() -with connectionManager.connect() as myInterface: - module = TMCM1240(myInterface) - motor = module.motors[0] + print("Preparing parameters") + # preparing drive settings + motor.drive_settings.max_current = 60 + motor.drive_settings.standby_current = 8 + motor.drive_settings.boost_current = 0 + motor.drive_settings.microstep_resolution = motor.ENUM.MicrostepResolution256Microsteps - print("Preparing parameters") - # preparing drive settings - motor.drive_settings.max_current = 60 - motor.drive_settings.standby_current = 8 - motor.drive_settings.boost_current = 0 - motor.drive_settings.microstep_resolution = motor.ENUM.MicrostepResolution256Microsteps - print(motor.drive_settings) + print(motor.drive_settings) + print(motor.linear_ramp) - print(motor.linear_ramp) + time.sleep(1.0) - time.sleep(1.0) + # clear position counter + motor.actual_position = 0 - # clear position counter - motor.actual_position = 0 + # set up StallGuard2 + print("Configuring StallGuard2 parameters...") + stallguard2_init(motor,init_velocity = 10000) + print("Apply load and try to stall the motor...") + while not (motor.actual_velocity == 0): + pass + print("Motor stopped by StallGuard2!") - # set up StallGuard2 - print("Initial StallGuard2 values:") - print(motor.stallguard2) - stallguard2_init(10000) +if __name__ == "__main__": + main() \ No newline at end of file From ab98ad840711bcc35aabbc5668b1000c02c15ddd Mon Sep 17 00:00:00 2001 From: Sundas Date: Tue, 13 Dec 2022 14:36:23 +0100 Subject: [PATCH 46/51] made cosmetic changes to the code --- .../modules/TMCM1231/TMCL/StallGuard2_demo.py | 83 ++++++++++--------- 1 file changed, 46 insertions(+), 37 deletions(-) diff --git a/examples/modules/TMCM1231/TMCL/StallGuard2_demo.py b/examples/modules/TMCM1231/TMCL/StallGuard2_demo.py index d923739f..36077618 100644 --- a/examples/modules/TMCM1231/TMCL/StallGuard2_demo.py +++ b/examples/modules/TMCM1231/TMCL/StallGuard2_demo.py @@ -1,69 +1,78 @@ +""" +Sets the StallGuard2 threshold such that the stall guard value (i.e SG value) is zero +when the motor comes close to stall and also sets the stop on stall velocity to a value +one less than the actual velocity of the motor +""" import pytrinamic from pytrinamic.connections import ConnectionManager from pytrinamic.modules import TMCM1231 import time -import numpy as np -def stallguard2_init(init_velocity): +def stallguard2_init(motor, init_velocity): + # Resetting SG2 threshold and stop on stall velocity to zero motor.stallguard2.set_threshold(0) motor.stallguard2.stop_velocity = 0 + print("Initial StallGuard2 values:") + print(motor.stallguard2) print("Rotating...") motor.rotate(init_velocity) sgthresh = 0 sgt = 0 - array = [] + load_samples = [] while (sgt == 0) and (sgthresh < 64): - array = [] + load_samples = [] motor.stallguard2.set_threshold(sgthresh) time.sleep(0.2) sgthresh += 1 for i in range(50): - array.append(motor.stallguard2.get_load_value()) - print(sgthresh) - print(array) - if not np.any(array): + load_samples.append(motor.stallguard2.get_load_value()) + if not any(load_samples): sgt = 0 else: - sgt = np.max(array) - motor.stallguard2.set_threshold(sgthresh - 1) + sgt = max(load_samples) while 1: - array = [] + load_samples = [] for i in range(50): - array.append(motor.stallguard2.get_load_value()) - print(array) - if 0 in array: + load_samples.append(motor.stallguard2.get_load_value()) + if 0 in load_samples: motor.drive_settings.max_current = motor.drive_settings.max_current - 1 else: break motor.stallguard2.stop_velocity = motor.get_actual_velocity() - 1 + print("Configured StallGuard2 parameters:") + print(motor.stallguard2) +def main(): + pytrinamic.show_info() -pytrinamic.show_info() - -# This example is using PCAN, if you want to use another connection please change the next line. -connectionManager = ConnectionManager("--interface pcan_tmcl") -with connectionManager.connect() as myInterface: - module = TMCM1231(myInterface) - motor = module.motors[0] - + # This example is using PCAN, if you want to use another connection please change the next line. + connection_manager = ConnectionManager("--interface pcan_tmcl") + with connection_manager.connect() as my_interface: + module = TMCM1231(my_interface) + motor = module.motors[0] - print("Preparing parameters") - # preparing drive settings - motor.drive_settings.max_current = 20 - motor.drive_settings.standby_current = 8 - motor.drive_settings.boost_current = 0 - motor.drive_settings.microstep_resolution = motor.ENUM.microstep_resolution_256_microsteps - print(motor.drive_settings) + print("Preparing parameters") + # preparing drive settings + motor.drive_settings.max_current = 20 + motor.drive_settings.standby_current = 8 + motor.drive_settings.boost_current = 0 + motor.drive_settings.microstep_resolution = motor.ENUM.microstep_resolution_256_microsteps + print(motor.drive_settings) + print(motor.linear_ramp) - print(motor.linear_ramp) + time.sleep(1.0) - time.sleep(1.0) + # clear position counter + motor.actual_position = 0 - # clear position counter - motor.actual_position = 0 + # set up StallGuard2 + print("Configuring StallGuard2 parameters...") + stallguard2_init(motor, init_velocity = 10000) + print("Apply load and try to stall the motor...") + while not (motor.actual_velocity == 0): + pass + print("Motor stopped by StallGuard2!") - # set up StallGuard2 - print("Initial StallGuard2 values:") - print(motor.stallguard2) - stallguard2_init(10000) +if __name__ == "__main__": + main() \ No newline at end of file From 6787c740c39c1f50d0017956ef72a882b87b8d8b Mon Sep 17 00:00:00 2001 From: Sundas Date: Tue, 13 Dec 2022 14:39:57 +0100 Subject: [PATCH 47/51] made cosmetic changes to StallGuard2_demo file --- examples/modules/TMCM1240/TMCL/StallGuard2_demo.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py b/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py index 52f83002..b866efd8 100644 --- a/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py +++ b/examples/modules/TMCM1240/TMCL/StallGuard2_demo.py @@ -1,9 +1,7 @@ """ Sets the StallGuard2 threshold such that the stall guard value (i.e SG value) is zero when the motor comes close to stall and also sets the stop on stall velocity to a value -one less than the actual velocity of the moto -:param motor: TMCL motor object -:param init_velocity: initial velocity to rotate the motor. +one less than the actual velocity of the motor """ import pytrinamic From efabaf58de2aef1be717fb58b3f2ced33e16d689 Mon Sep 17 00:00:00 2001 From: Sundas Date: Tue, 13 Dec 2022 14:49:02 +0100 Subject: [PATCH 48/51] made cosmetic changes to StallGuard2_demo file --- .../modules/TMCM6214/TMCL/StallGuard2_demo.py | 82 +++++++++++-------- 1 file changed, 46 insertions(+), 36 deletions(-) diff --git a/examples/modules/TMCM6214/TMCL/StallGuard2_demo.py b/examples/modules/TMCM6214/TMCL/StallGuard2_demo.py index 3eea8e12..f9e1d3b5 100644 --- a/examples/modules/TMCM6214/TMCL/StallGuard2_demo.py +++ b/examples/modules/TMCM6214/TMCL/StallGuard2_demo.py @@ -1,70 +1,80 @@ +""" +Sets the StallGuard2 threshold such that the stall guard value (i.e SG value) is zero +when the motor comes close to stall and also sets the stop on stall velocity to a value +one less than the actual velocity of the motor +""" import pytrinamic from pytrinamic.connections import ConnectionManager from pytrinamic.modules import TMCM6214 import time -import numpy as np -def stallguard2_init(init_velocity): +def stallguard2_init(motor, init_velocity): + # Resetting SG2 threshold and stop on stall velocity to zero motor.stallguard2.set_threshold(0) motor.stallguard2.stop_velocity = 0 + print("Initial StallGuard2 values:") + print(motor.stallguard2) print("Rotating...") motor.rotate(init_velocity) sgthresh = 0 sgt = 0 - array = [] + load_samples = [] while (sgt == 0) and (sgthresh < 64): - array = [] + load_samples = [] motor.stallguard2.set_threshold(sgthresh) time.sleep(0.2) sgthresh += 1 for i in range(50): - array.append(motor.stallguard2.get_load_value()) - print(sgthresh) - print(array) - if not np.any(array): + load_samples.append(motor.stallguard2.get_load_value()) + if not any(load_samples): sgt = 0 else: - sgt = np.max(array) - motor.stallguard2.set_threshold(sgthresh - 1) + sgt = max(load_samples) while 1: - array = [] + load_samples = [] for i in range(50): - array.append(motor.stallguard2.get_load_value()) - print(array) - if 0 in array or np.max(array) > 450: + load_samples.append(motor.stallguard2.get_load_value()) + print(load_samples) + if 0 in load_samples: motor.drive_settings.max_current = motor.drive_settings.max_current - 8 else: break motor.stallguard2.stop_velocity = motor.get_actual_velocity() - 1 + print("Configured StallGuard2 parameters:") + print(motor.stallguard2) +def main(): + pytrinamic.show_info() -pytrinamic.show_info() - -connectionManager = ConnectionManager() -with connectionManager.connect() as myInterface: - module = TMCM6214(myInterface) - motor = module.motors[0] - + connection_manager = ConnectionManager() + with connection_manager.connect() as my_interface: + module = TMCM6214(my_interface) + motor = module.motors[0] - print("Preparing parameters") - # preparing drive settings - motor.drive_settings.max_current = 128 - motor.drive_settings.standby_current = 8 - motor.drive_settings.boost_current = 0 - motor.drive_settings.microstep_resolution = motor.ENUM.MicrostepResolution256Microsteps - print(motor.drive_settings) + print("Preparing parameters") + # preparing drive settings + motor.drive_settings.max_current = 128 + motor.drive_settings.standby_current = 8 + motor.drive_settings.boost_current = 0 + motor.drive_settings.microstep_resolution = motor.ENUM.MicrostepResolution256Microsteps + print(motor.drive_settings) + print(motor.linear_ramp) - print(motor.linear_ramp) + time.sleep(1.0) - time.sleep(1.0) + # clear position counter + motor.actual_position = 0 - # clear position counter - motor.actual_position = 0 + # set up StallGuard2 + print("Configuring StallGuard2 parameters...") + stallguard2_init(motor, init_velocity = 10000) + print("Apply load and try to stall the motor...") + while not (motor.actual_velocity == 0): + pass + print("Motor stopped by StallGuard2!") - # set up StallGuard2 - print("Initial StallGuard2 values:") - print(motor.stallguard2) - stallguard2_init(10000) +if __name__ == "__main__": + main() From 265eb43e77732969268e04f6b219a7962994d0cd Mon Sep 17 00:00:00 2001 From: bp Date: Fri, 16 Dec 2022 13:45:14 +0100 Subject: [PATCH 49/51] Rework logging, use Python's logging module instead of print-style logging --- examples/evalboards/MAX22216/plunger_move.py | 8 ++- examples/evalboards/MAX22216/ramdebug.py | 5 +- .../TMC4671/TMC4671_eval_BLDC_open_loop.py | 1 - ...MC4671_eval_TMC6100_eval_BLDC_open_loop.py | 1 - ...MC4671_eval_TMC6200_eval_BLDC_open_loop.py | 1 - .../can_tmcl/ixxat_tmcl_interface.py | 23 +++---- .../can_tmcl/kvaser_tmcl_interface.py | 19 +++--- .../can_tmcl/pcan_tmcl_interface.py | 8 +-- .../can_tmcl/slcan_tmcl_interface.py | 14 ++--- .../can_tmcl/socketcan_tmcl_interface.py | 11 +--- pytrinamic/connections/can_tmcl_interface.py | 17 +++--- pytrinamic/connections/connection_manager.py | 60 +++++++------------ .../connections/dummy_tmcl_interface.py | 18 +++--- .../connections/serial_tmcl_interface.py | 16 ++--- pytrinamic/connections/tmcl_interface.py | 43 ++----------- pytrinamic/connections/uart_ic_interface.py | 32 ++++------ pytrinamic/tmcl.py | 6 -- 17 files changed, 103 insertions(+), 180 deletions(-) diff --git a/examples/evalboards/MAX22216/plunger_move.py b/examples/evalboards/MAX22216/plunger_move.py index 68cb2ed2..9feb99a6 100644 --- a/examples/evalboards/MAX22216/plunger_move.py +++ b/examples/evalboards/MAX22216/plunger_move.py @@ -1,12 +1,16 @@ +import logging +import time + import pytrinamic from pytrinamic.connections import ConnectionManager from pytrinamic.ic import MAX22216 from pytrinamic.evalboards import MAX22216_eval -import time + +logging.basicConfig(level=logging.DEBUG) pytrinamic.show_info() -with ConnectionManager(debug=True).connect() as my_interface: +with ConnectionManager().connect() as my_interface: print(my_interface) eval = MAX22216_eval(my_interface) diff --git a/examples/evalboards/MAX22216/ramdebug.py b/examples/evalboards/MAX22216/ramdebug.py index 9511ffe5..27c6b290 100644 --- a/examples/evalboards/MAX22216/ramdebug.py +++ b/examples/evalboards/MAX22216/ramdebug.py @@ -1,11 +1,14 @@ +import logging + import pytrinamic from pytrinamic.connections import ConnectionManager from pytrinamic.ic import MAX22216 from pytrinamic.RAMDebug import Channel, RAMDebug, RAMDebug_Trigger +logging.basicConfig(level=logging.DEBUG) pytrinamic.show_info() -with ConnectionManager(debug=True).connect() as my_interface: +with ConnectionManager().connect() as my_interface: print(my_interface) ch = Channel.field(0, MAX22216.FIELD.ADC_VM_RAW, signed=True, eval_channel=1) diff --git a/examples/evalboards/TMC4671/TMC4671_eval_BLDC_open_loop.py b/examples/evalboards/TMC4671/TMC4671_eval_BLDC_open_loop.py index c1f63e4f..16d8d19b 100644 --- a/examples/evalboards/TMC4671/TMC4671_eval_BLDC_open_loop.py +++ b/examples/evalboards/TMC4671/TMC4671_eval_BLDC_open_loop.py @@ -8,7 +8,6 @@ pytrinamic.show_info() with ConnectionManager().connect() as my_interface: - # my_interface.enable_debug(True) print(my_interface) if isinstance(my_interface, UartIcInterface): diff --git a/examples/evalboards/TMC4671/TMC4671_eval_TMC6100_eval_BLDC_open_loop.py b/examples/evalboards/TMC4671/TMC4671_eval_TMC6100_eval_BLDC_open_loop.py index af29b61f..1f306a26 100644 --- a/examples/evalboards/TMC4671/TMC4671_eval_TMC6100_eval_BLDC_open_loop.py +++ b/examples/evalboards/TMC4671/TMC4671_eval_TMC6100_eval_BLDC_open_loop.py @@ -7,7 +7,6 @@ pytrinamic.show_info() with ConnectionManager().connect() as my_interface: - # my_interface.enable_debug(True) print(my_interface) # Create a TMC4671-EVAL and TMC6100-EVAL which communicates over the Landungsbrücke via TMCL diff --git a/examples/evalboards/TMC4671/TMC4671_eval_TMC6200_eval_BLDC_open_loop.py b/examples/evalboards/TMC4671/TMC4671_eval_TMC6200_eval_BLDC_open_loop.py index c5ec69eb..250ebbca 100644 --- a/examples/evalboards/TMC4671/TMC4671_eval_TMC6200_eval_BLDC_open_loop.py +++ b/examples/evalboards/TMC4671/TMC4671_eval_TMC6200_eval_BLDC_open_loop.py @@ -7,7 +7,6 @@ pytrinamic.show_info() with ConnectionManager().connect() as my_interface: - # myInterface.enable_debug(True) print(my_interface) # Create a TMC4671-EVAL and TMC6200-EVAL which communicates over the Landungsbrücke via TMCL diff --git a/pytrinamic/connections/can_tmcl/ixxat_tmcl_interface.py b/pytrinamic/connections/can_tmcl/ixxat_tmcl_interface.py index 198f39ea..4f198c98 100644 --- a/pytrinamic/connections/can_tmcl/ixxat_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl/ixxat_tmcl_interface.py @@ -26,36 +26,27 @@ class IxxatTmclInterface(CanTmclInterface): # Ixxat USB-to-CAN provides 1, 2 or 4 channels. _CHANNELS = ["0", "1", "2", "3", "4"] - def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): + def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, timeout_s=5): if not isinstance(port, str): raise TypeError if port not in self._CHANNELS: raise ValueError("Invalid port!") - CanTmclInterface.__init__(self, host_id, module_id, debug, timeout_s) - self._channel = port - self._bitrate = datarate + CanTmclInterface.__init__(self, port, datarate, host_id, module_id, timeout_s) + self.logger.info("Connect to bus with bit-rate %s.", self._bitrate) try: - if self._debug: - self._connection = can.Bus(interface="ixxat", channel=self._channel, bitrate=self._bitrate) - else: - self._connection = can.Bus( - interface="ixxat", - channel=self._channel, - bitrate=self._bitrate, - can_filters=[{"can_id": host_id, "can_mask": 0x7F}], - ) + self._connection = can.Bus(interface="ixxat", + channel=self._channel, + bitrate=self._bitrate, + can_filters=[{"can_id": host_id, "can_mask": 0x7F}]) except can.CanError as e: self._connection = None raise ConnectionError( f"Failed to connect to {self.__class__.__name__} on channel {str(self._channel)}" ) from e - if self._debug: - print(f"Opened {self.__class__.__name__} on channel {str(self._channel)}") - @classmethod def list(cls): """ diff --git a/pytrinamic/connections/can_tmcl/kvaser_tmcl_interface.py b/pytrinamic/connections/can_tmcl/kvaser_tmcl_interface.py index be5e37ec..d6015f73 100644 --- a/pytrinamic/connections/can_tmcl/kvaser_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl/kvaser_tmcl_interface.py @@ -9,30 +9,25 @@ class KvaserTmclInterface(CanTmclInterface): """ _CHANNELS = ["0", "1", "2"] - def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): + def __init__(self, port="0", datarate=1000000, host_id=2, module_id=1, timeout_s=5): if not isinstance(port, str): raise TypeError if port not in self._CHANNELS: raise ValueError("Invalid port!") - CanTmclInterface.__init__(self, host_id, module_id, debug, timeout_s) - self._channel = port - self._bitrate = datarate + CanTmclInterface.__init__(self, port, datarate, host_id, module_id, timeout_s) + self.logger.info("Connect to bus with bit-rate %s.", self._bitrate) try: - if self._debug: - self._connection = can.Bus(interface="kvaser", channel=self._channel, bitrate=self._bitrate) - else: - self._connection = can.Bus(interface="kvaser", channel=self._channel, bitrate=self._bitrate, - can_filters=[{"can_id": host_id, "can_mask": 0x7F}]) + self._connection = can.Bus(interface="kvaser", + channel=self._channel, + bitrate=self._bitrate, + can_filters=[{"can_id": host_id, "can_mask": 0x7F}]) except can.CanError as e: self._connection = None raise ConnectionError("Failed to connect to Kvaser CAN bus") from e - if self._debug: - print("Opened bus on channel " + str(self._channel)) - @classmethod def list(cls): """ diff --git a/pytrinamic/connections/can_tmcl/pcan_tmcl_interface.py b/pytrinamic/connections/can_tmcl/pcan_tmcl_interface.py index 9d20e303..6d82078a 100644 --- a/pytrinamic/connections/can_tmcl/pcan_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl/pcan_tmcl_interface.py @@ -32,17 +32,18 @@ class PcanTmclInterface(CanTmclInterface): "PCAN_LANBUS13", "PCAN_LANBUS14", "PCAN_LANBUS15", "PCAN_LANBUS16" ] - def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): + def __init__(self, port, datarate=1000000, host_id=2, module_id=1, timeout_s=5): if not isinstance(port, str): raise TypeError if port not in self._CHANNELS: raise ValueError("Invalid port!") - CanTmclInterface.__init__(self, host_id, module_id, debug, timeout_s) + CanTmclInterface.__init__(self, port, datarate, host_id, module_id, timeout_s) self._channel = port self._bitrate = datarate + self.logger.info("Connect to bus with bit-rate %s.", self._bitrate) try: self._connection = can.Bus(interface="pcan", channel=self._channel, bitrate=self._bitrate) self._connection.set_filters([{"can_id": host_id, "can_mask": 0xFFFFFFFF}]) @@ -50,9 +51,6 @@ def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, self._connection = None raise ConnectionError("Failed to connect to PCAN bus") from e - if self._debug: - print("Opened bus on channel " + self._channel) - @classmethod def list(cls): """ diff --git a/pytrinamic/connections/can_tmcl/slcan_tmcl_interface.py b/pytrinamic/connections/can_tmcl/slcan_tmcl_interface.py index 864a96ca..ff3096eb 100644 --- a/pytrinamic/connections/can_tmcl/slcan_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl/slcan_tmcl_interface.py @@ -12,26 +12,24 @@ class SlcanTmclInterface(CanTmclInterface): Maybe SerialBaudrate has to be changed based on adapter. """ - def __init__(self, com_port, datarate=1000000, host_id=2, module_id=1, debug=True, timeout_s=5, serial_baudrate=115200): + def __init__(self, com_port, datarate=1000000, host_id=2, module_id=1, timeout_s=5, serial_baudrate=115200): if not isinstance(com_port, str): raise TypeError - CanTmclInterface.__init__(self, host_id, module_id, debug, timeout_s) - self._bitrate = datarate - self._port = com_port + CanTmclInterface.__init__(self, com_port, datarate, host_id, module_id, timeout_s) self._serial_baudrate = serial_baudrate + self.logger.info("Connect to bus. (Baudrate=%s)", self._serial_baudrate) try: - self._connection = can.Bus(interface='slcan', channel=self._port, bitrate=self._bitrate, + self._connection = can.Bus(interface='slcan', + channel=self._channel, + bitrate=self._bitrate, ttyBaudrate=self._serial_baudrate) self._connection.set_filters([{"can_id": host_id, "can_mask": 0x7F}]) except CanError as e: self._connection = None raise ConnectionError("Failed to connect to CAN bus") from e - if self._debug: - print("Opened slcan bus on channel " + self._port) - @classmethod def list(cls): """ diff --git a/pytrinamic/connections/can_tmcl/socketcan_tmcl_interface.py b/pytrinamic/connections/can_tmcl/socketcan_tmcl_interface.py index 50d69377..3886177f 100644 --- a/pytrinamic/connections/can_tmcl/socketcan_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl/socketcan_tmcl_interface.py @@ -12,28 +12,23 @@ class SocketcanTmclInterface(CanTmclInterface): """ _CHANNELS = ["can0", "can1", "can2", "can3", "can4", "can5", "can6", "can7"] - def __init__(self, port, datarate=1000000, host_id=2, module_id=1, debug=False, timeout_s=5): + def __init__(self, port, datarate=1000000, host_id=2, module_id=1, timeout_s=5): if not isinstance(port, str): raise TypeError if port not in self._CHANNELS: raise ValueError("Invalid port") - CanTmclInterface.__init__(self, host_id, module_id, debug, timeout_s) - self._channel = port - self._bitrate = datarate + CanTmclInterface.__init__(self, port, datarate, host_id, module_id, timeout_s) + self.logger.info("Connect to bus with bit-rate %s.", self._bitrate) try: self._connection = can.Bus(interface="socketcan", channel=self._channel, bitrate=self._bitrate) self._connection.set_filters([{"can_id": host_id, "can_mask": 0x7F}]) - except CanError as e: self._connection = None raise ConnectionError("Failed to connect to SocketCAN bus") from e - if self._debug: - print("Opened bus on channel " + self._channel) - @classmethod def list(cls): """ diff --git a/pytrinamic/connections/can_tmcl_interface.py b/pytrinamic/connections/can_tmcl_interface.py index a0f9f3e3..d8813505 100644 --- a/pytrinamic/connections/can_tmcl_interface.py +++ b/pytrinamic/connections/can_tmcl_interface.py @@ -1,4 +1,5 @@ +import logging import can from ..connections.tmcl_interface import TmclInterface @@ -6,17 +7,19 @@ class CanTmclInterface(TmclInterface): """Generic CAN interface class for the CAN adapters.""" - def __init__(self, host_id=2, default_module_id=1, debug=False, timeout_s=5): + def __init__(self, channel, datarate, host_id, default_module_id, timeout_s): - TmclInterface.__init__(self, host_id, default_module_id, debug) + TmclInterface.__init__(self, host_id, default_module_id) self._connection = None - self._channel = None - self._bitrate = None + self._channel = channel + self._bitrate = datarate if timeout_s == 0: self._timeout_s = None else: self._timeout_s = timeout_s + self.logger = logging.getLogger(f"{self.__class__.__name__}.{self._channel}") + def __enter__(self): return self @@ -28,8 +31,7 @@ def __exit__(self, exit_type, value, traceback): self.close() def close(self): - if self._debug: - print(f"Closing {self.__class__.__name__} (channel {str(self._channel)})") + self.logger.info("Shutdown.") self._connection.shutdown() @@ -71,8 +73,7 @@ def _recv(self, host_id, module_id): if msg.arbitration_id != host_id: # The filter shouldn't let wrong messages through. # This is just a sanity check - if self._debug: - print(f"Received wrong ID ({self.__class__.__name__}, on channel {str(self._channel)})") + self.logger.warning("Received a CAN Frame with unexpected ID (received: %d; expected: %d)", msg.arbitration_id, host_id) return bytearray([msg.arbitration_id]) + msg.data diff --git a/pytrinamic/connections/connection_manager.py b/pytrinamic/connections/connection_manager.py index 59b079b4..f149a31e 100644 --- a/pytrinamic/connections/connection_manager.py +++ b/pytrinamic/connections/connection_manager.py @@ -1,4 +1,5 @@ import sys +import logging import argparse from ..connections import DummyTmclInterface @@ -11,6 +12,8 @@ from ..connections import SlcanTmclInterface from ..connections import IxxatTmclInterface +logger = logging.getLogger(__name__) + class ConnectionManager: """ @@ -100,24 +103,23 @@ class which allows repeated connect() and disconnect() calls. ("ixxat_tmcl", IxxatTmclInterface, 1000000), ] - def __init__(self, arg_list=None, connection_type="any", debug=False): + def __init__(self, arg_list=None, connection_type="any"): # Attributes - self.__debug = debug self.__connection = None arg_parser = argparse.ArgumentParser(description='ConnectionManager to setup connections dynamically and interactively') ConnectionManager.argparse(arg_parser) if not arg_list: - if self.__debug: - print("Using arguments from the command line") + logger.info("Using arguments from the command line.") arg_list = sys.argv if isinstance(arg_list, str): arg_list = arg_list.split() - if self.__debug: - print("Splitting string:", arg_list) + logger.debug("List of input arguments: %s", arg_list) + + # Parse the command line args = arg_parser.parse_known_args(arg_list)[0] # Argument storage - default parameters are set here @@ -128,11 +130,7 @@ def __init__(self, arg_list=None, connection_type="any", debug=False): self.__host_id = 2 self.__module_id = 1 - # Parse the command line - if self.__debug: - print("Commandline argument list: {0:s}".format(str(arg_list))) - print("Parsed commandline arguments: {0:s}".format(str(args))) - print() + logger.debug("Combined default and parsed arguments: %s", args) # ## Interpret given arguments # Interface @@ -181,18 +179,18 @@ def __init__(self, arg_list=None, connection_type="any", debug=False): except ValueError as exc: raise ValueError("Invalid module id: " + args.module_id[0]) from exc - if self.__debug: - print("Connection parameters:") - print("\tInterface: " + self.__interface.__qualname__) - print("\tPort: " + self.__port) - print("\tBlacklist: " + str(self.__no_port)) - print("\tData rate: " + str(self.__data_rate)) - print("\tTimeout: " + str(self.__timeout_s)) - print("\tHost ID: " + str(self.__host_id)) - print("\tModule ID: " + str(self.__module_id)) - print() - - def connect(self, debug_interface=None): + logger.info("ConnectionManager created with [" + "Interface: %s; " + "Port: %s; " + "Blacklist: %s; " + "Data rate: %s; " + "Timeout: %s;" + "Host ID: %s; " + "Module ID: %s]", + self.__interface.__qualname__, self.__port, self.__no_port, self.__data_rate, self.__timeout_s, + self.__host_id, self.__module_id) + + def connect(self): """ Attempt to connect to a module with the stored connection parameters. @@ -201,19 +199,7 @@ def connect(self, debug_interface=None): If no connections are available or a connection attempt fails, a ConnectionError exception is raised - - Parameters: - debug_interface: - Type: bool, optional, default value: None - Control whether the connection should be created in - debug mode. A boolean value will enable or disable the debug mode, - a None value will set the connections debug mode according to the - ConnectionManagers debug mode. """ - # If no debug selection has been passed, inherit the debug state from the connection manager - if debug_interface is None: - debug_interface = self.__debug - # Get all available ports port_list = self.list_connections() @@ -257,10 +243,10 @@ def connect(self, debug_interface=None): if self.__interface.supports_tmcl(): # Open the connection to a TMCL interface self.__connection = self.__interface(port, self.__data_rate, self.__host_id, self.__module_id, - debug=debug_interface, timeout_s=self.__timeout_s) + timeout_s=self.__timeout_s) else: # Open the connection to a direct IC interface - self.__connection = self.__interface(port, self.__data_rate, debug=debug_interface, timeout_s=self.__timeout_s) + self.__connection = self.__interface(port, self.__data_rate, timeout_s=self.__timeout_s) except ConnectionError as e: raise ConnectionError("Couldn't connect to port " + port + ". Connection failed.") from e diff --git a/pytrinamic/connections/dummy_tmcl_interface.py b/pytrinamic/connections/dummy_tmcl_interface.py index e8736a63..f9dfd88e 100644 --- a/pytrinamic/connections/dummy_tmcl_interface.py +++ b/pytrinamic/connections/dummy_tmcl_interface.py @@ -1,23 +1,22 @@ +import logging + from ..connections.tmcl_interface import TmclInterface class DummyTmclInterface(TmclInterface): - def __init__(self, port, datarate=115200, host_id=2, module_id=1, debug=True, timeout_s=5): + def __init__(self, port, datarate=115200, host_id=2, module_id=1, timeout_s=5): """ Opens a dummy TMCL connection """ if not isinstance(port, str): raise TypeError - TmclInterface.__init__(self, host_id, module_id, debug) + TmclInterface.__init__(self, host_id, module_id) + + self.logger = logging.getLogger("{}.{}".format(self.__class__.__name__, port)) - if self._debug: - print("Opened dummy TMCL interface on port '" + port + "'") - print("\tData rate: " + str(datarate)) - print("\tHost ID: " + str(host_id)) - print("\tModule ID: " + str(module_id)) - print("\tTimeout: " + str(timeout_s)) + self.logger.debug("Opening port (baudrate=%s).", datarate) def __enter__(self): return self @@ -33,8 +32,7 @@ def close(self): """ Closes the dummy TMCL connection """ - if self._debug: - print("Closed dummy TMCL interface") + def _send(self, host_id, module_id, data): """ diff --git a/pytrinamic/connections/serial_tmcl_interface.py b/pytrinamic/connections/serial_tmcl_interface.py index c31e6ae3..ab612cc7 100644 --- a/pytrinamic/connections/serial_tmcl_interface.py +++ b/pytrinamic/connections/serial_tmcl_interface.py @@ -1,3 +1,5 @@ +import logging + from serial import Serial, SerialException import serial.tools.list_ports from ..connections.tmcl_interface import TmclInterface @@ -8,23 +10,23 @@ class SerialTmclInterface(TmclInterface): """ Opens a serial TMCL connection """ - def __init__(self, com_port, datarate=115200, host_id=2, module_id=1, debug=False, timeout_s=5): + def __init__(self, com_port, datarate=115200, host_id=2, module_id=1, timeout_s=5): if not isinstance(com_port, str): raise TypeError - TmclInterface.__init__(self, host_id, module_id, debug) + TmclInterface.__init__(self, host_id, module_id) self._baudrate = datarate if timeout_s == 0: timeout_s = None + self.logger = logging.getLogger("{}.{}".format(self.__class__.__name__, com_port)) + + self.logger.debug("Opening port (baudrate=%s).", datarate) try: self._serial = Serial(com_port, self._baudrate, timeout=timeout_s) except SerialException as e: raise ConnectionError from e - if self._debug: - print("Opened port: " + self._serial.portstr) - def __enter__(self): return self @@ -36,9 +38,7 @@ def __exit__(self, exit_type, value, traceback): self.close() def close(self): - if self._debug: - print("Closing port: " + self._serial.portstr) - + self.logger.info("Closing port.") self._serial.close() def _send(self, host_id, module_id, data): diff --git a/pytrinamic/connections/tmcl_interface.py b/pytrinamic/connections/tmcl_interface.py index dae5c686..5f1b6543 100644 --- a/pytrinamic/connections/tmcl_interface.py +++ b/pytrinamic/connections/tmcl_interface.py @@ -1,3 +1,4 @@ +import logging from abc import ABC from ..tmcl import TMCL, TMCLRequest, TMCLCommand, TMCLReply, TMCLReplyChecksumError, TMCLReplyStatusError from ..helpers import TMC_helpers @@ -19,13 +20,10 @@ class TmclInterface(ABC): _send(self, host_id, module_id, data) _recv(self, host_id, module_id) - A subclass may use the boolean _debug attribute to toggle printing further - debug output. - A subclass may read the _host_id and _module_id parameters. """ - def __init__(self, host_id=2, default_module_id=1, debug=False): + def __init__(self, host_id=2, default_module_id=1): """ Parameters: host_id: @@ -38,40 +36,14 @@ def __init__(self, host_id=2, default_module_id=1, debug=False): tmcl_interface functions. When only communicating with one module a script can omit the moduleID for all TMCL interface calls by declaring this default value once at the start. - debug: - Type: bool, optional, default: False - A switch for enabling debug mode. Can be changed with - enableDebug(). In debug mode all sent and received TMCL packets - get dumped to stdout. The boolean _debug attribute holds the - current state of debug mode - subclasses may read it to print - further debug output. """ + self.logger = logging.getLogger("TmclInterfaceAbstractBaseClassObject") # Will be overwritten in derived classes TMCL.validate_host_id(host_id) TMCL.validate_module_id(default_module_id) - if not isinstance(debug, bool): - raise TypeError - self._host_id = host_id self._module_id = default_module_id - self._debug = debug - - def enable_debug(self, enable): - """ - Set the debug mode, which dumps all TMCL datagrams written and read. - """ - if not isinstance(enable, bool): - raise TypeError("Expected boolean value") - - self._debug = enable - - def print_info(self): - info = "ConnectionInterface {" - info += "'debug_enabled':" + str(self._debug) + ", " - info = info[:-2] - info += "}" - print(info) def _send(self, host_id, module_id, data): """ @@ -106,14 +78,12 @@ def send_request(self, request): Send a TMCL_Request and read back a TMCL_Reply. This function blocks until the reply has been received. """ - if self._debug: - request.dump() + self.logger.debug("Tx: %s", request) self._send(self._host_id, request.moduleAddress, request.to_buffer()) reply = TMCLReply.from_buffer(self._recv(self._host_id, request.moduleAddress)) - if self._debug: - reply.dump() + self.logger.debug("Rx: %s", reply) self._reply_check(reply) @@ -151,8 +121,7 @@ def send_boot(self, module_id=None): request = TMCLRequest(module_id, TMCLCommand.BOOT, 0x81, 0x92, 0xA3B4C5D6) - if self._debug: - request.dump() + self.logger.debug("Tx: %s", request) # Send the request self._send(self._host_id, module_id, request.to_buffer()) diff --git a/pytrinamic/connections/uart_ic_interface.py b/pytrinamic/connections/uart_ic_interface.py index e2f7d799..48f8ac3d 100644 --- a/pytrinamic/connections/uart_ic_interface.py +++ b/pytrinamic/connections/uart_ic_interface.py @@ -1,3 +1,4 @@ +import logging import struct from serial import Serial import serial.tools.list_ports @@ -14,8 +15,8 @@ def __init__(self, address, value): def to_buffer(self): return struct.pack(REGISTER_PACKAGE_STRUCTURE, self.address, self.value) - def dump(self): - print("RegisterRequest: " + str(self.address) + "," + str(self.value)) + def __str__(self): + return "RegisterRequest: [Addr:{:02x}, Value:{}]".format(self.address, self.value) class RegisterReply: @@ -23,8 +24,8 @@ def __init__(self, reply_struct): self.address = reply_struct[0] self.value = reply_struct[1] - def dump(self): - print("RegisterReply: " + str(self.address) + "," + str(self.value)) + def __str__(self): + return "RegisterReply: [Addr:{:02x}, Value:{}]".format(self.address, self.value) def value(self): return self.value @@ -32,13 +33,15 @@ def value(self): class UartIcInterface: - def __init__(self, com_port, datarate=9600, debug=False, timeout_s=5): - self._debug = debug + def __init__(self, com_port, datarate=9600, timeout_s=5): self.baudrate = datarate if timeout_s == 0: timeout_s = None + + self.logger = logging.getLogger("{}.{}".format(self.__class__.__name__, com_port)) + + self.logger.debug("Opening port (baudrate=%s).", datarate) self.serial = Serial(com_port, self.baudrate, timeout=timeout_s) - print("Open port: " + self.serial.portstr) def __enter__(self): return self @@ -51,18 +54,13 @@ def __exit__(self, exit_type, value, traceback): self.close() def close(self): - if self._debug: - print("Close port: " + self.serial.portstr) - + self.logger.info("Closing port.") self.serial.close() def send_datagram(self, data, recv_size): self.serial.write(data) return self.serial.read(recv_size) - def enable_debug(self, enable): - self._debug = enable - @staticmethod def supports_tmcl(): return False @@ -71,15 +69,11 @@ def send(self, address, value): # prepare TMCL request request = RegisterRequest(address, value) - if self._debug: - request.dump() - # send request, wait, and handle reply + self.logger.debug("Tx %s", request) self.serial.write(request.to_buffer()) reply = RegisterReply(struct.unpack(REGISTER_PACKAGE_STRUCTURE, self.serial.read(REGISTER_PACKAGE_LENGTH))) - - if self._debug: - reply.dump() + self.logger.debug("Rx %s", reply) return reply diff --git a/pytrinamic/tmcl.py b/pytrinamic/tmcl.py index 0b79f439..edc780b4 100644 --- a/pytrinamic/tmcl.py +++ b/pytrinamic/tmcl.py @@ -151,9 +151,6 @@ def __str__(self): self.checksum ) - def dump(self): - print(self) - class TMCLReply: def __init__(self, reply_address, module_address, status, command, value, checksum=None, special=False): @@ -194,9 +191,6 @@ def __str__(self): self.checksum ) - def dump(self): - print(self) - def value(self): return self.value From 0f2b6196af84e9ae6a351f896d963c7378720805 Mon Sep 17 00:00:00 2001 From: bp Date: Fri, 16 Dec 2022 13:49:52 +0100 Subject: [PATCH 50/51] Docs: fix the link to the code examples on GitHub --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 287cbf9c..668304b5 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ pip install pytrinamic ## Getting Started -Please have a look at the [code examples on GitHub](https://github.com/trinamic/PyTrinamic/tree/feature_feature_hierarchy_v2/examples). +Please have a look at the [code examples on GitHub](https://github.com/trinamic/PyTrinamic/tree/master/examples). ## Migration Guide From 5d433f0311b98f83076b2f0c055f5480b504dd64 Mon Sep 17 00:00:00 2001 From: bp Date: Fri, 16 Dec 2022 13:55:41 +0100 Subject: [PATCH 51/51] Bump version for upcoming release --- pytrinamic/version.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pytrinamic/version.py b/pytrinamic/version.py index 1f4ca897..e807d946 100644 --- a/pytrinamic/version.py +++ b/pytrinamic/version.py @@ -1,2 +1,2 @@ -__version__ = "0.2.2" +__version__ = "0.2.3"