def printData():
# Read Dynamixel Data
# read1ByteTxRx return byte
# read2ByteTxRx return int
global t1
global velocity
tmpTime = timedelta(0, time.clock()-t1)
diff = (tmpTime.days * 24 * 60 * 60 + tmpTime.seconds) * 1000 + tmpTime.microseconds / 1000.0
if diff > 0.98: # each 1 ms = 0.98, each 10 ms = 9.98
dxl_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_POSITION)
dxl_present_speed = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_SPEED)
dxl_present_load = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_LOAD)
tmp_temperature = dynamixel.readTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_TEMPERATURE, 1)
dxl_present_temperature = dynamixel.getDataRead(port_num, PROTOCOL_VERSION, 1, 0)
tmp_voltage = dynamixel.readTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_VOLTAGE, 1)
dxl_present_voltage = dynamixel.getDataRead(port_num, PROTOCOL_VERSION, 1, 0)
dxl_current = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_CURRENT)
newTime = timedelta(0, time.clock()-t0)
end = round((newTime.days * 24 * 60 * 60 + newTime.seconds) * 1000 + newTime.microseconds / 1000.0,1)
t1 = time.clock()
# 4095 steps on 280.6 degree = 0,06852258852258852258852258852259
dxl_angle = dxl_present_position * 0.06852258852258852258852258852259
steer_angle = (dxl_angle/62) * 30
# 0 - 1023
speed = dxl_present_speed * 0.111 # in rpm (max 627 = 69.9 rpm)
load = dxl_present_load # 0-1023 CCW , 1024 - 2048 CW
temperature = dxl_present_temperature
voltage = dxl_present_voltage / 10.0
current = dxl_current
torque = 0
csvwriterUnits.writerow([end,steer_angle,speed,load,temperature,voltage,current,int(str(velocity).encode('hex'), 16)])
csvwriterRaw.writerow([end,dxl_present_position,dxl_present_speed,dxl_present_load,dxl_present_temperature,dxl_present_voltage,dxl_current,int(str(velocity).encode('hex'), 16)])
def get_fsr_readings(self, foot):
id_dic = {'left': 111, 'right': 112}
id = id_dic[foot]
#fsr_reading = {'1':0, '2':0, '3':0, '4':0, 'x':0, 'y':0}
fsr_reading = {}
fsr_reg1 = {
'1': ADDR_FSR_1,
'2': ADDR_FSR_2,
'3': ADDR_FSR_3,
'4': ADDR_FSR_4
}
fsr_reg2 = {'x': ADDR_FSR_X, 'y': ADDR_FSR_Y}
for reg in fsr_reg1.keys():
fsr_reading[reg] = self.to_newton(
dxl.read2ByteTxRx(self.port, self.protocol, id, fsr_reg1[reg]))
self.check_result(id)
self.check_error(id)
for reg in fsr_reg2.keys():
fsr_reading[reg] = dxl.read1ByteTxRx(self.port, self.protocol, id,
fsr_reg2[reg])
self.check_result(id)
self.check_error(id)
return fsr_reading
def readValue(self, address, size=1):
if(size==1):
v = dxl.read1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, address)
elif(size==2):
v = dxl.read2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, address)
return v
def GetGoalPosition(self, Id):
try:
goal_position = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.GOAL_POSITION)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return goal_position
except :
print "Oops! Problem reading..."
def GetTorqueLimit(self, Id):
try:
torque_limit = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_LIMIT)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return torque_limit
except :
print "Oops! Problem reading Torque limit..."
def GetMovingSpeed(self, Id):
try:
moving_speed = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MOVING_SPEED)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return moving_speed
except :
print "Oops! Problem reading moving speed..."
def GetCCWAngleLimit(self, Id):
try:
ccw_angle_limit = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CCW_ANGLE_LIMIT)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return ccw_angle_limit
except :
print "Oops! Problem reading..."
def GetPresentLoad(self, Id):
try:
present_load = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_LOAD)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return present_load
except :
print "Oops! Problem writing goal position..."
def GetMaxTorque(self, Id, maxTorque):
try:
max_torque = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MAX_TORQUE)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return max_torque
except :
print "Oops! Problem reading max torque..."
def get_present_position(self, ids):
positions = []
for id in ids:
present_position = dxl.read2ByteTxRx(self.port, self.protocol, id,
ADDR_PRES_POS)
# if not self.check_result() and not self.check_error():
present_position = self.to_degree(present_position) - 180
positions.append(present_position)
return dict(zip(ids, positions))
def get_position(self):
pos = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_POSITION)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
return self.value2rad(pos)
def _read(self, DXL_ID):
dxl_present_position = dynamixel.read2ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, self.ADDR_MX_PRESENT_POSITION)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler, self.PROTOCOL_VERSION)
if(dxl_comm_result != COMM_SUCCESS):
print(dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
elif(dxl_error != 0):
print(dynamixel.getRxPacketError(self.PROTOCOL_VERSION, dxl_error))
return (dxl_present_position - 2048) * 0.088
def readValue(self, address, size=1):
if (size == 1):
v = dxl.read1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id,
address)
elif (size == 2):
v = dxl.read2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id,
address)
return v
def get_load(self):
load = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_LOAD)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
return (load/1023.0)-1.0
def GetPresentPosition(self, Id):
try:
present_position = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_POSITION)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
else:
return present_position
except :
print "Oops! Problem writing goal position..."
return -3141592
def receiveCurrAngle(self):
''' Reads the current position of this
servomotor alone. The read position is
stored and can be accessed via method
getAngle()
'''
self.currValue = dxl.read2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
ADDR_MX_PRESENT_POSITION) - self.centerValue
self.currAngle = pi * float(self.currValue) / 2048.0
return self.currAngle
def GetGoalPosition(self, Id):
goal_position = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.GOAL_POSITION)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return goal_position
def GetMovingSpeed(self, Id):
moving_speed = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.MOVING_SPEED)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return moving_speed
def GetMaxTorque(self, Id, maxTorque):
max_torque = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.MAX_TORQUE)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return max_torque
def GetTorqueLimit(self, Id):
torque_limit = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.TORQUE_LIMIT)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return torque_limit
def GetPresentLoad(self, Id):
present_load = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.PRESENT_LOAD)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return present_load
def GetCWAngleLimit(self, Id):
cw_angle_limit = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION,
Id, Registers.CW_ANGLE_LIMIT)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return cw_angle_limit
def receiveCurrAngle(self):
''' Reads the current position of this
servomotor alone. The read position is
stored and can be accessed via method
getAngle()
'''
self.currValue = dxl.read2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
ADDR_MX_PRESENT_POSITION) - self.centerValue
self.currAngle = pi*float(self.currValue)/2048.0
return self.currAngle
def get_position(self, num_tries = RETRIES):
retry = 0
while(retry < num_tries):
pos = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_POSITION)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry +=1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry +=1
else:
return self.value2rad(pos)
def GetMaxTorque(self, Id, maxTorque):
try:
max_torque = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION,
Id, Registers.MAX_TORQUE)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return max_torque
except:
print "Oops! Problem reading max torque..."
def GetPresentLoad(self, Id):
try:
present_load = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION,
Id, Registers.PRESENT_LOAD)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return present_load
except:
print "Oops! Problem writing goal position..."
def GetCCWAngleLimit(self, Id):
try:
ccw_angle_limit = dynamixel.read2ByteTxRx(
self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.CCW_ANGLE_LIMIT)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return ccw_angle_limit
except:
print "Oops! Problem reading..."
def GetPresentPosition(self, Id):
# Read present position
present_position = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION,
Id,
Registers.PRESENT_POSITION)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
else:
return present_position
def GetPresentPosition(self, Id):
try:
present_position = dynamixel.read2ByteTxRx(
self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.PRESENT_POSITION)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
else:
return present_position
except:
print "Oops! Problem writing goal position..."
return -3141592
def get_load(self, num_tries=RETRIES):
retry = 0
while (retry < num_tries):
load = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_PRESENT_LOAD)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return (load / 1023.0) - 1.0
raise Exception("get_load failed after {} tries".format(num_tries))
def PresentPos_1(self):
#Print current pos without goalpos
global DXL_MOVING_STATUS_THRESHOLD
#Read present position
self.dxl_present_position = dynamixel.read2ByteTxRx(
self.port_num, self.pro_ver, self.ID, self.addr_present)
self.dxl_comm_result = dynamixel.getLastTxRxResult(
self.port_num, self.pro_ver)
self.dxl_error = dynamixel.getLastRxPacketError(
self.port_num, self.pro_ver)
if self.dxl_comm_result != self.COMM_SUCCESS:
return (dynamixel.getTxRxResult(self.pro_ver,
self.dxl_comm_result))
elif self.dxl_error != 0:
return (dynamixel.getRxPacketError(self.pro_ver, self.dxl_error))
print("[ID:%03d] PresPos:%03d" % (self.ID, self.dxl_present_position))
return None
def get_speed(self, num_tries = RETRIES):
# returns normalized speed. multiply by max speed to get radians/s
retry = 0
while(retry < num_tries):
speed = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_SPEED)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry +=1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry +=1
else:
if(speed > 1024):
return (speed & 1023)/1023.0
else:
return -(speed & 1023)/1023.0
def get_load(self, num_tries = RETRIES):
# returns normalized load
retry = 0
while(retry < num_tries):
load = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_LOAD)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry +=1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry +=1
else:
if(load > 1024):
return (load & 1023)/1023.0
else:
return -(load & 1023)/1023.0
def Read_Value(self, motorId, read_addr, read_len):
# Read value
dxl_result = -1
if(read_len == 4):
dxl_result = dynamixel.read4ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr)
elif(read_len == 2):
dxl_result = dynamixel.read2ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr)
elif(read_len == 1):
dxl_result = dynamixel.read1ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr)
else:
print('[ID:%03d]: invalid read length %d' %(motorId, read_len))
return dxl_result
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
return dxl_result
def PresentPos_spread(self):
#Print current pos with goalpos
global DXL_MOVING_STATUS_THRESHOLD
#Read present position
self.dxl_present_position = dynamixel.read2ByteTxRx(
self.port_num, self.pro_ver, self.ID, self.addr_present)
self.dxl_comm_result = dynamixel.getLastTxRxResult(
self.port_num, self.pro_ver)
self.dxl_error = dynamixel.getLastRxPacketError(
self.port_num, self.pro_ver)
if self.dxl_comm_result != self.COMM_SUCCESS:
return (dynamixel.getTxRxResult(self.pro_ver,
self.dxl_comm_result))
elif self.dxl_error != 0:
return (dynamixel.getRxPacketError(self.pro_ver, self.dxl_error))
print("[ID:%03d] GoalPos:%03d PresPos:%03d" %
(self.ID, self.goalpos, self.dxl_present_position))
if not (abs(self.goalpos - self.dxl_present_position) >
DXL_MOVING_STATUS_THRESHOLD_SPREAD):
return True
return False
# Add Dynamixel#2 goal position value to the Syncwrite parameter storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID))
# Syncwrite goal position
dynamixel.groupSyncWriteTxPacket(group_num)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
# Clear syncwrite parameter storage
dynamixel.groupSyncWriteClearParam(group_num)
while 1:
# Read Dynamixel#1 present position
dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
# Read Dynamixel#2 present position
dxl2_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
print("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position))
if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) or (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)):
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
while 1:
# Read present position
dxl_present_position = dynamixel.read4ByteTxRx(
port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_PRESENT_POSITION)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num,
PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
dxl_time = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID,
ADDR_PRO_REALTIME_TICK)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num,
PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
currentTimeuS = datetime.datetime.now().microsecond
print(
"[ID:%03d] GoalPos:%03d PresPos:%03d Device time %d Local time: %d"
% (DXL_ID, dxl_goal_position[index], dxl_present_position,
dxl_time, currentTimeuS))
if not (abs(dxl_goal_position[index] - dxl_present_position) >
DXL_MOVING_STATUS_THRESHOLD):
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# Write Dynamixel#2 goal position
dynamixel.write2ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID,
ADDR_MX_GOAL_POSITION, dxl_goal_position[index])
dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
while 1:
# Read present position
dxl1_present_position = dynamixel.read2ByteTxRx(
port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num1,
PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# Read present position
dxl2_present_position = dynamixel.read2ByteTxRx(
port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num2,
PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
def motor_operate(self, theta1_vp, theta2_vp, e, tx, ty, div_pt_x_vp, div_pt_y_vp):
# ta1_vp and theta2_vp from list to float type
theta1_vp = theta1_vp.tolist()
theta2_vp = theta2_vp.tolist()
for i in range(len(theta1_vp)):
theta1_vp[i] = float(theta1_vp[i][0])
theta2_vp[i] = float(theta2_vp[i][0])
# Read a keypress and return the resulting character as a byte string.
if os.name == 'nt':
import msvcrt
def getch():
return msvcrt.getch().decode()
else:
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
def getch():
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
# Control table address
ADDR_MX_TORQUE_ENABLE = 24 # 1 byte RW # Control table address is different in Dynamixel model
ADDR_MX_GOAL_POSITION = 30 # 2 bytes RW
ADDR_MX_PRESENT_POSITION = 36 # 2 bytes R
ADDR_MX_MOVING_SPEED = 32 # 2 bytes RW
ADDR_MX_PRESENT_SPEED = 38 # 2 bytes R
ADDR_MX_PRESENT_LOAD = 40 # 2 bytes R
ADDR_MX_PRESENT_VOLTAGE = 42 # 1 byte R
ADDR_MX_GOAL_ACCELERATION = 73 # 1 byte RW
ADDR_MX_MAX_TORQUE = 14 # 2 bytes RW
# PID Controller Address
ADDR_MX_DERIVATIVE_GAIN = 26 # 1 byte RW
ADDR_MX_INTEGRAL_GAIN = 27 # 1 byte RW
ADDR_MX_PROPORTIONAL_GAIN = 28 # 1 byte RW
# Protocol version
PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel
# Default setting
DXL_ID = [1, 2, 3, 4]
BAUDRATE = 1000000
DEVICENAME = "COM7".encode('utf-8') # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
# Resolution is 0.088 degree / Position 0~4095 /
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 5 # Dynamixel moving status threshold
ESC_ASCII_VALUE = 0x1b # for ESC key to escape out from the operation
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_error = 0 # Dynamixel error
dxl_present_position = 0 # Present position
theta1 = []
for i in range(len(theta1_vp)):
theta1.append(int((180 * theta1_vp[i]) / (0.088 * np.pi)) + theta1_offset - 2048 + dynamixel_offset_90)
print('=====================theta1 =======================')
print(theta1)
theta2 = []
for i in range(len(theta2_vp)):
theta2.append(int((180 * theta2_vp[i]) / (0.088 * np.pi)) + theta2_offset - 2048 + 2048)
print('=====================theta2=======================')
print(theta2)
theta3 = [theta3_offset] * len(theta2_vp)
#theta4 = [2048]
# Open port
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
print("Press any key to terminate...")
getch()
quit()
# Set port baudrate
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
#set position P gain, I gain, D gain - all 1 byte - P only 254 for unit communication
# Motor 1
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_INTEGRAL_GAIN, 30)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PROPORTIONAL_GAIN, 80)
# Motor 2
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_INTEGRAL_GAIN, 50)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_PROPORTIONAL_GAIN, 120)
# Motor 3
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_INTEGRAL_GAIN, 50)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_PROPORTIONAL_GAIN, 120)
# Motor 4
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_INTEGRAL_GAIN, 50)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_PROPORTIONAL_GAIN, 120)
for id in DXL_ID:
# Enable Dynamixel Torque - 1byte
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_MAX_TORQUE, 1024)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
# Initialize Goal Position 2048
dxl_goal_position_init = 2048
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_MAX_TORQUE, 1024)
# Estimate time
start_time = time.time()
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, 2200)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, 2200)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, 2048)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500)
time.sleep(2)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[0])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[0])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[0])
time.sleep(5)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038)
time.sleep(4)
while not (len(theta1) - index) == 0:
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[index])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[index])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[index])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# write moving speed
for id in DXL_ID:
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_MOVING_SPEED, 50)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_MOVING_SPEED, 20)
while 1:
# Read present position
dxl_present_position_theta1 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0],
ADDR_MX_PRESENT_POSITION)
dxl_present_position_theta2 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1],
ADDR_MX_PRESENT_POSITION)
dxl_present_position_theta3 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2],
ADDR_MX_PRESENT_POSITION)
dxl_present_position_theta4 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3],
ADDR_MX_PRESENT_POSITION)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# read voltage
dxl_present_voltage = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0],
ADDR_MX_PRESENT_VOLTAGE)
# read load
dxl_present_load = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_LOAD)
# read present speed
dxl_present_vel = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_SPEED)
#print("[ID:%03d] GoalPos:%03d PresPos:%03d PresVolt:%03d PresLoad:%03d PresVel:%03d" % (
#DXL_ID[0], theta2[index], dxl_present_position_theta2,
#dxl_present_voltage, dxl_present_load, dxl_present_vel))
#print('e', e.is_set())
if e.is_set() == 0:
pass
elif e.is_set() != 0:
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500)
time.sleep(5)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, 2200)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, 2200)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, 2048)
#contour
cam_paper = cv2.VideoCapture(0)
ret_paper, frame_paper = cam_paper.read()
frame_paper = cv2.flip(frame_paper, -1)
cv2.imshow('frame', frame_paper)
cv2.waitKey(1)
cont_paper = contour_paper.ContourPaper()
tx, ty = cont_paper.cont_paper(frame_paper)
print('translated tx and ty', tx, ty)
#paper move
#new space for current theta1 and theta2 to return IKINE
new_theta1 = np.zeros([len(theta1) - index, 1])
new_theta2 = np.zeros([len(theta2) - index, 1])
#new space of current theta1 and theta2 in degree
theta1_deg = np.zeros([])
theta2_deg = np.zeros([])
theta1_deg = np.delete(theta1_deg, 0)
theta2_deg = np.delete(theta2_deg, 0)
#current theta1 and theta2, remove past theta value
current_theta1 = theta1[index: ]
current_theta2 = theta2[index: ]
div_pt_x_vp = div_pt_x_vp[index: ]
div_pt_y_vp = div_pt_y_vp[index: ]
#print('x position, ', div_pt_x_vp)
#print('imhere1')
for i in range(len(current_theta1)):
theta1_deg = np.append(theta1_deg, ((current_theta1[i] - theta1_offset + 2048 - dynamixel_offset_90) * np.pi * 0.088 / 180))
theta2_deg = np.append(theta2_deg, ((current_theta2[i] - theta2_offset) * np.pi * 0.088 / 180))
IK = ikine.InverseKinematics()
for i in range(len(current_theta2)):
#print('theta1_deg', theta1_deg[i])
new_theta1[i], new_theta2[i] = IK.ikine(np.int(div_pt_x_vp[i]), np.int(div_pt_y_vp[i]), tx, ty)
#print('new_theta1, ', new_theta1)
new_theta1 = new_theta1.tolist()
new_theta2 = new_theta2.tolist()
for i in range(len(new_theta1)):
new_theta1[i] = float(new_theta1[i][0])
new_theta2[i] = float(new_theta2[i][0])
theta1 = []
for i in range(len(new_theta1)):
theta1.append(int((180 * new_theta1[i]) / (0.088 * np.pi)) + theta1_offset - 2048 + dynamixel_offset_90)
print('=====================theta1 =======================')
print(theta1)
theta2 = []
for i in range(len(new_theta2)):
theta2.append(int((180 * new_theta2[i]) / (0.088 * np.pi)) + theta2_offset - 2048 + 2048)
print('=====================theta2=======================')
print(theta2)
theta3 = [theta3_offset] * len(new_theta2)
index = 0
time.sleep(5)
break
else:
pass
if not ((abs(theta1[index] - dxl_present_position_theta1) > DXL_MOVING_STATUS_THRESHOLD) | ((abs(theta2[index] - dxl_present_position_theta2)) > DXL_MOVING_STATUS_THRESHOLD) | ((abs(theta3[index] - dxl_present_position_theta3)) > DXL_MOVING_STATUS_THRESHOLD)) :
break
# check event
#ABOUT TX and TY -> IKINE -> New theta1 and theta2
init_tx = tx
init_ty = ty
if e.is_set() != 0:
print('error')
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[0])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[0])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[0])
time.sleep(5)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038)
time.sleep(3)
e.clear()
else:
pass
index += 1
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500)
time.sleep(5)
# Disable Dynamixel Torque
for id in DXL_ID:
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# Close port
dynamixel.closePort(port_num)
return tx, ty
if getch() == chr(ESC_ASCII_VALUE):
break
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID,
ADDR_MX_GOAL_POSITION, dxl_goal_position[index])
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
while 1:
# Read present position
dxl_present_position = dynamixel.read2ByteTxRx(
port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_POSITION)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num,
PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
print("[ID:%03d] GoalPos:%03d PresPos:%03d" %
(DXL_ID, dxl_goal_position[index], dxl_present_position))
if not (abs(dxl_goal_position[index] - dxl_present_position) >
DXL_MOVING_STATUS_THRESHOLD):
break
