balance/u2can/__pycache__/DM_CAN.cpython-310.pyc

o
<00>[9ijX<00>@s<>dd
lm
Z

ddlZddlmZ
ddlmZ
ddlmZ
Gdd<07>d<07>Z	Gdd	<09>d	<09>Z
d
d<0B>Zded
edefdd<10>Z
dejdedefdd<14>Zdd<16>Zdd<18>Zdd<1A>Zdd<1C>Zdd<1E>Zdd <20>Zd!d"<22>ZGd#d$<24>d$e<05>ZGd%d&<26>d&e<05>ZGd'd(<28>d(e<05>ZdS))<29>)
<01>sleepN)
<01>IntEnum)
<01>unpack)
<01>packc@sJeZ
dZd
d<02>Zdededefdd<07>Zdd	<09>Zd
d<0B>Zdd
<0A>Zdd<0F>Z	dS)<11>MotorcCs\td
<01>
|_
td
<01>
|_td
<01>
|_td
<01>
|_td
<01>
|_||_||_|
|_d|_	t
j|_i|_
dS)u<>
        define Motor object 定义电机对象
        :param MotorType: Motor type 电机类型
        :param SlaveID: CANID 电机ID
        :param MasterID: MasterID 主机ID 建议不要设为0
        r
FN)<0E>floatZPdZVd<56>state_q<5F>state_dq<64>	state_tau<61>SlaveID<49>MasterID<49>	MotorTypeZisEnable<6C>Control_Type<70>MITZNowControlMode<64>temp_param_dict)<04>selfr
rr<00>r<00>'/home/allenyuan/balance/u2can/DM_CAN.py<70>__init__	s















zMotor.__init__<5F>
q<>dq<64>taucCs|
|_||_
||_dS<00>
N)rr	r
)rrrrrrr<00>	recv_datas



zMotor.recv_datac


C<00>|jS)
uz
        get the position of the motor 获取电机位置
        :return: the position of the motor 电机位置
        )
r<00>
rrrr<00>getPosition!<00>zMotor.getPositionc


Cr)
uz
        get the velocity of the motor 获取电机速度
        :return: the velocity of the motor 电机速度
        )
r	rrrr<00>getVelocity(rzMotor.getVelocityc


Cr)
uv
        get the torque of the motor 获取电机力矩
        :return: the torque of the motor 电机力矩
        )
r
rrrr<00>	getTorque/rzMotor.getTorquecCs|
|jvr
|j|
Sd
S)u<>
        get the parameter of the motor 获取电机内部的参数，需要提前读取
        :param RID: DM_variable 电机参数
        :return: the parameter of the motor 电机参数
        N)
r)r<00>RIDrrr<00>getParam6s


zMotor.getParamN)
<EFBFBD>__name__<5F>
__module__<EFBFBD>__qualname__rrrrrrr!rrrrrs
rc
@s<>
eZ
dZe<03>gd
<01>
ej<05>Zgd<02>
gd<03>
gd<04>
gd<05>
gd<06>
gd<07>
gd<08>
gd	<09>
gd
<EFBFBD>
gd<0B>
gd<0C>
gd<0C>
gZd
d<0E>Zde	de	de	de	de	f
dd<15>Z
de	de	de	de	de	de	fdd<18>Zde	de	fdd<1C>Zdd<1E>Z
de	fd d!<21>Zd"d#<23>Zd$d%<25>Zd&d'<27>Zd(d)<29>Zd*d+<2B>Zd,d-<2D>Zd.d/<2F>Zd0d1<64>Zd2d3<64>Zd4ejfd5d6<64>Zd7d8<64>Zd9d:<3A>Zd;d<<3C>Zd=d><3E>Zd?d@<40>ZdAdB<64>ZdCdD<64>ZdEdF<64>Z dGdH<64>Z!dIdJ<64>Z"dKS)L<>MotorControl)<1E>U<00><><00><00><00>
r
r
r
<00>
r
r
r
r
r
r
r
r
r
<00>r
r
r
r
r
r
r
r
r
r
r
)<03>)@r(r+)r-<00>2r+)r-r,<00>)r-r+r/)r-<00>-<00>)r-r0<00>()r-r0<00>6)r-<00><00><>)r-r1r5)r-i
r*)r-r0r+cCs<|
|_t
<01>|_t<03>|_|jjrtd
<01>

|
<01><07>
|j<00><08>
dS)u~
        define MotorControl object 定义电机控制对象
        :param serial_device: serial object 串口对象
        zSerial port is openN)	<09>serial_<6C>dict<63>
motors_map<EFBFBD>bytes<65>	data_save<76>is_open<65>print<6E>close<73>open)r<00>
serial_devicerrrrMs





zMotorControl.__init__<5F>kp<6B>kdrrrcCsD
|
j|j
v
rtd
<01>

dSt|ddd<05>}t|ddd<05>}|
j}	|j|	d}
|j|	d}|j|	d}t||
|
d	<09>}
t|||d<05>}t|||d<05>}t<06>gd
<EFBFBD>
tj<08>}|
d?d@|d<|
d@|d<|d
?|d<|d@d
>|d?d@B|d<|d@|d
<|d
?|d<|d@d
>|d?d@B|d<|d@|d<|<00>	|
j|<10>
|<00>
<EFBFBD>
dS)uA

        MIT Control Mode Function 达妙电机MIT控制模式函数
        :param DM_Motor: Motor object 电机对象
        :param kp: kp
        :param kd:  kd
        :param q:  position  期望位置
        :param dq:  velocity  期望速度
        :param tau: torque  期望力矩
        :return: None
        z%controlMIT ERROR : Motor ID not foundNr
i<>
<00><00>r*<00><00><00>r
r
r
r
r
r
r
r
r,<00><><00><00>r)<00><00>)rr8r<<00>
float_to_uintr
<00>Limit_Param<61>np<6E>array<61>uint8<74>_MotorControl__send_data<74>recv)r<00>DM_Motorr@rArrrZkp_uintZkd_uintr
<00>Q_MAX<41>DQ_MAX<41>TAU_MAX<41>q_uint<6E>dq_uint<6E>tau_uint<6E>data_bufrrr<00>
controlMITZs.





















zMotorControl.controlMIT<49>delaycCs |<00>|
|||||<06>
t
|<07>

d
S)uv

        MIT Control Mode Function with delay 达妙电机MIT控制模式函数带延迟
        :param DM_Motor: Motor object 电机对象
        :param kp: kp
        :param kd: kd
        :param q:  position  期望位置
        :param dq:  velocity  期望速度
        :param tau: torque  期望力矩
        :param delay: delay time 延迟时间 单位秒
        N)r[r)rrSr@rArrrr\rrr<00>
control_delay}s
zMotorControl.control_delay<61>	P_desired<65>	V_desiredcCst|
j|j
v
rtd
<01>

dSd|
j}t<03>gd<04>
tj<05>}t|<02>
}t|<03>
}||dd<06><||dd<07><|<00>||<05>
|<00><08>
dS)u

        control the motor in position and velocity control mode 电机位置速度控制模式
        :param Motor: Motor object 电机对象
        :param P_desired: desired position 期望位置
        :param V_desired: desired velocity 期望速度
        :return: None
        z*Control Pos_Vel Error : Motor ID not foundN<64>
rFr
rHr,<00>	rr8r<rNrOrP<00>float_to_uint8srQrR)rrr^r_<00>motoridrZZP_desired_uint8sZV_desired_uint8srrr<00>control_Pos_Vel<65>s









zMotorControl.control_Pos_VelcCs`|
j|j
v
rtd
<01>

dSd|
j}t<03>gd<04>
tj<05>}t|<02>
}||dd<06><|<00>||<04>
|<00><08>
dS)u<>
        control the motor in velocity control mode 电机速度控制模式
        :param Motor: Motor object 电机对象
        :param Vel_desired: desired velocity 期望速度
        z&control_VEL ERROR : Motor ID not foundNirFr
rHra)rrZVel_desiredrcrZZVel_desired_uint8srrr<00>control_Vel<65>s








zMotorControl.control_Vel<65>Pos_desc
Cs<>|
j|j
v
rtd
<01>

dSd|
j}t<03>gd<04>
tj<05>}t|<02>
}||dd<06><t<03>|<03>
}t<03>|<04>
}	|d@|d<|d?|d	<|	d@|d
<|	d?|d<|<00>||<06>
|<00>	<09>
dS)u<>

        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倍
        电流标幺值：实际电流值除以最大电流值，最大电流见上电打印
        z*control_pos_vel ERROR : Motor ID not foundNirFr
rHrGr,rCrJrK)
rr8r<rNrOrPrb<00>uint16rQrR)
rrrfZVel_desZi_desrcrZZPos_desired_uint8sZVel_uintZ	ides_uintrrr<00>control_pos_force<63>s
















zMotorControl.control_pos_forcecC<00>&|<00>|
t
<01>d
<01>
<01>
td<02>

|<00><04>
dS)u<>
        enable motor 使能电机
        最好在上电后几秒后再使能电机
        :param Motor: Motor object 电机对象
        <20><>皙<><E79A99><EFBFBD><EFBFBD><EFBFBD>?N<><05>_MotorControl__control_cmdrNrPrrR<00>rrrrr<00>enable<6C>s

zMotorControl.enablecCsHt<00>
gd
<01>
tj<02>}t|<02>
dd>|
j}|<00>||<03>
td<04>

|<00><07>
dS)u

        enable motor old firmware 使能电机旧版本固件，这个是为了旧版本电机固件的兼容性
        可恶的旧版本固件使能需要加上偏移量
        最好在上电后几秒后再使能电机
        :param Motor: Motor object 电机对象
        )rGrGrGrGrGrGrGrjr*rDrkN)rNrOrP<00>intrrQrrR)rr<00>ControlModerZZ	enable_idrrr<00>
enable_old<EFBFBD>s




zMotorControl.enable_oldcCs|<00>|
t
<01>d
<01>
<01>
td<02>

dS)u\
        disable motor 失能电机
        :param Motor: Motor object 电机对象
        <20><>g{<14>G<EFBFBD>z<EFBFBD>?N)rmrNrPrrnrrr<00>disable<6C>s
zMotorControl.disablecCri)uu
        set the zero position of the motor 设置电机0位
        :param Motor: Motor object 电机对象
        <20><>rkNrlrnrrr<00>set_zero_position<6F>s

zMotorControl.set_zero_positionc
Cs|d
<01>|j
|j<02><03>g<02>
}
|<00>|
<01>
}|D])}|dd<03>}|dd>|dd>B|dd	>B|d
B}|d}|<00>|||<06>
qdS)N<>rKrIrJ<00>rCrErHr,r)r*)<06>joinr:r6<00>read_all<6C>_MotorControl__extract_packets<74>_MotorControl__process_packet<65>rZ	data_recvZpacketsZpacket<65>data<74>CANID<49>CMDrrrrR<00>s



,


<04>zMotorControl.recvc
Csp|j<00>
<01>}
|<00>|
<01>
}|D])}|d
d<02>}|dd>|dd>B|dd>B|d	B}|d
}|<00>|||<06>
qdS)NrKrIrJrxrCrErHr,r)r*)r6rzr{<00>'_MotorControl__process_set_param_packetr}rrr<00>recv_set_param_data<74>s





,


<04>z MotorControl.recv_set_param_datacCs<>
|d
kr<>|dkr||jvr}t
<01>t
<01>|
d<00>
d>|
dB<00>
}t
<01>t
<01>|
d<00>
d>|
dd?B<00>
}t
<01>|
dd@d>|
d	B<00>
}|j|j}|j|d}|j|d}	|j|d}
t|||d
<EFBFBD>}t||	|	d<0B>}t||
|
d<0B>}
|j|<00>|||
<0A>
dSdS|
dd@}||jvr<>t
<01>t
<01>|
d<00>
d>|
dB<00>
}t
<01>t
<01>|
d<00>
d>|
dd?B<00>
}t
<01>|
dd@d>|
d	B<00>
}|j|j}|j|d}|j|d}	|j|d}
t|||d
<EFBFBD>}t||	|	d<0B>}t||
|
d<0B>}
|j|<00>|||
<0A>
dSdSdS)N<>r
r*r,rDr)rHrIrCrErB)r8rNrgr
rM<00>
uint_to_floatr)rr~rr<>rWrXrYZMotorType_recvrTrUrVZrecv_qZrecv_dqZrecv_taurrrrZ__process_packet
s<



 
$









<04>

 
$









<04>zMotorControl.__process_packetcCs<>|d
krj|
ddks|
ddkrl|}|
dd>|
dB}|dkr"|}||jv
r0||jv
r.dS|}|
d}t
|<06>
rQt|
d	|
d
|
d|
d<00>}||j|j|<dSt|
d	|
d
|
d|
d<00>}||j|j|<dSdSdS)
Nr<4E>rD<00>3r&r*r,r
r)rHrCrJrK)r8<00>is_in_ranges<65>uint8s_to_uint32r<00>uint8s_to_float)rr~rr<>ZmasteridZslaveIdr <00>numrrrZ__process_set_param_packet"
s  










<08>z'MotorControl.__process_set_param_packetcCs&|
|j|
j
<|
jd
kr|
|j|
j<dS)u<>
        add motor to the motor control object 添加电机到电机控制对象
        :param Motor: Motor object 电机对象
        r
T)r8rrrnrrr<00>addMotor<
s



zMotorControl.addMotor<6F>cmdc
Cs0t<00>
d
d
d
d
d
d
d
|gtj<02>}|<00>|
j|<03>
dS)NrG)rNrOrPrQr)rrr<>rZrrrZ
__control_cmdF
s

zMotorControl.__control_cmdcCsF|
d
@|jd<|
d?d
@|jd<||jdd<06><|j
<01>t|jj<04>
<01>

dS)u}
        send data to the motor 发送数据到电机
        :param motor_id:
        :param data:
        :return:
        rG<00>
r,<00><00><00>N)<05>send_data_framer6<00>writer9<00>
T)rZmotor_idr~rrrZ__send_dataJ
s


zMotorControl.__send_datac
CsX|
jd
@}|
jd?d
@}t
<01>t
<01>|<03>
t
<01>|<04>
dt
<01>|<02>
ddddgt
j<03>}|<00>d|<05>
dS)NrGr,r<>r
<00><>)rrNrOrPrQ)rrr <00>can_id_l<5F>can_id_hrZrrrZ__read_RID_paramV
s


0

zMotorControl.__read_RID_paramc
Cs<>|
jd
@}|
jd?d
@}t
<01>t
<01>|<04>
t
<01>|<05>
dt
<01>|<02>
ddddgt
j<03>}t|<02>
s1t|<03>
|dd<02><n
tt|<03>
<01>
|dd<02><|<00>d|<06>
dS)NrGr,r&r
rHr<>)	rrNrOrPr<>rb<00>data_to_uint8srprQ)rrr r~r<>r<>rZrrrZ__write_motor_param\
s


0


z MotorControl.__write_motor_paramcCs<>d
}d}d
}|<00>|
|t
<01>|<02>
<01>
t|<03>
D]*}t|<04>

|<00><05>
|
j|jvr>||j|
jjvr>|j|
jj||kr;
dS
dSqdS)u<>
        switch the control mode of the motor 切换电机控制模式
        :param Motor: Motor object 电机对象
        :param ControlMode: Control_Type 电机控制模式 example:MIT:Control_Type.MIT MIT模式
        r+皙<><E79A99><EFBFBD><EFBFBD><EFBFBD>?TF)	<09> _MotorControl__write_motor_paramrNrP<00>rangerr<>rr8r)rrrq<00>max_retries<65>retry_intervalr <00>
_rrr<00>switchControlModeh
s









<02>
zMotorControl.switchControlModec
Csd|
jd
@}|
jd?d
@}t
<01>t
<01>|<02>
t
<01>|<03>
ddddddgt
j<03>}|<00>|
<01>

|<00>d|<04>
td<06>

dS)u<>
        save the all parameter  to flash 保存所有电机参数
        :param Motor: Motor object 电机对象
        :return:
        rGr,r'r
r<>g<><67><EFBFBD><EFBFBD>MbP?N)rrNrOrPrtrQr<00>rrr<>r<>rZrrr<00>save_motor_param}
s

*




zMotorControl.save_motor_paramcCs.||j|
d
<||j|
d<||j|
d<dS)u<>
        change the PMAX VMAX TMAX of the motor 改变电机的PMAX VMAX TMAX
        :param Motor_Type:
        :param PMAX: 电机的PMAX
        :param VMAX: 电机的VMAX
        :param TMAX: 电机的TMAX
        :return:
        r
r*rDN)
rM)rZ
Motor_Type<EFBFBD>PMAX<41>VMAX<41>TMAXrrr<00>change_limit_param<61>
s	

zMotorControl.change_limit_paramc
CsZ|
jd
@}|
jd?d
@}t
<01>t
<01>|<02>
t
<01>|<03>
ddddddgt
j<03>}|<00>d|<04>
|<00><05>
dS)u9
        get the motor status 获得电机状态
        rGr,<00><>r
r<>N)rrNrOrPrQrRr<>rrr<00>refresh_motor_status<75>
s


*


z!MotorControl.refresh_motor_statuscCs<>d
}d}|<00>|
||<03>
t
|<04>
D].}|<00><02>
|
j|jvr9||j|
jjvr9t|j|
jj||<00>
dkr6
dS
dSt|<05>

qdS)u


        change the RID of the motor 改变电机的参数
        :param Motor: Motor object 电机对象
        :param RID: DM_variable 电机参数
        :param data: 电机参数的值
        :return: True or False ,True means success, False means fail
        r1r<>rkTF)r<>r<>r<>rr8r<00>absr)rrr r~r<>r<>r<>rrr<00>change_motor_param<61>
s








zMotorControl.change_motor_paramcCspd
}d}|<00>|
|<02>
t
|<03>
D]'}t|<04>

|<00><03>
|
j|jvr5||j|
jjvr2|j|
jj|
S
dSqdS)u<>
        read only the RID of the motor 读取电机的内部信息例如 版本号等
        :param Motor: Motor object 电机对象
        :param RID: DM_variable 电机参数
        :return: 电机参数的值
        r1r<>N)<07>_MotorControl__read_RID_paramr<6D>rr<>rr8r)rrr r<>r<>r<>rrr<00>read_motor_param<61>
s







<02>zMotorControl.read_motor_paramc	Cs<>g}d
}d}d}d}d}|t|
<01>
|k
rD|
||kr8|
||d|kr8|
|||<00>}|<02>
|<08>

||7}|}n|d7}|t|
<01>
|k
s|
|d<00>|_|S)Nr'r&rEr
r*)<03>len<65>appendr:)	rr~<00>frames<65>header<65>tailZframe_length<74>
iZ
remainder_pos<6F>framerrrZ__extract_packets<74>
s 





 





<10>
zMotorControl.__extract_packetsN)#r"r#r$rNrOrPr<>rMrrr[r]rdrerhrorrrtrvrRr<>r|r<>r<>rmrQr<>r<>r<>r<>r<>r<>r<>r<>r{rrrrr%BsF


<04><04>
"#

	
	



r%cCs$||
k
r|
}dS||kr|}dSdSrr)<03>
x<>min<69>maxrrr<00>
LIMIT_MIN_MAX<41>
s




<04>r<EFBFBD>r<><00>x_min<69>x_maxcCs6t||
|<02>
||
}||
|}t
<01>|d
|>d
<00>
S<00>Nr*)r<>rNrg)r<>r<>r<><00>bits<74>span<61>	data_normrrrrL<00>
s



rLr<>r<>cCs2||
}t|<00>
d
|>d
}|||
}t
<01>|<06>
Sr<>)rrN<00>float32)r<>r<>r<>r<>r<>r<><00>temprrrr<><00>
s




r<>c
Cstd
|<00>}
t
d|
<01>S)N<>
f<>4B<34>rr<00><02>value<75>packedrrrrb<00>
s

rbc
CsFt|t
<01>rd
|k
rdk
rn
td<04>
<01>
td|<00>}
ntd<04>
<01>
td|
<01>S)Nr
l<00><EFBFBD><00>
Iz3Value must be an integer within the range of uint32r<32>)<05>
isinstancerpr<00>
ValueErrorrr<>rrrr<><00>
s
 <0C>
r<>c

CsPd
|k
r
dk
s$n

d|k
rdk
s$n

d|k
r!dk
r&dS
dSdSdS)	zW
    check if the number is in the range of uint32
    :param number:
    :return:
    rKr+r<>rE<00>#<00>$TFr)
<01>numberrrrr<>	s>
<02><04>
r<>cC<00>td
||
||<03>}t
d|<04>dS)N<><4Bz<Ir
r<><00>Zbyte1Zbyte2Zbyte3Zbyte4r<34>rrrr<><00>r<>cCr<>)Nr<4E>z<fr
r<>r<>rrrr<>r<>r<>c
Cs d
d<02>|D<00>
}
td<03>
|
<01>
<01>

dS)Nc
Ssg|]}
|
d<00><04>qS)
<01>02Xr)<02>.0<EFBFBD>byterrr<00>
<listcomp>#szprint_hex.<locals>.<listcomp><3E>
 )r<ry)r~Z
hex_valuesrrr<00>	print_hex"s

r<>cCs z|
|<00>
WSty


YdSwr)
r<>)<02>index<65>
enum_classrrr<00>get_enum_by_index's





<02>r<EFBFBD>c
@s<eZ
dZd
ZdZdZdZdZdZdZ	dZ
d	Zd
ZdZ
dZd
S)<0E>
DM_Motor_Typer
r*rDr)rHrCrJrKr,<00>	r+<00>N)r"r#r$ZDM4310Z
DM4310_48V<EFBFBD>DM4340Z
DM4340_48VZDM6006ZDM8006ZDM8009ZDM10010LZDM10010ZDMH3510<31>DMH6215ZDMG6220rrrrr<>.s











r<>c
@s<>eZ
dZd
ZdZdZdZdZdZdZ	dZ
d	Zd
ZdZ
dZd
ZdZdZdZdZdZdZdZdZdZdZdZdZdZdZdZdZdZ dZ!d Z"d!Z#d"Z$d#Z%d$Z&d%Z'd&Z(d'Z)d(Z*d)Z+d*Z,d+Z-d,Z.d-Z/d.S)/<2F>DM_variabler
r*rDr)rHrCrJrKr,r<>r+r<>rBr<>r<>rIrEr<><00><00>r1r<><00><00>rxr4<00><00>r/r<>r(<00><00> <00>!<00>"r<>r<>r.r<><00>4<00>5r3<00>7<00>P<00>QN)0r"r#r$ZUV_ValueZKT_ValueZOT_ValueZOC_ValueZACCZDECZMAX_SPDZMST_IDZESC_ID<49>TIMEOUTZ	CTRL_MODEZDampZInertiaZhw_verZsw_verZSNZNPPZRsZLSZFluxZGrr<72>r<>r<>ZI_BWZKP_ASRZKI_ASRZKP_APRZKI_APRZOV_ValueZGREFZDetaZV_BWZIQ_c1ZVL_c1Zcan_brZsub_verZu_offZv_off<66>k1<6B>k2Zm_off<66>dirZp_mZxoutrrrrr<>=s\












































r<>c
@seZ
dZd
ZdZdZdZdS)rr*rDr)rHN)r"r#r$rZPOS_VELZVELZ
Torque_Posrrrrrms




r)<1A>timer<00>numpyrN<00>enumr<00>structrrrr%r<>rrLrgr<>rbr<>r<>r<>r<>r<>r<>r<>r<>rrrrr<00><module>
s.



:"0