def stage_sort()

in groups/arm/ggd/stages.py [0:0]
45 lines of code
2 McCabe index (conditional complexity)

    def stage_sort(self, should_run=None):
        """
        The arm has grabbed an object and will place that object at the sort
        position.

        :param should_run: a `threading.Event` that tells the stage to continue
            if `is_set()` is True
        :return: a dict containing this stage's results
        """
        log.info("[stage_sort] _begin_")
        stage_results = dict()

        sort_base = 512
        sort_femur_1 = 280
        sort_femur_2 = 280
        sort_tibia = 675

        # ensure there are the expected number of servos in the ServoGroup
        if len(self.sg) is not 5:
            log.error("[stage_sort] Unexpected sg length:{0}".format(
                len(self.sg)))
            return stage_results

        # slow down the move speed of all servos to reduce dropped objects
        ######################################################
        self.sg.write_values("moving_speed", [150, 150, 150, 150, 150])
        stage_results['slow_down'] = True

        # go to SORT "high" location
        ######################################################
        self.sg.goal_position([
            sort_base,  # first servo value
            HOME_FEMUR_1,  # second servo value
            HOME_FEMUR_2,  # third servo value
            sort_tibia,  # fourth servo value
            GRAB_EFFECTOR  # fifth servo value
        ], block=False, margin=POSITION_MARGIN)
        time.sleep(0.5)
        stage_results['raise_complete'] = True

        # go to SORT "extended" location
        ######################################################
        self.sg.goal_position([
            sort_base,  # first servo value
            sort_femur_1,  # second servo value
            sort_femur_2,  # third servo value
            sort_tibia,  # fourth servo value
            GRAB_EFFECTOR  # fifth servo value
        ], block=True, margin=POSITION_MARGIN + 15)
        time.sleep(0.5)
        stage_results['reach_complete'] = True

        # open the end effector/claw to drop object
        ######################################################
        self.sg['effector']['goal_position'] = OPEN_EFFECTOR
        time.sleep(0.5)

        # go to SORT "away" location
        ######################################################
        self.sg.goal_position([
            sort_base,  # first servo value
            sort_femur_1 + 150,  # second servo value
            sort_femur_2 + 150,  # third servo value
            HOME_TIBIA,  # fourth servo value
            OPEN_EFFECTOR  # fifth servo value
        ], block=False, margin=POSITION_MARGIN)

        # increase move speed of all servos to normal
        ######################################################
        self.sg.write_values("moving_speed", [200, 200, 200, 200, 200])
        stage_results['slow_down'] = False

        # the sort stage is now complete
        stage_results['sort_complete'] = True
        log.info("[stage_sort] _end_")
        return stage_results