30 lines
674 B
Python
30 lines
674 B
Python
import time
|
|
import sys
|
|
sys.path.append('../lib')
|
|
from unitree_actuator_sdk import *
|
|
|
|
|
|
serial = SerialPort('/dev/ttyUSB0')
|
|
cmd = MotorCmd()
|
|
data = MotorData()
|
|
|
|
while True:
|
|
data.motorType = MotorType.B1
|
|
cmd.motorType = MotorType.B1
|
|
cmd.mode = queryMotorMode(MotorType.B1,MotorMode.FOC)
|
|
cmd.id = 0
|
|
cmd.q = 0.0
|
|
cmd.dq = 6.28*queryGearRatio(MotorType.B1)
|
|
cmd.kp = 0.0
|
|
cmd.kd = 3
|
|
cmd.tau = 0.0
|
|
serial.sendRecv(cmd, data)
|
|
print('\n')
|
|
print("q: " + str(data.q))
|
|
print("dq: " + str(data.dq))
|
|
print("temp: " + str(data.temp))
|
|
print("merror: " + str(data.merror))
|
|
print('\n')
|
|
time.sleep(0.0002) # 200 us
|
|
|