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 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 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)