diff --git a/.gitignore b/.gitignore index cdb4e1d53168246fd32c692b3a31a714b2ac49a5..f982fc8e1de095f8e7bf5666384d9bf8033bb9a6 100644 --- a/.gitignore +++ b/.gitignore @@ -96,5 +96,8 @@ venv.bak/ */build/ */dist/ +/.vscode/ +/OutputPicture/ + /test*.py /Test diff --git a/Controller/config/components/devices/COARM/COARM b/Controller/config/components/devices/COARM/COARM new file mode 100644 index 0000000000000000000000000000000000000000..e6d4b81e1fd2e31f6720ed6e9ca830ee35957fbc --- /dev/null +++ b/Controller/config/components/devices/COARM/COARM @@ -0,0 +1,7 @@ +[MODEL] +name = COARM + +[MOTORS] +count = 1 +motor1 = COARM-M1 +motor2 = COARM-M2 diff --git a/Controller/config/components/devices/COARM/COARM-M1 b/Controller/config/components/devices/COARM/COARM-M1 new file mode 100644 index 0000000000000000000000000000000000000000..68f8c88c8f791977689dd766269aea1312801252 --- /dev/null +++ b/Controller/config/components/devices/COARM/COARM-M1 @@ -0,0 +1,38 @@ +[DETAIL] +name = COARM-M1 +id = 1 +motor = DMBOTS-4340 +type = revolute + +lower_angle_limit = -360 +upper_angle_limit = 360.0 +velocity_limit = 230 +acceleration_limit = 1200 + +kinematics_flip = 0 +kinematics_offset = 0.0 + +# Unit rad, for judge motion complete status +motion_complete_limit = 0.2 +motion_complete_ratio = 1.5 +control_error_time = 3 + +mounting_direction = 1 +mechanical_stop = 0 +mechanical_offset = 0 + +host_callback_mode = 0 + +# Useless +calibration_mode = 1 +calibration_direction = -1 +calibration_bit = 3 +calibration_factor = 0.0 +calibration_isrest = 0 +calibration_rest = 0.0 +calibration_vel1 = 5.0 +calibration_acc1 = 20.0 +calibration_vel2 = 10.0 +calibration_acc2 = 30.0 +calibration_vel_rest = 80.0 +calibration_acc_rest = 120.0 diff --git a/Controller/config/components/devices/COARM/COARM-M2 b/Controller/config/components/devices/COARM/COARM-M2 new file mode 100644 index 0000000000000000000000000000000000000000..57ae6748b29c9d376c01990e302bd4bf58bf182e --- /dev/null +++ b/Controller/config/components/devices/COARM/COARM-M2 @@ -0,0 +1,38 @@ +[DETAIL] +name = COARM-M2 +id = 2 +motor = DMBOTS-4310 +type = revolute + +lower_angle_limit = -360 +upper_angle_limit = 360.0 +velocity_limit = 700 +acceleration_limit = 1200 + +kinematics_flip = 0 +kinematics_offset = 0.0 + +# Unit rad, for judge motion complete status +motion_complete_limit = 0.2 +motion_complete_ratio = 1.5 +control_error_time = 3 + +mounting_direction = 1 +mechanical_stop = 0 +mechanical_offset = 0 + +host_callback_mode = 0 + +# Useless +calibration_mode = 1 +calibration_direction = -1 +calibration_bit = 3 +calibration_factor = 0.0 +calibration_isrest = 0 +calibration_rest = 0.0 +calibration_vel1 = 5.0 +calibration_acc1 = 20.0 +calibration_vel2 = 10.0 +calibration_acc2 = 30.0 +calibration_vel_rest = 80.0 +calibration_acc_rest = 120.0 diff --git a/Controller/config/components/devices/COARM/default b/Controller/config/components/devices/COARM/default new file mode 100644 index 0000000000000000000000000000000000000000..b93cd0af418e720157a18b9413f7d9848f0622d0 --- /dev/null +++ b/Controller/config/components/devices/COARM/default @@ -0,0 +1,9 @@ +[DEFAULT_VALUE] +velocity_in_deg = 20.0 +acceleration_in_deg = 150.0 +velocity_in_mm = 150.0 +acceleration_in_mm = 200.0 +interval_in_record = 0.05 +interval_in_calculation = 0.025 +disconnect_time=1 + diff --git a/Controller/config/components/devices/DMBOTS/default b/Controller/config/components/devices/DMBOTS/default index b6eb4a8b4d77552c7a81dd845fdd2859ea3292d2..2515f45fb8baf34ab9614c8faf8ef1f69f032805 100644 --- a/Controller/config/components/devices/DMBOTS/default +++ b/Controller/config/components/devices/DMBOTS/default @@ -5,4 +5,3 @@ velocity_in_mm = 150.0 acceleration_in_mm = 200.0 interval_in_record = 0.05 interval_in_calculation = 0.025 - diff --git a/Controller/config/components/motors/DMBOTS-4310/config b/Controller/config/components/motors/DMBOTS-4310/config new file mode 100644 index 0000000000000000000000000000000000000000..230bf2c9e9ab1d0493a10ea41ff1a9ff929077d4 --- /dev/null +++ b/Controller/config/components/motors/DMBOTS-4310/config @@ -0,0 +1,19 @@ +[DETAIL] +name = DMBOTS-4310 +version = DMBots + +loop_control = CLOSE + +# Useless +transmission_ratio = 1 +resolution = 1 + +encoder_type = ABSOLUTE +encoder_resolution = 1 +encoder_direction = 1 + +current_limit = 1.2 + +# Keep default +i_max_ps = 1 +ts_s = 1 diff --git a/Controller/config/components/motors/DMBOTS-4340/config b/Controller/config/components/motors/DMBOTS-4340/config new file mode 100644 index 0000000000000000000000000000000000000000..e60c2e54c9bee080e23979b6fcc6579a0cde07fe --- /dev/null +++ b/Controller/config/components/motors/DMBOTS-4340/config @@ -0,0 +1,19 @@ +[DETAIL] +name = DMBOTS-4340 +version = DMBots + +loop_control = CLOSE + +# Useless +transmission_ratio = 1 +resolution = 1 + +encoder_type = ABSOLUTE +encoder_resolution = 1 +encoder_direction = 1 + +current_limit = 1.2 + +# Keep default +i_max_ps = 1 +ts_s = 1 diff --git a/Controller/driver/commander.py b/Controller/driver/commander.py index dccc546fa7d9347f12cd43a0d767dd93c296a6dc..67c92f3f7bd710151dcd55c449d650c3e4e517ec 100644 --- a/Controller/driver/commander.py +++ b/Controller/driver/commander.py @@ -8,7 +8,7 @@ import time import math import datetime -from pymodbus.client.sync import ModbusSerialClient as ModbusClient +from pymodbus.client import ModbusSerialClient as ModbusClient from queue import Queue from threading import get_ident @@ -105,7 +105,8 @@ def format_parameters(func): class PVTMotion(threading.Thread): def __init__(self, commander, joint_ids): threading.Thread.__init__(self) - self.setDaemon(True) + # self.setDaemon(True) + self.daemon = True # Command handle self.__commander = commander # PVT point cache @@ -440,9 +441,12 @@ class Commander: stopbits = self.__parameters[MODBUS]['stopbits'] timeout = self.__parameters[MODBUS]['timeout'] # Create modbus client + # connection = ModbusClient(port=port, baudrate=baudrate, + # databits=bytesize, parity=parity, stopbits=stopbits, + # errorcheck="crc", method="RTU", timeout=timeout) connection = ModbusClient(port=port, baudrate=baudrate, - databits=bytesize, parity=parity, stopbits=stopbits, - errorcheck="crc", method="RTU", timeout=timeout) + bytesize=bytesize, parity=parity, stopbits=stopbits, + framer="rtu", timeout=timeout) connected = connection.connect() if not connected: error_message = "Open channel failed with %s" % self.__parameters[MODBUS] diff --git a/Controller/driver/context.py b/Controller/driver/context.py index 79369e42858fe3b62d494a1e496f93de171136f2..b5678ee431d12f657061de925bcab21a6e10505a 100644 --- a/Controller/driver/context.py +++ b/Controller/driver/context.py @@ -37,7 +37,8 @@ class Context: self.context = context self.thread_name = "ContextDiscovery" threading.Thread.__init__(self, name=self.thread_name) - self.setDaemon(True) + # self.setDaemon(True) + self.daemon = True def run(self): logger.info("Creating discovery context") @@ -120,7 +121,8 @@ class Context: self.context = context self.thread_name = "ContextJudgement" threading.Thread.__init__(self, name=self.thread_name) - self.setDaemon(True) + # self.setDaemon(True) + self.daemon = True def run(self): logger.info("Creating judgement context") @@ -155,7 +157,8 @@ class Context: self.context = context self.thread_name = "ContextUpdate" threading.Thread.__init__(self, name=self.thread_name) - self.setDaemon(True) + # self.setDaemon(True) + self.daemon = True def run(self): logger.info("Creating updating context") diff --git a/Controller/driver/driver.py b/Controller/driver/driver.py index ccad0b379fcd8687df205c15f4bacc9b98266937..6a3fe1b6a30bbb981d278551efe759f6e6dde2fb 100644 --- a/Controller/driver/driver.py +++ b/Controller/driver/driver.py @@ -759,7 +759,7 @@ class Driver: logger.warning("Device(%s) do not have Kinematics model" % name) # Load dynamics - if self.__kinematics_name: + if self.__dynamics_name: self.__dynamics_name = self.__config.get_dynamics() if self.__dynamics_name: try: @@ -1387,7 +1387,8 @@ class Driver: move_thread = threading.Thread(target=self.move_process, args=(joint_ids, target_joints, None, None, None, jmove, 'movej', advance, )) - move_thread.setDaemon(True) + # move_thread.setDaemon(True) + move_thread.daemon = True move_thread.start() time.sleep(0.1) self.__commander.wait_for_pvt_motion_start() @@ -1477,7 +1478,8 @@ class Driver: move_thread = threading.Thread(target=self.move_process, args=(joint_ids, target_joints, point, euler, current_joints, lmove, 'movel', advance, )) - move_thread.setDaemon(True) + # move_thread.setDaemon(True) + move_thread.daemon = True move_thread.start() time.sleep(0.1) # Wait for pvt motion starting diff --git a/Controller/driver/motors/dmbots/dmbotscan.py b/Controller/driver/motors/dmbots/dmbotscan.py index 571bb26e70d9d0dde6dae59cd2fbcb9de035b6f5..8cd8db3dd0121670ee65dfcea415e9f367bb6068 100644 --- a/Controller/driver/motors/dmbots/dmbotscan.py +++ b/Controller/driver/motors/dmbots/dmbotscan.py @@ -84,6 +84,7 @@ class Listener(can.Listener): :return: """ + # print(msg, msg.arbitration_id) if self.__cob_id == msg.arbitration_id: self.__dpool[msg.arbitration_id] = msg @@ -106,10 +107,11 @@ class DMBotsCAN(threading.Thread): def __init__(self, aid, bus: can.interface.BusABC, notifier: can.Notifier): threading.Thread.__init__(self) - self.setDaemon(True) + # self.setDaemon(True) + self.daemon = True # Arbitration Id self.__send_cob_id = 0x100 + aid - self.__recv_cob_id = 0x000 + self.__recv_cob_id = 0x010 + aid # CAN messgae listener self.__listener = None # CAN bus handle @@ -150,6 +152,7 @@ class DMBotsCAN(threading.Thread): try: # Send message and check return self.__bus.send(msg.message, timeout=self.TIMEOUT) + # print(msg.message) # Wait for receiving message timeout = time.monotonic() + self.TIMEOUT while True: @@ -159,6 +162,7 @@ class DMBotsCAN(threading.Thread): if self.__listener.dpool[self.__recv_cob_id].timestamp >= msg.message.timestamp: self.__recv_pool[msg.uuid] = self.__listener.dpool[self.__recv_cob_id] self.__listener.dpool.pop(self.__recv_cob_id) + # print(self.__recv_pool[msg.uuid]) break time.sleep(0.005) except Exception as err: diff --git a/Controller/driver/motors/dmbots/motor.py b/Controller/driver/motors/dmbots/motor.py index 50220b44384d010bece22613ccffb8dcd2bde9cd..420a52290a243f13aff37811ca47588fd86188ce 100644 --- a/Controller/driver/motors/dmbots/motor.py +++ b/Controller/driver/motors/dmbots/motor.py @@ -18,6 +18,7 @@ from Controller.utils.error import * from Controller import logger +import matplotlib.pyplot as plt # /********************************************************************************************************************/ # /********************************************* Motor DMBots ***********************************************/ @@ -27,7 +28,8 @@ class Sync(threading.Thread): def __init__(self, parent, bus, config): threading.Thread.__init__(self) - self.setDaemon(True) + # self.setDaemon(True) + self.daemon = True # Motor handle self.__motor = parent # CAN bus handle @@ -105,10 +107,17 @@ class Sync(threading.Thread): :return: """ + # def LIMIT_MIN_MAX(x, min, max): + # if x <= min: + # x = min + # elif x > max: + # x = max + def uint_to_float(x, x_min, x_max, bits): return x * (x_max - x_min) / ((1 << bits) - 1) + x_min # def float_to_uint(x, x_min, x_max, bits): + # LIMIT_MIN_MAX(x, x_min, x_max) # return (x - x_min) * (((1 << bits) - 1) / (x_max - x_min)) def decode_response_frame(data): @@ -142,9 +151,19 @@ class Sync(threading.Thread): else: target_velocity = self.__target_velocity target_position = self.__target_position + + # # 发送前数据查看 + # print("target_position", target_position) + # print("target_velocity", target_velocity) + target_position = write_multi_float(target_position, 4, True) target_velocity = write_multi_float(target_velocity, 4, True) target_position.extend(target_velocity) + + # # 发送数据查看 + # hex_data = [hex(num) for num in target_position] + # print(hex_data) + response = self.__bus.transmit(target_position) self.__status, self.__position, \ self.__velocity, self.__torque, \ @@ -201,6 +220,47 @@ class Sync(threading.Thread): self.__target_position = position self.__motion_completed = False + def plot_trajectory(): + # 多个电机一起运行,图容易画在一起重叠,出事故 + # for pvt_group in self.__pvts: + # print(pvt_group) + + # 指定保存文件的文件夹 + output_folder = 'OutputPicture' # 替换为你的文件夹路径 + os.makedirs(output_folder, exist_ok=True) # 创建文件夹（如果不存在） + + # 提取位置、速度和持续时间 + positions = [pvt[0][0] for pvt in self.__pvts] + velocities = [pvt[1][0] for pvt in self.__pvts] + durations = [pvt[2] for pvt in self.__pvts] + + # 创建时间轴 + time_steps = [sum(durations[:i]) for i in range(len(durations) + 1)] + + # 绘制位置图 + plt.figure(figsize=(10, 5)) + plt.plot(time_steps[:-1], positions, color='blue', label='Position') + plt.title(f'Position Joint {ids}') + plt.xlabel('Time (s)') + plt.ylabel('Position (rad)') + plt.legend() + plt.grid() + plt.savefig(os.path.join(output_folder, f'position_plot_Joint_{ids}.png')) # 保存为文件 + plt.close() # 关闭当前图 + + # 绘制速度图 + plt.figure(figsize=(10, 5)) + plt.plot(time_steps[:-1], velocities, color='red', label='Velocity') + plt.title(f'Velocity Joint {ids}') + plt.xlabel('Time (s)') + plt.ylabel('Velocity (rad/s)') + plt.legend() + plt.grid() + plt.savefig(os.path.join(output_folder, f'velocity_plot_Joint_{ids}.png')) # 保存为文件 + plt.close() # 关闭当前图 + + # plot_trajectory() + def set_velocity(self, velocity): """ Set target velocity to motor @@ -586,15 +646,15 @@ class Motor(MotorBase): unit_acceleration = abs(self.deg_to_enc(acceleration)) unit_acceleration = self.enc_to_aiu(unit_acceleration) self.power_on() - # Set velocity + # Set Acceleration self.sync.set_acceleration(unit_acceleration) # Set velocity self.sync.set_velocity(unit_velocity) # Set target position self.sync.set_position(unit_position) - # Records motion variables - self.motion_time = motion_time - self.motion_position = position + # Records motion variables + self.motion_time = motion_time + self.motion_position = position except BaseError as err: raise err except Exception as err: @@ -604,29 +664,30 @@ class Motor(MotorBase): def run_position_trigger(self): try: - if self.mode == ONLINE: - # Wait for MOTION COMPLETE reset - logger.info(self.message_filter("Wait for MOTION COMPLETE reset, range: %s" % self.motion_range)) - if self.motion_range > self.config.get('motion_complete_limit'): - timeout = time.monotonic() + 0.5 - while True: - # MOTION COMPLETE reset - if not self.sync.motion_completed: - logger.info(self.message_filter("MOTION COMPLETE status reset")) - break - # Timed out - if time.monotonic() > timeout: - current_position = self.read("read_position") - offset = abs(current_position - self.motion_position) - error_message = self.message_filter("motion reset timed out, " - "target: %s, current: %s" - % (self.motion_position, current_position)) - logger.warning(error_message) - if offset > self.config.get('motion_complete_limit'): - raise ExecuteError(CONTROLLER_ERROR_DRIVER_EXECUTE, error_message) - else: - break - time.sleep(0.01) + # if self.mode == ONLINE: + # # Wait for MOTION COMPLETE reset + # logger.info(self.message_filter("Wait for MOTION COMPLETE reset, range: %s" % self.motion_range)) + # if self.motion_range > self.config.get('motion_complete_limit'): + # timeout = time.monotonic() + 0.5 + # while True: + # # MOTION COMPLETE reset + # if not self.sync.motion_completed: + # logger.info(self.message_filter("MOTION COMPLETE status reset")) + # break + # # Timed out + # if time.monotonic() > timeout: + # current_position = self.read("read_position") + # offset = abs(current_position - self.motion_position) + # error_message = self.message_filter("motion reset timed out, " + # "target: %s, current: %s" + # % (self.motion_position, current_position)) + # logger.warning(error_message) + # if offset > self.config.get('motion_complete_limit'): + # raise ExecuteError(CONTROLLER_ERROR_DRIVER_EXECUTE, error_message) + # else: + # break + # time.sleep(0.01) + time.sleep(0.5) # Records motion variables self.motion_start_time = time.monotonic() except BaseError as err: @@ -680,19 +741,19 @@ class Motor(MotorBase): time.sleep(0.05) event_status = self.read("read_event_status") if event_status: - if not bit_checking: - # 检查到位情况，强制作为运动结束的条件保障 - # time.sleep(0.1) - position = self.read("read_position") - motion_complete_limit = self.config.get('motion_complete_limit') - error = abs(position - self.motion_position) - if error > motion_complete_limit: - self.motion_event = MOTION_LOST_TIMING - error_message = "Failed in wait for motors when Joint %s motion losing timing, " \ - "current: %s, target: %s" \ - % (str(self.id), position, self.motion_position) - logger.error(error_message) - raise ExecuteError(CONTROLLER_ERROR_DRIVER_MOTION_LOST_TIMING, error_message) + # if not bit_checking: + # # 检查到位情况，强制作为运动结束的条件保障 + # # time.sleep(0.1) + # position = self.read("read_position") + # motion_complete_limit = self.config.get('motion_complete_limit') + # error = abs(position - self.motion_position) + # if error > motion_complete_limit: + # self.motion_event = MOTION_LOST_TIMING + # error_message = "Failed in wait for motors when Joint %s motion losing timing, " \ + # "current: %s, target: %s" \ + # % (str(self.id), position, self.motion_position) + # logger.error(error_message) + # raise ExecuteError(CONTROLLER_ERROR_DRIVER_MOTION_LOST_TIMING, error_message) logger.info(self.message_filter("motion completed")) return True diff --git a/Controller/driver/motors/motor.py b/Controller/driver/motors/motor.py index 31337394284d21123178e518313931464bf3a795..e29bbe02171e77efbc7945ffedbf3e3c3b89febf 100644 --- a/Controller/driver/motors/motor.py +++ b/Controller/driver/motors/motor.py @@ -24,7 +24,8 @@ class Simulator(threading.Thread): def __init__(self, motor): threading.Thread.__init__(self) - self.setDaemon(True) + # self.setDaemon(True) + self.daemon = True self.motor = motor # Simulation params @@ -675,7 +676,8 @@ class MotorBase(threading.Thread): def __init__(self, aid, model, connection, mode, resources, config=None): self.thread_name = 'Motor' + str(aid) threading.Thread.__init__(self, name=self.thread_name) - self.setDaemon(True) + # self.setDaemon(True) + self.daemon = True # 身份识别 self.id = str(aid) self.model = model