def goal_position()

in groups/arm/ggd/servo/servode.py [0:0]

• 36 lines of code
• 10 McCabe index (conditional complexity)

    def goal_position(self, goal_positions,
                      block=False,
                      should_run=None,
                      margin=POSITION_MARGIN):
        """

        :param goal_positions: the list of goal position values to write in
            servo order
        :param block: Validate that present_position for each servo is within
            `margin` of the goal position. Block until validation occurs.
        :param should_run: `threading.Event` used to interrupt block if
            necessary, will be cleared when goal position is met.
        :param margin:
        :return:
        """
        log.info("[goal_position] requested positions:{0}".format(
            goal_positions))

        self.write_values('goal_position', goal_positions)

        event = should_run
        if block is True and event is None:
            log.info("[goal_position] using local event")
            event = threading.Event()
            event.clear()
            event.set()

        if block:
            while event.is_set():
                close = dict()
                i = 0
                for servo in self.servos:
                    s = self.servos[servo]
                    pos = s['present_position']
                    goal = goal_positions[i]
                    log.debug(
                        "[goal_position] 'present_position' id:{0} is:{1}".format(
                            s.servo_id, pos))

                    horseshoes = margin + goal
                    hand_grenades = goal - margin
                    if hand_grenades < 0:
                        hand_grenades = 0

                    # we are close enough when servo position is between
                    # horseshoes and hand grenades
                    if horseshoes > pos > hand_grenades:
                        close[servo] = pos
                    i += 1

                if len(close) == len(self.servos):
                    # all servos close when the dict has a value per servo
                    event.clear()

                log.info("[goal_position] actual close positions:{0}".format(
                    close))
                time.sleep(0.5)