def init(self):
self.I2C = I2C(self)
self.rgbEyes = RGBEyes(self)
# preset
self.rgbEyes.setColor(xrange(18), 0, 0, 0)
self.rgbEyes.setColor([0,1,2,9,10,11], 0, 0, 7)
self.rgbEyes.setColor([4,13], 7, 7, 7)
self.setPose(EZB.DEFAULT_POSE)
def init(self):
self.I2C = I2C(self)
self.rgbEyes = RGBEyes(self)
# preset
self.rgbEyes.setColor(xrange(18), 0, 0, 0)
self.rgbEyes.setColor([0, 1, 2, 9, 10, 11], 0, 0, 7)
self.rgbEyes.setColor([4, 13], 7, 7, 7)
self.setPose(EZB.DEFAULT_POSE)
class EZB(object):
# Default IP Address and Port for the JD Humanoid Robot.
ConnectedEndPointAddress = "192.168.1.1"
ConnectedEndPointPort = 23
CmdUnknown = 0
CmdReleaseAllServos = 1
CmdGetUniqueID = 2
CmdEZBv3 = 3
CmdEZBv4 = 4
CmdSoundBeep = 5
CmdEZServo = 6
CmdI2CWrite = 10
CmdI2CRead = 11
CmdBootLoader = 14
CmdSetPWMSpeed = 15 # +23 (38)
CmdSetServoSpeed = 39 # +23 (62)
CmdPing = 0x55
CmdSetDigitalPortOn = 100 # +23 (123)
CmdSetDigitalPortOff = 124 # +23 (147)
CmdGetDigitalPort = 148 # +23 (171)
CmdSetServoPosition = 172 # +23 (195)
CmdGetADCValue = 196 # +7 (203)
CmdSendSerial = 204 # +23 (227)
CmdHC_SR04 = 228 # +23 (251)
CmdSoundStreamCmd = 254
# Existing motor ID's are D0-D9, D12-D14 and D16-D18 there are more limits
LIMITS = [
(30, 180),
(70, 170),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 160),
(0, 180),
(0, 130),
(0, 180),
(0, 160),
(0, 180),
(50, 130),
(0, 180),
(0, 180),
(0, 180),
]
SERVOS = 23
SERVO_SPEED_SLOWEST = 20
SERVO_SPEED_FASTEST = 0
CmdSoundInitStop = 0
CmdSoundLoad = 1
CmdSoundPlay = 2
DEFAULT_P = [
(0, 110), # head left - right
(1, 90), # neck up - down
(6, 90), # right hand closed
(5, 110), # right elbow
(4, 160), # right shoulder close - open
(3, 170), # right shoulder rotation
(9, 90), # left hand
(8, 70), # left elbow
(7, 20), # left shoulder close - open
(2, 10), # left shoulder rotation
(14, 90), # right feet
(13, 80), # right knee
(12, 79), # right hip
(18, 80), # left feet
(17, 80), # left knee
(16, 90), # left hip
]
DEFAULT_POSE = [-1] * 23
for servo, position in DEFAULT_P:
DEFAULT_POSE[servo] = position
def __init__(self, ip=ConnectedEndPointAddress, port=ConnectedEndPointPort):
self.ip = ip
self.port = port
self.sock = None
self.isConnected = False
self._currentPose = [-1] * 24
self.connect()
self.init()
def init(self):
self.I2C = I2C(self)
self.rgbEyes = RGBEyes(self)
# preset
self.rgbEyes.setColor(xrange(18), 0, 0, 0)
self.rgbEyes.setColor([0, 1, 2, 9, 10, 11], 0, 0, 7)
self.rgbEyes.setColor([4, 13], 7, 7, 7)
self.setPose(EZB.DEFAULT_POSE)
def connect(self):
try:
self.sock = socket.create_connection((self.ip, self.port), timeout=10)
self.isConnected = True
except:
raise RuntimeError(EMSG_ROBOT_NOT_FOUND % (self.ip, self.port))
def sendCommand(self, bytesToExpect, cmd, params=None):
# check command
if cmd == EZB.CmdUnknown:
raise ValueError, "unknown command: %s" % cmd
# prepare data
cmd_data = bytearray([cmd])
if params:
cmd_data += bytearray([params]) if not isinstance(params, bytearray) else params
# send data
self.sock.send(cmd_data)
if bytesToExpect > 0:
return self.sock.recv(bytesToExpect)
def __del__(self):
if self.isConnected and self.sock:
self.sock.close()
def setPosition(self, servo, position):
""" This method sets a position of the specified servo.
The joint position values are clipped to the values defined in LIMITS.
@param servo - id of the servo
@param position - absolute value for the given servo position
"""
position = int(position)
# check general limits
if 0 <= servo < EZB.SERVOS and 0 <= position <= 180:
# clip joint limits
low, high = self.LIMITS[servo]
position = min(max(low, position), high)
# send command
self.sendCommand(0, EZB.CmdSetServoPosition + servo, position)
self._currentPose[servo] = position
def setServoSpeed(self, servo, speed):
""" This method sets the speed of the specified servo.
@param servo - id of the servo
@param position - absolute value for the given servo position
"""
speed = int(speed)
# check general limits
if 0 <= servo < EZB.SERVOS and EZB.SERVO_SPEED_FASTEST <= speed <= EZB.SERVO_SPEED_SLOWEST:
self.sendCommand(0, EZB.CmdSetServoSpeed + servo, speed)
def getCurrentPose(self):
""" This method returns a copy of the current pose.
@return list - [ <position>* ] The index in the list is the id of the servo.
"""
return [x for x in self._currentPose]
def setPose(self, pose):
""" This methods sets a full pose to the robot.
@param pose - [ <position>* ] The index in the list is the id of the servo.
"""
for servo, position in enumerate(pose):
self.setPosition(servo, position)
class EZB(object):
# Default IP Address and Port for the JD Humanoid Robot.
ConnectedEndPointAddress = '192.168.1.1'
ConnectedEndPointPort = 23
CmdUnknown = 0
CmdReleaseAllServos = 1
CmdGetUniqueID = 2
CmdEZBv3 = 3
CmdEZBv4 = 4
CmdSoundBeep = 5
CmdEZServo = 6
CmdI2CWrite = 10
CmdI2CRead = 11
CmdBootLoader = 14
CmdSetPWMSpeed = 15 # +23 (38)
CmdSetServoSpeed = 39 # +23 (62)
CmdPing = 0x55
CmdSetDigitalPortOn = 100 # +23 (123)
CmdSetDigitalPortOff = 124 # +23 (147)
CmdGetDigitalPort = 148 # +23 (171)
CmdSetServoPosition = 172 # +23 (195)
CmdGetADCValue = 196 # +7 (203)
CmdSendSerial = 204 # +23 (227)
CmdHC_SR04 = 228 # +23 (251)
CmdSoundStreamCmd = 254
# Existing motor ID's are D0-D9, D12-D14 and D16-D18 there are more limits
LIMITS = [ (30, 180),
(70, 170),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 180),
(0, 160),
(0, 180),
(0, 130),
(0, 180),
(0, 160),
(0, 180),
(50, 130),
(0, 180),
(0, 180),
(0, 180) ]
SERVOS = 23
SERVO_SPEED_SLOWEST = 20
SERVO_SPEED_FASTEST = 0
CmdSoundInitStop = 0
CmdSoundLoad = 1
CmdSoundPlay = 2
DEFAULT_P = [ ( 0, 110 ), # head left - right
( 1, 90 ), # neck up - down
( 6, 90 ), # right hand closed
( 5, 110 ), # right elbow
( 4, 160 ), # right shoulder close - open
( 3, 170 ), # right shoulder rotation
( 9, 90 ), # left hand
( 8, 70 ), # left elbow
( 7, 20 ), # left shoulder close - open
( 2, 10 ), # left shoulder rotation
( 14, 90 ), # right feet
( 13, 80 ), # right knee
( 12, 79 ), # right hip
( 18, 80 ), # left feet
( 17, 80 ), # left knee
( 16, 90 ), # left hip
]
DEFAULT_POSE = [-1] * 23
for servo, position in DEFAULT_P:
DEFAULT_POSE[servo] = position
def __init__(self, ip = ConnectedEndPointAddress, port = ConnectedEndPointPort):
self.ip = ip
self.port = port
self.sock = None
self.isConnected = False
self._currentPose = [-1] * 24
self.connect()
self.init()
def init(self):
self.I2C = I2C(self)
self.rgbEyes = RGBEyes(self)
# preset
self.rgbEyes.setColor(xrange(18), 0, 0, 0)
self.rgbEyes.setColor([0,1,2,9,10,11], 0, 0, 7)
self.rgbEyes.setColor([4,13], 7, 7, 7)
self.setPose(EZB.DEFAULT_POSE)
def connect(self):
try:
self.sock = socket.create_connection((self.ip, self.port), timeout = 10)
self.isConnected = True
except:
raise RuntimeError(EMSG_ROBOT_NOT_FOUND % (self.ip, self.port))
def sendCommand(self, bytesToExpect, cmd, params = None):
# check command
if cmd == EZB.CmdUnknown:
raise ValueError, "unknown command: %s" % cmd
# prepare data
cmd_data = bytearray([cmd])
if params:
cmd_data += bytearray([params]) if not isinstance(params, bytearray) else params
# send data
self.sock.send(cmd_data)
if bytesToExpect > 0:
return self.sock.recv(bytesToExpect)
def __del__(self):
if self.isConnected and self.sock:
self.sock.close()
def setPosition(self, servo, position):
""" This method sets a position of the specified servo.
The joint position values are clipped to the values defined in LIMITS.
@param servo - id of the servo
@param position - absolute value for the given servo position
"""
position = int(position)
# check general limits
if 0 <= servo < EZB.SERVOS and 0 <= position <= 180:
# clip joint limits
low, high = self.LIMITS[servo]
position = min(max(low, position), high)
# send command
self.sendCommand(0, EZB.CmdSetServoPosition + servo, position)
self._currentPose[servo] = position
def setServoSpeed(self, servo, speed):
""" This method sets the speed of the specified servo.
@param servo - id of the servo
@param position - absolute value for the given servo position
"""
speed = int(speed)
# check general limits
if 0 <= servo < EZB.SERVOS and EZB.SERVO_SPEED_FASTEST <= speed <= EZB.SERVO_SPEED_SLOWEST:
self.sendCommand(0, EZB.CmdSetServoSpeed + servo, speed)
def getCurrentPose(self):
""" This method returns a copy of the current pose.
@return list - [ <position>* ] The index in the list is the id of the servo.
"""
return [x for x in self._currentPose]
def setPose(self, pose):
""" This methods sets a full pose to the robot.
@param pose - [ <position>* ] The index in the list is the id of the servo.
"""
for servo, position in enumerate(pose):
self.setPosition(servo, position)
