diff --git a/Controller/config/components/devices/COARM/COARM-M1 b/Controller/config/components/devices/COARM/COARM-M1 index 68f8c88c8f791977689dd766269aea1312801252..0324c5a3ab3c1fbc100686e734fb07e154dfcd05 100644 --- a/Controller/config/components/devices/COARM/COARM-M1 +++ b/Controller/config/components/devices/COARM/COARM-M1 @@ -13,10 +13,18 @@ kinematics_flip = 0 kinematics_offset = 0.0 # Unit rad, for judge motion complete status -motion_complete_limit = 0.2 +motion_complete_limit = 0.02 motion_complete_ratio = 1.5 control_error_time = 3 +# Motor control factors, rad, rad/s, Nm +motor_angle_limit = 12.5 +motor_velocity_limit = 8 +motor_torque_limit = 28 +motor_kp = 49 +motor_kd = 1.3 +motor_tau = 0 + mounting_direction = 1 mechanical_stop = 0 mechanical_offset = 0 diff --git a/Controller/config/components/devices/COARM/COARM-M2 b/Controller/config/components/devices/COARM/COARM-M2 index 57ae6748b29c9d376c01990e302bd4bf58bf182e..a3db31ba83f0834fa1a51726ab1a2a82474537c4 100644 --- a/Controller/config/components/devices/COARM/COARM-M2 +++ b/Controller/config/components/devices/COARM/COARM-M2 @@ -13,10 +13,18 @@ kinematics_flip = 0 kinematics_offset = 0.0 # Unit rad, for judge motion complete status -motion_complete_limit = 0.2 +motion_complete_limit = 0.02 motion_complete_ratio = 1.5 control_error_time = 3 +# Motor control factors, rad, rad/s, Nm +motor_angle_limit = 12.5 +motor_velocity_limit = 30 +motor_torque_limit = 10 +motor_kp = 20 +motor_kd = 0.45 +motor_tau = 0 + mounting_direction = 1 mechanical_stop = 0 mechanical_offset = 0 diff --git a/Controller/driver/motors/dmbots/dmbotscan.py b/Controller/driver/motors/dmbots/dmbotscan.py index 9619f01775f73d9f5897c30538ba7fd75943626b..579c9f785f80119d7eba15afd0d92dda4af48eb3 100644 --- a/Controller/driver/motors/dmbots/dmbotscan.py +++ b/Controller/driver/motors/dmbots/dmbotscan.py @@ -110,7 +110,8 @@ class DMBotsCAN(threading.Thread): # self.setDaemon(True) self.daemon = True # Arbitration Id - self.__send_cob_id = 0x100 + aid + # self.__send_cob_id = 0x100 + aid + self.__send_cob_id = 0x000 + aid self.__recv_cob_id = 0x010 + aid # CAN messgae listener self.__listener = None diff --git a/Controller/driver/motors/dmbots/motor.py b/Controller/driver/motors/dmbots/motor.py index 23c76f066bf1352f859a663bdd18ed9e16aa0492..e8ac6eb357ef1341fe7fd0003f501f735ecc727c 100644 --- a/Controller/driver/motors/dmbots/motor.py +++ b/Controller/driver/motors/dmbots/motor.py @@ -4,6 +4,7 @@ import copy import os import time import math +import numpy as np # Location from Controller.driver.trajectory.trajectory import Trajectory @@ -25,7 +26,7 @@ import matplotlib.pyplot as plt # /********************************************* Motor DMBots ***********************************************/ # /********************************************************************************************************************/ class Sync(threading.Thread): - DUTATION = 0.01 + DURATION = 0.01 def __init__(self, parent, bus, config): threading.Thread.__init__(self) @@ -58,6 +59,13 @@ class Sync(threading.Thread): self.__pvts = list() # Config variable self.__motion_complete_limit = self.__config.get('motion_complete_limit') + self.__motor_angle_limit = self.__config.get('motor_angle_limit') + self.__motor_velocity_limit = self.__config.get('motor_velocity_limit') + self.__motor_torque_limit = self.__config.get('motor_torque_limit') + + self.__motor_kp = 0.0 + self.__motor_kd = 0.0 + self.__motor_tau = 0.0 @property def bus(self): @@ -108,18 +116,18 @@ class Sync(threading.Thread): :return: """ - # def LIMIT_MIN_MAX(x, min, max): - # if x <= min: - # x = min - # elif x > max: - # x = max + 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 float_to_uint(x, x_min, x_max, bits): + limit_min_max(x, x_min, x_max) + return np.uint16((x - x_min) * (((1 << bits) - 1) / (x_max - x_min))) def decode_response_frame(data): status = (data[0] & 0x10) >> 4 @@ -130,6 +138,25 @@ class Sync(threading.Thread): motor_coil_temperature = data[7] return status, position, velocity, torque, mos_temperature, motor_coil_temperature + def encode_send_frame(q,dq): + kp_uint = float_to_uint(self.__motor_kp, 0, 500, 12) + kd_uint = float_to_uint(self.__motor_kd, 0, 5, 12) + + q_uint = float_to_uint(q, -self.__motor_angle_limit, self.__motor_angle_limit, 16) + dq_uint = float_to_uint(dq, -self.__motor_velocity_limit, self.__motor_velocity_limit, 12) + tau_uint = float_to_uint(self.__motor_tau, -self.__motor_torque_limit, self.__motor_torque_limit, 12) + + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + data_buf[0] = (q_uint >> 8) & 0xff + data_buf[1] = q_uint & 0xff + data_buf[2] = dq_uint >> 4 + data_buf[3] = ((dq_uint & 0xf) << 4) | ((kp_uint >> 8) & 0xf) + data_buf[4] = kp_uint & 0xff + data_buf[5] = kd_uint >> 4 + data_buf[6] = ((kd_uint & 0xf) << 4) | ((tau_uint >> 8) & 0xf) + data_buf[7] = tau_uint & 0xff + return data_buf + self.__is_active = True self.__is_pause = False power_off = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD] @@ -150,22 +177,27 @@ class Sync(threading.Thread): target_velocity = pvt[1][0] target_position = pvt[0][0] else: - target_velocity = self.__target_velocity + # target_velocity = self.__target_velocity + target_velocity = 0 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) + # target_position = write_multi_float(target_position, 4, True) + # target_velocity = write_multi_float(target_velocity, 4, True) + # target_position.extend(target_velocity) + + data_buf = encode_send_frame(target_position,target_velocity) # # 发送数据查看 # hex_data = [hex(num) for num in target_position] # print(hex_data) - response = self.__bus.transmit(target_position) + # response = self.__bus.transmit(target_position) + response = self.__bus.transmit(data_buf) + self.__status, self.__position, \ self.__velocity, self.__torque, \ self.__mos_temperature, self.__motor_coil_temperature = decode_response_frame(response) @@ -173,7 +205,7 @@ class Sync(threading.Thread): self.__motion_completed = True else: self.__motion_completed = False - time.sleep(self.DUTATION) + time.sleep(self.DURATION) self.__is_sync = False def initialize(self): @@ -188,6 +220,11 @@ class Sync(threading.Thread): while not self.__is_sync: time.sleep(0.01) + if self.__config.get('version') == 'DMBots': + self.__motor_kp = self.__config.get('motor_kp') + self.__motor_kd = self.__config.get('motor_kd') + self.__motor_tau = self.__config.get('motor_tau') + def set_position(self, position): """ Set target position to motor @@ -200,7 +237,7 @@ class Sync(threading.Thread): jmove = Trajectory("joints") jmove.load({"joint_ids": ids}) points = [[self.__position], [position]] - jmove.set(points, self.__target_velocity, self.__target_acceleration, self.DUTATION, 1, False) + jmove.set(points, self.__target_velocity, self.__target_acceleration, self.DURATION, 1, False) pvts = list() last_flag = list() while True: @@ -211,7 +248,7 @@ class Sync(threading.Thread): for i in range(len(ids)): p.append(pvt[i].position) v.append(pvt[i].velocity) - pvts.append([p, v, self.DUTATION]) + pvts.append([p, v, self.DURATION]) last_flag.append(False) if not info.success: last_flag[-1] = True diff --git a/Controller/driver/motors/motor.py b/Controller/driver/motors/motor.py index e29bbe02171e77efbc7945ffedbf3e3c3b89febf..25281fc6a9aa5af599dc14d85933d3baa53ab1d9 100644 --- a/Controller/driver/motors/motor.py +++ b/Controller/driver/motors/motor.py @@ -327,6 +327,7 @@ class MotorConfig: self.__robot_config_path = os.path.join(self.__resources, 'devices', self.__robot_name) self.__motor_config_path = os.path.join(self.__resources, 'motors') self.__parameters = {} + self.__control_factors = {} self.__setup_file = None @property @@ -497,7 +498,33 @@ class MotorConfig: motion_complete_preview = abs(float(self.read_parameter(self.DETAIL, 'motion_complete_preview', file))) self.__parameters['motion_complete_preview'] = motion_complete_preview # Add by sjhuang 20240705 end - self.__parameters['control_error_time'] = float(self.read_parameter(self.DETAIL, 'control_error_time', file)) + self.__parameters['control_error_time'] = float( + self.read_parameter(self.DETAIL, 'control_error_time', file)) + # Get control factors + motor_angle_limit = 0.0 + if 'motor_angle_limit' in file[self.DETAIL]: + motor_angle_limit = float(self.read_parameter(self.DETAIL, 'motor_angle_limit', file)) + self.__control_factors['motor_angle_limit'] = motor_angle_limit + motor_velocity_limit = 0.0 + if 'motor_velocity_limit' in file[self.DETAIL]: + motor_velocity_limit = float(self.read_parameter(self.DETAIL, 'motor_velocity_limit', file)) + self.__control_factors['motor_velocity_limit'] = motor_velocity_limit + motor_torque_limit = 0.0 + if 'motor_torque_limit' in file[self.DETAIL]: + motor_torque_limit = float(self.read_parameter(self.DETAIL, 'motor_torque_limit', file)) + self.__control_factors['motor_torque_limit'] = motor_torque_limit + motor_kp = 0.0 + if 'motor_kp' in file[self.DETAIL]: + motor_kp = float(self.read_parameter(self.DETAIL, 'motor_kp', file)) + self.__control_factors['motor_kp'] = motor_kp + motor_kd = 0.0 + if 'motor_kd' in file[self.DETAIL]: + motor_kd = float(self.read_parameter(self.DETAIL, 'motor_kd', file)) + self.__control_factors['motor_kd'] = motor_kd + motor_tau = 0.0 + if 'motor_tau' in file[self.DETAIL]: + motor_tau = float(self.read_parameter(self.DETAIL, 'motor_tau', file)) + self.__control_factors['motor_tau'] = motor_tau # Get mechanical params mounting_direction = int(self.read_parameter(self.DETAIL, 'mounting_direction', file)) if abs(mounting_direction) != 1: @@ -587,6 +614,9 @@ class MotorConfig: self.__parameters['collision_brake_release'] = collision_brake_release # Add by sjhuang 20240604 end + # Merge two dictionaries + self.__parameters.update(self.__control_factors) + def __read_motor_parameters(self, file): """ Load motor parameters @@ -800,6 +830,7 @@ class MotorBase(threading.Thread): if self.mode == OFFLINE: self.enabled = True return + # Virtual Function def finalize(self): diff --git a/README.md b/README.md index 30fcf42af3e95f7b54351e2e02de74cf7c5d8d80..0e84c2d8caba24c829243aa90e162c49ec4d9595 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,10 @@ Coarm机械臂底层运动控制库 ## 更新说明 +### Update v1.0.3 +1. 更改为MIT控制模式 +2. 优化配置文件 + ### Update v1.0.2 1. 修复轨迹缺点问题 2. 增加dmbotscan线程锁