def set_mode(self,mode, num_tries = 5):
# 1 for wheel, 2 for joint
if((mode != 1) & (mode != 2)):
print_v('Error: Mode must be 1 or 2')
return -1
retry = 0
while(retry < num_tries):
if(mode == 1):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CW_LIMIT, 0)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CCW_LIMIT, 0)
if(mode == 2):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CW_LIMIT, 0)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CCW_LIMIT, 4095)
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:
self.mode = mode
return 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
def is_moving(self, num_tries = RETRIES):
retry = 0
while(retry < num_tries):
moving = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_MOVING)
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 moving
print('[COMM_ERR] ' + str(get_caller()))
return None
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)
print('[COMM_ERR] ' + str(get_caller()))
return None
def get_voltage(self, num_tries = RETRIES):
retry = 0
while(retry < num_tries):
voltage = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_VOLTAGE)
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 voltage/10.0
print('[COMM_ERR] ' + str(get_caller()))
return None
def set_torque_limit(self, torque, num_tries = RETRIES):
retry = 0
while(retry < num_tries):
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_TORQUE_LIMIT, int(torque * 1023))
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 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
def disable_torque(self, num_tries = RETRIES):
retry = 0
while(retry < num_tries):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_TORQUE_ENABLE, 0)
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 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
def set_position(self, pos, num_tries = RETRIES):
value = self.rad2value(pos)
retry = 0
while(retry < num_tries):
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_GOAL_POSITION, value)
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 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
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
print('[COMM_ERR] ' + str(get_caller()))
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
print('[COMM_ERR] ' + str(get_caller()))
return None
def set_speed(self, speed, num_tries = RETRIES):
if(self.mode == 2):
value = int(1023*abs(speed))
elif(self.mode == 1):
value = int(1023*abs(speed))
if(speed>0.0):
value = value + 1024
retry = 0
while(retry < num_tries):
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_MOVING_SPEED, value)
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 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
def set_gains(self,kp,ki,kd, num_tries = RETRIES):
retry = 0
while(retry < num_tries):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_P_GAIN, kp)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_I_GAIN, ki)
dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL)
derror += dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_D_GAIN, kd)
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 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
def get_mode(self, num_tries = RETRIES):
mode=0
retry = 0
while(retry < num_tries):
cw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CW_LIMIT)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
ccw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CCW_LIMIT)
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((cw_lim == 0)&(ccw_lim == 0)):
mode = 1
else:
mode = 2
return mode
print('[COMM_ERR] ' + str(get_caller()))
return -1
