def setUpClass(cls): channel = guess_channel(bustype_hint=bustype) can_bus: can.Bus = can.Bus(bustype=bustype, channel=channel) iface: IFace = CAN(can_bus) cls.tm = Tinymovr(node_id=1, iface=iface) cls.tm.reset() time.sleep(0.2)
def spawn_shell(): ''' Spawns the Tinymovr Studio IPython shell. ''' version: str = pkg_resources.require("tinymovr")[0].version arguments: Dict[str, str] = docopt(__doc__, version=shell_name + ' ' + str(version)) logger = configure_logging() num_parser = pynumparser.NumberSequence(limits=(0, 16)) node_ids = num_parser(arguments['--ids']) bustype: str = arguments['--bustype'] channel: str = arguments['--chan'] bitrate: int = int(arguments['--bitrate']) if channel == 'auto': channel = guess_channel(bustype_hint=bustype) can_bus: can.Bus = can.Bus(bustype=bustype, channel=channel, bitrate=bitrate) iface: IFace = CAN(can_bus) tms: Dict = {} for node_id in node_ids: try: tm: UserWrapper = UserWrapper(node_id=node_id, iface=iface) tm_name: str = base_name + str(node_id) logger.info("Connected to " + tm_name) tms[tm_name] = tm except TimeoutError: logger.info("Node " + str(node_id) + " timed out") except IOError: logger.error("Node " + str(node_id) + " received abnormal message (possibly wrong ID?)") if len(tms) == 0: logger.error("No Tinymovr instances detected. Exiting shell...") else: tms_discovered: str = ", ".join(list(tms.keys())) user_ns: Dict = {} user_ns.update(tms) user_ns["tms"] = list(tms.values()) user_ns["plot"] = plot user_ns["ureg"] = get_registry() print(shell_name + ' ' + str(version)) print("Discovered instances: " + tms_discovered) print("Access Tinymovr instances as tmx, where x \ is the index starting from 1") print("e.g. the first Tinymovr instance will be tm1.") print("Instances are also available by index in the tms list.") c = Config() c.InteractiveShellApp.gui = 'tk' c.TerminalIPythonApp.display_banner = False IPython.start_ipython(argv=[], config=c, user_ns=user_ns) logger.debug("Exiting shell...")
def main(): global tm1, tm2 channel = guess_channel(bustype_hint='slcan') can_bus: can.Bus = can.Bus(bustype='slcan', channel=channel, bitrate=1000000) iface: IFace = CAN(can_bus) tm1 = Tinymovr(node_id=1, iface=iface) tm2 = Tinymovr(node_id=2, iface=iface) assert (tm1.motor_config.flags == 1) assert (tm2.motor_config.flags == 1) tm1.set_limits(200000, 15) tm2.set_limits(200000, 15) sleep(0.1) tm1.current_control() tm2.current_control() sleep(0.1) offset_1 = tm1.encoder_estimates.position offset_2 = tm2.encoder_estimates.position while True: est_1 = tm1.encoder_estimates est_2 = tm2.encoder_estimates p_1 = est_1.position - offset_1 p_2 = est_2.position - offset_2 v_1 = est_1.velocity v_2 = est_2.velocity Iq_1 = (3e-3 * (p_2 - p_1) * (A / tick) + 5e-5 * (v_2 - v_1) * (A * s / tick)) Iq_2 = (3e-3 * (p_1 - p_2) * (A / tick) + 5e-5 * (v_1 - v_2) * (A * s / tick)) tm1.set_cur_setpoint(Iq_1) tm2.set_cur_setpoint(Iq_2) sleep(0.0001)
def main(): global tm1, tm2 channel = guess_channel(bustype_hint='slcan') can_bus: can.Bus = can.Bus(bustype='slcan', channel=channel, bitrate=1000000) iface: IFace = CAN(can_bus) tm1 = Tinymovr(node_id=1, iface=iface) tm2 = Tinymovr(node_id=2, iface=iface) assert (tm1.motor_config.flags == 1) assert (tm2.motor_config.flags == 1) tm1.set_limits(300000, 25) tm2.set_limits(300000, 25) tm1.set_gains(280, 1.1e-4) tm2.set_gains(280, 1.1e-4) sleep(0.1) tm1.position_control() tm2.position_control() sleep(0.1) offset_1 = tm1.encoder_estimates.position offset_2 = tm2.encoder_estimates.position while True: est_1 = tm1.encoder_estimates est_2 = tm2.encoder_estimates mean_pos = ((est_1.position - offset_1) + (est_2.position - offset_2)) / 2.0 mean_vel = (est_1.velocity + est_2.velocity) / 2.0 tm1.set_pos_setpoint(mean_pos + offset_1, mean_vel, 0) tm2.set_pos_setpoint(mean_pos + offset_2, mean_vel, 0) sleep(0.0005)
def get_tm() -> Tinymovr: can_bus: can.Bus = can.Bus(bustype=bustype, channel=channel) iface: IFace = CAN(can_bus) tm = Tinymovr(node_id=1, iface=iface) tm.iface.bus.legacy_errors = True return tm
def get_tm() -> Tinymovr: can_bus: can.Bus = can.Bus(bustype=bustype, channel=channel) iface: IFace = CAN(can_bus) return Tinymovr(node_id=1, iface=iface)
font = pygame.font.SysFont(None, 48) dt = 0.01 step = 1 * deg period = 2 * s current_threshold = 8.0 * A sign = 0 ratio = 9 goon = True position = 0 * deg modepygame = 0 # channel = guess_channel(bustype_hint='slcan') channel = '/dev/ttyS7' can_bus = can.Bus(bustype='slcan', channel=channel, bitrate=1000000) iface = CAN(can_bus) tm = Tinymovr(node_id=1, iface=iface) tm.set_limits(velocity=2000 * turn / min, current=8.0 * A) tm.set_gains(position=100.0, velocity=0.0001) tm.set_integrator_gains(velocity=0.001) print(tm.motor_info) print(tm.device_info) print("ZERO") position = tm.encoder_estimates.position while goon: for event in pygame.event.get(): if event.type == pygame.QUIT: