DM motor matlab control library function达妙电机matlab库函数

classdef MotorControl < handle properties serial_ motors_map send_data_frame Limit_param data_save end methods function obj = MotorControl(COM, baudrate) obj.motors_map = containers.Map('KeyType','uint32', 'ValueType','any'); obj.data_save = []; %限制参数 PMAX VMAX TMAX 行为所代表的不同电机 obj.Limit_param=[12.5,30,10; %DM4310 12.5,50,10; %DM4310_48V 12.5,8,28; %DM4340 12.5,10,28; %DM4340_48V 12.5,45,20; %DM6006 12.5,45,40; %DM8006 12.5,45,54; %DM8009 12.5,25,200;%DM10010L 12.5,20,200;%DM10010 ]; obj.serial_ = serialport(COM,baudrate,"Timeout",0.5); obj.send_data_frame = uint8([0x55, 0xAA, 0x1e, 0x03, 0x01, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0, 0, 0, 0, 0x00, 0x08, 0x00, 0x00, 0, 0, 0, 0, 0, 0, 0, 0, 0x00]); flush(obj.serial_); end function delete(obj) delete(obj.serial_) end function controlMIT(obj, DM_Motor, kp, kd, q, dq, tau) % MIT Control Mode Function if ~isKey(obj.motors_map, DM_Motor.SlaveID) disp('Motor ID not found'); return; end kp_uint = obj.float_to_uint(kp, 0, 500, 12); kd_uint = obj.float_to_uint(kd, 0, 5, 12); MotorType = DM_Motor.MotorType; q_uint = obj.float_to_uint(q, -obj.Limit_param(MotorType,1), obj.Limit_param(MotorType,1), 16); dq_uint = obj.float_to_uint(dq, -obj.Limit_param(MotorType,2), obj.Limit_param(MotorType,2), 12); tau_uint = obj.float_to_uint(tau, -obj.Limit_param(MotorType,3), obj.Limit_param(MotorType,3), 12); data_buf = uint8(zeros(1,8)); data_buf(1) = bitand(bitshift(q_uint, -8),255); data_buf(2) = bitand(q_uint,255); data_buf(3) = bitshift(dq_uint, -4); data_buf(4) = bitor(bitshift(bitand(dq_uint, 15), 4),bitand(bitshift(kp_uint, -8), 15)); data_buf(5) = bitand(kp_uint,255); data_buf(6) = bitshift(kd_uint, -4); data_buf(7) = bitor(bitshift(bitand(kd_uint, 15), 4), bitand(bitshift(tau_uint, -8), 15)); data_buf(8) = bitand(tau_uint,255); obj.send_data(DM_Motor.SlaveID,data_buf); obj.recv(); end function control_Pos_Vel(obj, Motor, P_desired, V_desired) % Control the motor in position and velocity control mode if ~isKey(obj.motors_map, Motor.SlaveID) disp('Motor ID not found'); return; end temp_id=hex2dec('100')+Motor.SlaveID;% vel pos control need 0x100+canid data_buf = uint8(zeros(1, 8)); P_desired_uint8s = obj.float_to_uint8s(P_desired); V_desired_uint8s = obj.float_to_uint8s(V_desired); data_buf(1:4) = P_desired_uint8s; data_buf(5:8) = V_desired_uint8s; obj.send_data(temp_id,data_buf); obj.recv(); end function control_Vel(obj, Motor, Vel_desired) % control the motor in velocity control mode 电机速度控制模式 % :param Motor: Motor object 电机对象 % :param Vel_desired: desired velocity 期望速度 if ~isKey(obj.motors_map, Motor.SlaveID) disp('Motor ID not found'); return; end temp_id=hex2dec('200')+Motor.SlaveID; %vel control need 0x200+canid data_buf = uint8(zeros(1, 8)); Vel_desired_uint8s = obj.float_to_uint8s(Vel_desired); data_buf(1:4) = Vel_desired_uint8s; obj.send_data(temp_id,data_buf); obj.recv(); % receive the data from serial port end function control_pos_force(obj, Motor, Pos_des, Vel_des, i_des) % control the motor in EMIT control mode 电机力位混合模式 % :param Pos_des: desired position rad 期望位置 单位为rad % :param Vel_des: desired velocity rad/s 期望速度 为放大100倍 % :param i_des: desired current rang 0-10000 期望电流标幺值放大10000倍 % 电流标幺值：实际电流值除以最大电流值，最大电流见上电打印 if ~isKey(obj.motors_map, Motor.SlaveID) disp('Motor ID not found'); return; end temp_id=hex2dec('300')+Motor.SlaveID; %pos force model is 0x300 data_buf = uint8(zeros(1, 8)); Pos_desired_uint8s = obj.float_to_uint8s(Pos_des); data_buf(1:4) = Pos_desired_uint8s; Vel_uint = uint16(Vel_des); ides_uint = uint16(i_des); data_buf(5) = bitand(Vel_uint, 255); data_buf(6) = bitshift(Vel_uint, -8); data_buf(7) = bitand(ides_uint, 255); data_buf(8) = bitshift(ides_uint, -8); obj.send_data_frame(22:29) = data_buf; obj.send_data(temp_id,data_buf); obj.recv(); % receive the data from serial port end function enable(obj, Motor) % enable motor 使能电机 % 最好在上电后几秒后再使能电机 % :param Motor: Motor object 电机对象 obj.control_cmd(Motor, uint8(0xFC)); pause(0.1); obj.recv(); end function enable_old(obj,Motor,ControlMode) % enable motor for old motor version 使能电机针对低版本固件的 % 最好在上电后几秒后再使能电机 % :param Motor: Motor object 电机对象 data_buf = uint8([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, cmd]); motor_id=bitshift(uint32(ControlMode)-1,2)+Motor.SlaveID; obj.send_data(motor_id,data_buf); pause(0.1); obj.recv(); end function disable(obj, Motor) % disable motor 失能电机 % :param Motor: Motor object 电机对象 obj.control_cmd(Motor, uint8(0xFD)); pause(0.1); obj.recv(); end function set_zero_position(obj, Motor) % set the zero position of the motor 设置电机0位 % :param Motor: Motor object 电机对象 obj.control_cmd(Motor, uint8(0xFE)); pause(0.1); obj.recv(); end function recv(obj) read_num=obj.serial_.NumBytesAvailable; if read_num ~=0 data_recv = [obj.data_save,read(obj.serial_, read_num ,"uint8")]; packets = obj.extract_packets(data_recv); for i = 1:length(packets) packet = packets{i}; data = packet(8:15); CANID = bitor(bitshift(uint32(packet(7)), 24), bitor(bitshift(uint32(packet(6)), 16), bitor(bitshift(uint32(packet(5)), 8), uint32(packet(4))))); CMD = packet(2); obj.process_packet(data, CANID, CMD); end end end function addMotor(obj, Motor) % add motor to the motor control object 添加电机到电机控制对象 % :param Motor: Motor object 电机对象 obj.motors_map(Motor.SlaveID) = Motor; if Motor.MasterID ~= 0 obj.motors_map(Motor.MasterID) = Motor; end end function read_RID_param(obj, Motor, RID) data_buf = uint8([uint8(Motor.SlaveID), 0, 51, uint8(RID), 0, 0, 0, 0]); obj.send_data(0x7FF,data_buf); pause(0.01);