def setAngle(self, steerAngle):
print('setAngle begins')
self.mapTargetAngleToMotorValue(steerAngle)
print("Angle: " + str(steerAngle) + " mappedValue: " +
str(self.mappedSteerAngle))
if self.mappedSteerAngle <= 100 or self.mappedSteerAngle >= 4000:
return
# Write goal position
dynamixel.write2ByteTxRx(self.port_num, self.PROTOCOL_VERSION,
self.DXL_ID, self.ADDR_MX_MOVING_SPEED, 200)
dynamixel.write2ByteTxRx(self.port_num, self.PROTOCOL_VERSION,
self.DXL_ID, self.ADDR_MX_GOAL_POSITION,
int(self.mappedSteerAngle))
if dynamixel.getLastTxRxResult(
self.port_num, self.PROTOCOL_VERSION) != self.COMM_SUCCESS:
dynamixel.printTxRxResult(
self.PROTOCOL_VERSION,
dynamixel.getLastTxRxResult(self.port_num,
self.PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(self.port_num,
self.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
self.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
self.PROTOCOL_VERSION))
print('setAngle finished')
def SetTorqueLimit(self, Id, torqueLimit):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_LIMIT, torqueLimit)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
except :
print "Oops! Problem writing torque limit..."
def writeValue(self, address, value, size=1):
if(size==1):
dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
address, value)
elif(size==2):
dxl.write2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
address, value)
def SetCCWAngleLimit(self, Id, angleLimit):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CCW_ANGLE_LIMIT, angleLimit)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
except :
print "Oops! Problem writing ..."
def dxl_torcon(goal_torq):
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, 1, ADDR_MX_GOAL_TORQUE, goal_torq[0])
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))
def SetMaxTorque(self, Id, maxTorque):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MAX_TORQUE, maxTorque)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
except :
print "Oops! Problem writing max torque..."
def writeValue(self, address, value, size=1):
if (size == 1):
dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
address, value)
elif (size == 2):
dxl.write2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
address, value)
def setAngle(na):
print('setAngle begins')
global targetAngle
while True:
newTargetAngle = L.getTargets()[1]
#print('targetAngle, newTA: ' + str(targetAngle) + str(newTargetAngle))
#if targetAngle changes, send new angle to dynamixel
if targetAngle != newTargetAngle:
motorValue = mapTargetAngleToMotorValue(newTargetAngle)
targetAngle = newTargetAngle
#print(' targetAngle', targetAngle)
#motorValue = motorValue - 109 ????
#print('newMotorvalue' , motorValue) ???
if motorValue <= 100 or motorValue >= 4000:
return
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_MOVING_SPEED, 200)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, motorValue)
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))
def set_torque_limit(self, torque):
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(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
def set_angle_limit(id,cw_angle_limit, ccw_angle_limit,baudrate):
"""Configure the angle limits of a servo.
:param int id: Servo ``ìd``
:param int cw_angle_limit: Clockwise angle limit to be configured
:param int ccw_angle_limit: Counter-clockwise angle limit to be configured
:param baudrate: Baudrate of the servo to be configured
:return:
--``PORT_ERROR`` case it fails to open the port.
--``BAUDRATE_ERROR`` case it fails to change baudrate.
--``COMM_ERROR`` case there is a communication error.
--``HARDWARE_COMM_ERROR`` case there is a hardware communication error.
--``NONE`` case the operation succeeds."""
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Open port
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
return PORT_ERROR
# Set port baudrate
if dynamixel.setBaudRate(port_num, baudrate):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
return BAUDRATE_ERROR
#Write CW angle limit
dynamixel.write2ByteTxRx(port_num, PROTOCOL_1 , id, ADDR_CW_ANGLE_LIMIT, cw_angle_limit)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_1 , dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ))
return COMM_ERROR
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ) != 0:
dynamixel.printRxPacketError(PROTOCOL_1 , dynamixel.getLastRxPacketError(port_num,PROTOCOL_1 ))
return HARDWARE_COMM_ERROR
else:
print("CW angle changed to: %s" % cw_angle_limit)
time.sleep(0.5)
# Write CCW angle limit
dynamixel.write2ByteTxRx(port_num, PROTOCOL_1 , id, ADDR_CCW_ANGLE_LIMIT, ccw_angle_limit)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_1 , dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ))
return COMM_ERROR
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ) != 0:
dynamixel.printRxPacketError(PROTOCOL_1 , dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ))
return HARDWARE_COMM_ERROR
else:
print("CCW angle changed to: %s" % ccw_angle_limit)
time.sleep(0.5)
def SetTorqueLimit(self, Id, torqueLimit):
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.TORQUE_LIMIT, torqueLimit)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def set_speed(ID):
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, ID,
ADDR_MX_MOVING_SPEED, MOVING_SPEED)
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))
def SetGoalPosition(self, Id, goalPose):
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.GOAL_POSITION, goalPose)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def SetMovingSpeed(self, Id, movingSpeed):
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.MOVING_SPEED, movingSpeed)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def SetMaxTorque(self, Id, maxTorque):
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.MAX_TORQUE, maxTorque)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def set_position(self, pos):
value = self.rad2value(pos)
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(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
def SetCCWAngleLimit(self, Id, angleLimit):
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.CCW_ANGLE_LIMIT, angleLimit)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def SetMovingSpeed(self, Id, movingSpeed):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MOVING_SPEED, movingSpeed)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return False
else:
return True
except :
print "Oops! Problem writing moving speed..."
def init_actuator(dxl_id, operating_mode):
# Enable Dynamixel Torque
print "enabling pwm control"
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, dxl_id,
ADDR_X_OPERATING_MODE, operating_mode)
print "limiting torque"
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, dxl_id,
ADDR_X_PWM_LIMIT, PWM_LIMIT)
print "enabling torque"
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, dxl_id,
ADDR_X_TORQUE_ENABLE, TORQUE_ENABLE)
def write_value(ID, final_pos):
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, ID,
ADDR_MX_GOAL_POSITION, final_pos)
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))
def SetTorqueVale(self, Id, torqueValue, directionTurn):
if directionTurn == True:
torqueValue = torqueValue + 1024
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.MOVING_SPEED, movingSpeed)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def sendGoalAngle(self, goalAngle=None):
''' Sends a command to this specific
servomotor to set its goal angle.
If no parameter is passed then it
sends the goal angle that was set
via setGoalAngle()
'''
if goalAngle:
self.setGoalAngle(goalAngle)
dxl.write2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
ADDR_MX_GOAL_POSITION, self.goalValue)
def sendMaxTorque(self, maxTorque=None):
''' Sends a command to this specific
servomotor to set its maximum torque.
If the argument maxTorque is not
provided, then it sends the last
value set using setMaxTorque().
'''
if maxTorque:
self.setMaxTorque(maxTorque)
dxl.write2ByteTxRx(self.socket, PROTOCOL_VERSION, \
self.servo_id, MAXADDR_MX_TORQUE_ENABLE, self.maxTorque)
def SetCWAngleLimit(self, Id, angleLimit):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION,
Id, Registers.CW_ANGLE_LIMIT, angleLimit)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
except:
print "Oops! Problem writing ..."
def SetTorqueVale(self, Id, torqueValue, directionTurn):
try:
if directionTurn == True:
torqueValue = torqueValue + 1024
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MOVING_SPEED, torqueValue)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return False
else:
return True
except :
print "Oops! Problem writing torque value..."
def SetMaxTorque(self, Id, maxTorque):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION,
Id, Registers.MAX_TORQUE, maxTorque)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
except:
print "Oops! Problem writing max torque..."
def SetTorqueLimit(self, Id, torqueLimit):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION,
Id, Registers.TORQUE_LIMIT, torqueLimit)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
except:
print "Oops! Problem writing torque limit..."
def set_torque_max(id, percentage, baudrate):
"""Sets a servo max torque.
:param int id: Servo ``ìd``
:param baudrate: Baudrate of the servo to be configured
:param int percentage: Torque percentage
:return:
--``PORT_ERROR`` case it fails to open the port.
--``BAUDRATE_ERROR`` case it fails to change baudrate.
--``COMM_ERROR`` case there is a communication error.
--``HARDWARE_COMM_ERROR`` case there is a hardware communication error.
--``NONE`` case the operation succeeds."""
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Open port
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
return PORT_ERROR
# Set port baudrate
if dynamixel.setBaudRate(port_num, baudrate):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
return BAUDRATE_ERROR
#Converting percentage to bit value (check dynamixel e-manual for info)
if percentage == 100:
value = 1023
else:
value = int(percentage / 0.0977517107)
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_1, id, ADDR_MAX_TORQUE, value)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_1)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_1)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_1, dxl_comm_result))
return COMM_ERROR
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_1, dxl_error))
return HARDWARE_COMM_ERROR
else:
print("Torque set to %s " % percentage)
time.sleep(0.2)
dynamixel.closePort(port_num)
def sendGoalAngle(self, goalAngle = None):
''' Sends a command to this specific
servomotor to set its goal angle.
If no parameter is passed then it
sends the goal angle that was set
via setGoalAngle()
'''
if goalAngle:
self.setGoalAngle(goalAngle)
dxl.write2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
ADDR_MX_GOAL_POSITION, self.goalValue)
def _disable_torque(self, DXL_ID):
dynamixel.write2ByteTxRx(self.portHandler, self.PROTOCOL_VERSION,
DXL_ID, self.ADDR_MX_TORQUE_ENABLE, 0)
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))
def sendMaxTorque(self, maxTorque = None):
''' Sends a command to this specific
servomotor to set its maximum torque.
If the argument maxTorque is not
provided, then it sends the last
value set using setMaxTorque().
'''
if maxTorque:
self.setMaxTorque(maxTorque)
dxl.write2ByteTxRx(self.socket, PROTOCOL_VERSION, \
self.servo_id, MAXADDR_MX_TORQUE_ENABLE, self.maxTorque)
def Move(self, goalpos):
self.goalpos = int(goalpos)
# Write goal position
dynamixel.write2ByteTxRx(self.port_num, self.pro_ver, self.ID,
self.addr_goal, self.goalpos)
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:
print(dynamixel.getTxRxResult(self.pro_ver, self.dxl_comm_result))
elif self.dxl_error != 0:
print(dynamixel.getRxPacketError(self.pro_ver, self.dxl_error))
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
def set_speed(self, speed):
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
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(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
def _set_moving_speed(self, DXL_ID, speed):
dynamixel.write2ByteTxRx(self.portHandler, self.PROTOCOL_VERSION,
DXL_ID, self.ADDR_MX_SPEED,
int(speed * 1024 / 1000))
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))
def _write(self, DXL_ID, angle):
dynamixel.write2ByteTxRx(self.portHandler, self.PROTOCOL_VERSION,
DXL_ID, self.ADDR_MX_GOAL_POSITION,
2048 + int(angle / 0.088))
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))
def SetGoalPosition(self, Id, goalPose):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.GOAL_POSITION, goalPose)
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.GOAL_POSITION, goalPose)
if dynamixel.getLastTxRxResult(self.port_num, Registers.PROTOCOL_VERSION) != Registers.COMM_SUCCESS:
#dynamixel.printTxRxResult(Registers.PROTOCOL_VERSION, dynamixel.getLastTxRxResult(self.port_num, Registers.PROTOCOL_VERSION))
return False
elif dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
else:
return True
except :
print "Oops! Problem writing..."
return False
def SetMovingSpeed(self, Id, movingSpeed):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION,
Id, Registers.MOVING_SPEED, movingSpeed)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return False
else:
return True
except:
print "Oops! Problem writing moving speed..."
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
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION2, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION2, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2))
else:
print("Dynamixel#%d has been successfully connected" % (DXL2_ID))
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# Write Dynamixel#1 goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl1_goal_position[index])
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION1, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION1, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1))
# Write Dynamixel#1 goal position
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_GOAL_POSITION, dxl2_goal_position[index])
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION2, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION2, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2))
while 1:
# Read Dynamixel#1 present position
dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_PRESENT_POSITION)
# Profile Velocity Limit #3
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL3_ID, ADDR_XM430_PROF_VELOCITY, dxl_prof_vel)
# Profile Velocity Limit #4
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL4_ID, ADDR_XM430_PROF_VELOCITY, dxl_prof_vel)
# Profile Velocity Limit #5
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL5_ID, ADDR_XM430_PROF_VELOCITY, dxl_prof_vel)
# Profile Velocity Limit #6
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL6_ID, ADDR_XM430_PROF_VELOCITY, dxl_prof_vel)
# Profile Velocity Limit #7
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL7_ID, ADDR_XM430_PROF_VELOCITY, dxl_prof_vel)
# Enable position p gain #1
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_XM430_POSITION_P_GAIN, dxl_position_p_gain)
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))
# Enable position p gain #2
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_XM430_POSITION_P_GAIN, dxl_position_p_gain)
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))
# Enable position p gain #3
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL3_ID, ADDR_XM430_POSITION_P_GAIN, dxl_position_p_gain)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
# Disable Dynamixel Torque :
# Indirect address would not accessible when the torque is already enabled
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_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))
else:
print("Dynamixel has been successfully connected")
# INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0)
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))
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1)
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))
