def msg_angle(self, angle=None): """Builds a message given the target angle.""" msg = servo_pololu() msg.id = self.motor_entry['motorid'] msg.angle = self._saturatedAngle(angle or self.target) msg.speed = self.motor_entry['speed'] msg.acceleration = self.motor_entry['acceleration'] return msg
def create_cmd(confentry, angle, speed=None):
"""Argument 'val' takes values in rad."""
cmd = servo_pololu()
cmd.id = confentry['motorid']
cmd.angle = angle
cmd.speed = speed or confentry['speed']
cmd.acceleration = confentry['acceleration']
return cmd
def create_cmd(confentry, angle, speed=None): """Argument 'val' takes values in rad.""" cmd = servo_pololu() cmd.id = confentry['motorid'] cmd.angle = angle cmd.speed = speed or confentry['speed'] cmd.acceleration = confentry['acceleration'] return cmd
def handle_joint_angles(self, msg):
rospy.logdebug("Received a joint angle target")
for i, joint_name in enumerate(msg.name):
servo_msg = servo_pololu()
servo_msg.id = self.joint_ids[joint_name]
servo_msg.angle = msg.position[i]
servo_msg.speed = (msg.velocity * 255.0)
servo_msg.acceleration = msg.effort #TODO: check this
self.pololu_pub.publish(servo_msg)
def position_pololu(self, config, angle):
msg = servo_pololu()
msg.id = int(config["motorid"])
msg.angle = float(angle * float(config["scale"]) + float(config["translate"]))
msg.speed = int(config["speed"])
msg.acceleration = int(config["acceleration"])
pub = self.pololus[config["name"]]
if config["enabled"]:
#print("POLOLU: %s @ %s" % (config["name"], angle))
pub.publish(msg)
def handle_joint_angles(self, msg):
rospy.logdebug("Received a joint angle target")
for i, joint_name in enumerate(msg.name):
servo_msg = servo_pololu()
servo_msg.id = self.joint_ids[joint_name]
servo_msg.angle = msg.position[i]
servo_msg.speed = (msg.velocity * 255.0)
servo_msg.acceleration = msg.effort #TODO: check this
self.pololu_pub.publish(servo_msg)
def msg_angle(self, angle=None):
"""Builds a message given the target angle."""
msg = servo_pololu()
msg.id = self.motor_entry['motorid']
msg.angle = self._saturatedAngle(angle or self.target)
# Default values are set here, if speed or acceleration are not found in
# the motor entry.
msg.speed = self.motor_entry.get('speed', 200)
msg.acceleration = self.motor_entry.get('acceleration', 10)
return msg
def build_cmd(self, incoming_msg):
"""Builds a motor command given a PAU message."""
coeff = self.parser.get_coeff(incoming_msg)
angle = self.mapper.map(coeff)
rospy.logdebug("Motor: %s, coeff: %s, angle: %s",
self.motor_entry['name'], coeff, angle
)
msg = servo_pololu()
msg.id = self.motor_entry['motorid']
msg.angle = self._saturated(angle)
msg.speed = self.motor_entry['speed']
msg.acceleration = self.motor_entry['acceleration']
return msg
def send(self, angle):
if not self._pub:
self._pub = rospy.Publisher(self._topic, servo_pololu,
queue_size=10)
self._pub.publish(servo_pololu(self._motorid, angle,
self.SPEED, self.ACCELERATION))
def load_handler(dummy):
global leye, reye, pololu, doeyes, domotors, jaw, neck0, neck1, neck2, neck3, jawdm, neck0dm, neck1dm, neck2dm, neck3dm
newstuff = False
newleye = get_bones_rotation_rad('leye','leyebone','z')
if leye != newleye:
leye = newleye
eyedeg = get_bones_rotation('leye','leyebone','z')
if doeyes:
msg = servo_pololu()
msg.id = 0
msg.angle = newleye
msg.speed = 0
msg.acceleration = 0
pololu.publish(msg)
print("LEYE: %s" % msg)
newreye = get_bones_rotation_rad('reye','reyebone','z')
if reye != newreye:
reye = newreye
eyedeg = get_bones_rotation('reye','reyebone','z')
if doeyes:
msg = servo_pololu()
msg.id = 1
msg.angle = newreye
msg.speed = 0
msg.acceleration = 0
pololu.publish(msg)
print("REYE: %s" % msg)
newjaw = get_bones_rotation_rad('jawarm','jawbone','x')
if jaw != newjaw:
jaw = newjaw
jawdeg = get_bones_rotation('jawarm','jawbone','x')
if domotors: jawdm.publish(float(jaw))
print("JAW: %s (%s)" % (jaw, jawdeg))
newneck0 = get_bones_rotation_rad('Armature','base','y') + 2.2
if neck0 != newneck0:
neck0 = newneck0
neck0deg = get_bones_rotation('Armature','base','y')
if domotors: neck0dm.publish(float(newneck0))
print("BASE: %s (%s)" % (neck0, neck0deg))
newneck1 = (get_bones_rotation_rad('Armature','bracket1','x') * -2) + 2.5
if neck1 != newneck1:
neck1 = newneck1
if domotors: neck1dm.publish(float(newneck1))
print("NECK1: %s" % neck1)
newneck2 = (get_bones_rotation_rad('Armature','bracket2','z') * 2) + 2
if neck2 != newneck2:
neck2 = newneck2
if domotors: neck2dm.publish(float(newneck2))
print("NECK2: %s" % neck2)
newneck3 = (get_bones_rotation_rad('Armature','bracket3','x') * 2) + 3
if neck3 != newneck3:
neck3 = newneck3
if domotors: neck3dm.publish(float(newneck3))
print("NECK3: %s" % neck3)
def load_handler(dummy):
global leye, reye, pololu, doeyes, domotors, jaw, neck0, neck1, neck2, neck3, jawdm, neck0dm, neck1dm, neck2dm, neck3dm
newstuff = False
newleye = get_bones_rotation_rad('leye', 'leyebone', 'z')
if leye != newleye:
leye = newleye
eyedeg = get_bones_rotation('leye', 'leyebone', 'z')
if doeyes:
msg = servo_pololu()
msg.id = 0
msg.angle = newleye
msg.speed = 0
msg.acceleration = 0
pololu.publish(msg)
print("LEYE: %s" % msg)
newreye = get_bones_rotation_rad('reye', 'reyebone', 'z')
if reye != newreye:
reye = newreye
eyedeg = get_bones_rotation('reye', 'reyebone', 'z')
if doeyes:
msg = servo_pololu()
msg.id = 1
msg.angle = newreye
msg.speed = 0
msg.acceleration = 0
pololu.publish(msg)
print("REYE: %s" % msg)
newjaw = get_bones_rotation_rad('jawarm', 'jawbone', 'x')
if jaw != newjaw:
jaw = newjaw
jawdeg = get_bones_rotation('jawarm', 'jawbone', 'x')
if domotors: jawdm.publish(float(jaw))
print("JAW: %s (%s)" % (jaw, jawdeg))
newneck0 = get_bones_rotation_rad('Armature', 'base', 'y') + 2.2
if neck0 != newneck0:
neck0 = newneck0
neck0deg = get_bones_rotation('Armature', 'base', 'y')
if domotors: neck0dm.publish(float(newneck0))
print("BASE: %s (%s)" % (neck0, neck0deg))
newneck1 = (get_bones_rotation_rad('Armature', 'bracket1', 'x') * -2) + 2.5
if neck1 != newneck1:
neck1 = newneck1
if domotors: neck1dm.publish(float(newneck1))
print("NECK1: %s" % neck1)
newneck2 = (get_bones_rotation_rad('Armature', 'bracket2', 'z') * 2) + 2
if neck2 != newneck2:
neck2 = newneck2
if domotors: neck2dm.publish(float(newneck2))
print("NECK2: %s" % neck2)
newneck3 = (get_bones_rotation_rad('Armature', 'bracket3', 'x') * 2) + 3
if neck3 != newneck3:
neck3 = newneck3
if domotors: neck3dm.publish(float(newneck3))
print("NECK3: %s" % neck3)
