예제 #1
0
class Application(object):
    """ The application object.
    """

    pose_home = {
        "shoulder": 260,
        "elbow": 15,
        "base": 240,
        "wrist": 55,
        "gripper": 150
    }

    def __init__(self, minitel_port=None,
                 arm_port=None, arm_busname=None,
                 config_file=None, log=None, debug=False):
        self._log = log or DummyLogger()

        self._mt = None
        self._arm = None

        self._debug = debug
        self._log.setLevel(logging.DEBUG if debug else logging.INFO)
        self._log.info('current logging level : %s', logging.getLevelName(self._log.getEffectiveLevel()))
        joints_logger.setLevel(self._log.getEffectiveLevel())

        self._log.info('args:')
        self._log.info('- minitel_port=%s', minitel_port)
        self._log.info('- arm_port=%s', arm_port)
        self._log.info('- arm_busname=%s', arm_busname)
        self._log.info('- config_file=%s', config_file)

        self._demos = [
            (m.__doc__, m) for m in (getattr(self, name)for name in sorted(dir(self)) if name.startswith('demo_'))
        ]

        try:
            cfg = json.load(config_file)
        except ValueError as e:
            raise ValueError('invalid configuration data : %s' % e)

        cfg_minitel = cfg['minitel']
        minitel_port = minitel_port or cfg_minitel['port']
        if os.path.exists(minitel_port):
            self._minitel_port = minitel_port
        else:
            raise ValueError('port not found : %s' % minitel_port)

        self._arm_port = self._arm_busname = None

        self._cfg_arm = cfg["arm"]
        arm_port = arm_port or self._cfg_arm.get("port", None)
        arm_busname = arm_busname or self._cfg_arm.get("busname", None)

        if arm_port:
            if os.path.exists(arm_port):
                self._arm_port = arm_port
                self._arm_baudrate = self._cfg_arm.get("baudrate", 1000000)
            else:
                raise ValueError('port not found : %s' % arm_port)

        elif arm_busname:
            self._arm_busname = arm_busname

        else:
            raise ValueError('at least on of arm_port or arm_busname must be provided')

    def run(self):
        """ Application run mainline
        """
        self._log.info('creating Minitel instance on %s', self._minitel_port)
        self._mt = Minitel(self._minitel_port)  # , debug=self._debug)
        self._mt.clear_all()
        self._mt.display_status(
            self._mt.text_style_sequence(inverse=True) +
            u'Démonstration YouPinitel'.ljust(self._mt.get_screen_width())
        )

        if self._arm_port:
            arm_type = 'AX12' if AX12_ARM else 'Youpi'
            if not AX12_ARM:
                self._arm_baudrate = 9600
            self._log.info('creating %s arm interface instance on %s (baudrate=%d)',
                           arm_type, self._arm_port, self._arm_baudrate)
            if AX12_ARM:
                intf = USB2AX(self._arm_port, baudrate=self._arm_baudrate)
                self._arm = GestureController(intf)
                self._arm.configure_joints(self._cfg_arm['joints'])
            else:
                self._log.info('initializing Youpi interface and arm')
                self._mt.display_text_center("Initialisation du bras", y=5)
                self._mt.display_text_center(u"Veuillez patienter...", y=7)
                intf = YoupiArduinoInterface(self._arm_port)
                intf.wait_for_ready()

                self._mt.clear_screen()

                self._arm = YoupiArmController(intf)
                # self._arm.base.set_goal_angle(150)
                # while math.fabs(self._arm.base.get_current_angle() - 150) >= 1:
                #     time.sleep(0.1)
                self.pose_home = dict(self._arm.get_pose())
                self._log.info('Youpi home pose:')
                for n, a in self.pose_home.iteritems():
                    self._log.info('- %-10s : %5.1f', n, a)

        elif self._arm_busname:
            self._log.info("connecting to arm nROS controller '%s'", self._arm_busname)
            bus = dbus.SessionBus()
            self._arm = bus.get_object(self._arm_busname, DEFAULT_SERVICE_OBJECT_PATH)
            self._arm.joints = dict([
                (n, bus.get_object(self._arm_busname, '/joint/%s' % n)) for n in self._arm.get_joint_names()
            ])

        else:
            raise Exception('no arm controller connection was configured')

        title = 'Menu principal'

        while True:
            self._log.info('displaying main menu')
            menu = Menu(
                self._mt,
                title=[title, '-' * len(title)],
                choices=[t[0] for t in self._demos],
                prompt='Votre choix',
                line_skip=2,
                margin_top=1,
                prompt_line=20,
                addit=[(0, 23, ' SOMMAIRE: fin '.center(40, '-'))]
            )

            choice = menu.get_choice()

            if choice:
                label, method = self._demos[choice - 1]
                self._log.info('selected demo : %s', label)
                method()
            else:
                break

        self._mt.clear_all()
        self._mt.display_text(u"Damien & Eric vous disent à bientôt.")

    def _move_arm_home(self):
        self._log.info('moving arm to its home position...')
        if AX12_ARM:
            gesture = Gesture([(self.pose_home, 2)])
            data = gesture.as_json() if self._arm_busname else gesture
            self._arm.execute_gesture(data)
        else:
            # for j, a in self.pose_home.iteritems():
            #     self._log.info("- %-10s -> %5.1f", j, a)
            #     self._arm.joints[j].set_goal_angle(a)
            self._arm.reset()

    def demo_00_infos(self):
        u"""quelques explications"""

        self._mt.clear_screen()
        for l, text in enumerate([
            u'La rencontre des années 80:',
            u'',
            u"Youpi: ",
            u"   Un bras robotique pour l'enseignement",
            "",
            u"Le Minitel: ",
            u"   L'ancêtre d'Internet",
            u'',
            u'... et du 21ème siècle:',
            u'',
            u"L'Arduino: ",
            u"   Une carte pour l'initiation à la ",
            u"   programmation des micro-contrôleurs",
            '',
            u"La RaspberryPi: ",
            u"   Un ordinateur de la taille d'une ",
            u"   carte de crédit"
        ]):
            self._mt.display_text(text=text, x=0, y=l + 2)

        self._mt.display_text_center(u'Retour : menu principal', y=23)
        self._mt.wait_for_key([constants.SEP + constants.KeyCode.RETOUR], max_wait=300)

    def _demo_01(self):
        u"""dis bonjour avec le bras"""
        self._mt.display_text_center(' je vous dis bonjour ', 23, pad_char='-')
        pose_extended = {
            "shoulder": 170,    # 82,
            "elbow": 60,        # 73,
            "base": 240,
            "wrist": 202,
            "gripper": 150
        }
        pose_to_the_right = {
            "base": 200
        }
        pose_to_the_left = {
            "base": 280
        }

        gesture = Gesture([
            (pose_extended, 1),
            (None, 0.5),
            (pose_to_the_right, 0.5),
            (pose_to_the_left, 1),
            (pose_extended, 0.5),
            (None, 1),
            (self.pose_home, 2)
        ])
        data = gesture.as_json() if self._arm_busname else gesture
        self._arm.execute_gesture(data)

    def _demo_02(self):
        u"""déplace le Rubik's cube"""
        self._mt.display_text_center(' moving the cube ', 23, pad_char='-')
        time.sleep(3)

    def demo_03(self):
        u"""contrôle manuel"""

        base = self._arm.joints['base']
        shoulder = self._arm.joints['shoulder']
        elbow = self._arm.joints['elbow']
        wrist = self._arm.joints['wrist']
        gripper = self._arm.joints['gripper']
        wrist_rot = None if AX12_ARM else self._arm.joints['wrist_rot']

        speed = 10 if AX12_ARM else None

        def _move_joint(joint, step):
            goal_pos = joint.get_current_angle() + step
            self._log.info('moving joint %s from angle %5.1f to angle %5.1f', joint.servo_id, joint.get_current_angle(), goal_pos)
            joint.set_goal_angle(goal_pos, speed, immediate=True, wait=False)

        def shoulder_up():
            _move_joint(shoulder, 5)

        def shoulder_down():
            _move_joint(shoulder, -5)

        def elbow_up():
            _move_joint(elbow, +5 * (1 if AX12_ARM else -1))

        def elbow_down():
            _move_joint(elbow, -5 * (1 if AX12_ARM else -1))

        def wrist_up():
            _move_joint(wrist, +5)

        def wrist_down():
            _move_joint(wrist, -5)

        def base_left():
            _move_joint(base, 5)

        def base_right():
            _move_joint(base, -5)

        def gripper_open():
            if hasattr(self._arm, 'open_gripper'):
                self._arm.open_gripper()
            else:
                gripper.set_goal_angle(100, 0, immediate=True, wait=False)

        def gripper_close():
            if hasattr(self._arm, 'close_gripper'):
                self._arm.close_gripper()
            else:
                gripper.set_goal_angle(150, 0, immediate=True, wait=False)

        def wrist_ccw():
            _move_joint(wrist_rot, -5)

        def wrist_cw():
            _move_joint(wrist_rot, 5)

        actions = {
            '1': shoulder_up,
            '4': shoulder_down,
            '2': elbow_up,
            '5': elbow_down,
            '3': wrist_up,
            '6': wrist_down,
            '7': base_left,
            '9': base_right,
            'O': gripper_open,
            'F': gripper_close,
            '*': wrist_ccw,
            '#': wrist_cw,
            'R': self._move_arm_home,
        }

        self._mt.clear_screen()
        for l, text in enumerate([
            u'Utilisez les touches du clavier',
            u'pour contrôler le bras.',
            '',
            u'1 4 : épaule',
            u'2 5 : coude',
            u'3 6 : poignet',
            u'7 9 : rotation bras',
            u'* # : rotation pince',
            u'O F : ouverture/fermeture pince',
            '',
            u' R  : retour position initiale'
        ]):
            self._mt.display_text(text=text, x=0, y=l + 4)

        self._mt.display_text_center(u'Retour : menu principal', y=23)

        key_return = constants.SEP + constants.KeyCode.RETOUR
        valid_keys = actions.keys() + [key_return]
        while True:
            key = self._mt.wait_for_key(valid_keys, max_wait=60)
            if key in (None, key_return):
                if not key:
                    self._log.info('no input => return to main menu')
                self._mt.clear_screen()
                self._mt.display_text_center(u"Réinitialisation du bras", y=5)
                self._mt.display_text_center(u"Veuillez patienter...", y=7)
                self._move_arm_home()
                return
            else:
                try:
                    actions[key]()
                except KeyError:
                    self._mt.beep()
예제 #2
0
    def run(self):
        """ Application run mainline
        """
        self._log.info('creating Minitel instance on %s', self._minitel_port)
        self._mt = Minitel(self._minitel_port)  # , debug=self._debug)
        self._mt.clear_all()
        self._mt.display_status(
            self._mt.text_style_sequence(inverse=True) +
            u'Démonstration YouPinitel'.ljust(self._mt.get_screen_width())
        )

        if self._arm_port:
            arm_type = 'AX12' if AX12_ARM else 'Youpi'
            if not AX12_ARM:
                self._arm_baudrate = 9600
            self._log.info('creating %s arm interface instance on %s (baudrate=%d)',
                           arm_type, self._arm_port, self._arm_baudrate)
            if AX12_ARM:
                intf = USB2AX(self._arm_port, baudrate=self._arm_baudrate)
                self._arm = GestureController(intf)
                self._arm.configure_joints(self._cfg_arm['joints'])
            else:
                self._log.info('initializing Youpi interface and arm')
                self._mt.display_text_center("Initialisation du bras", y=5)
                self._mt.display_text_center(u"Veuillez patienter...", y=7)
                intf = YoupiArduinoInterface(self._arm_port)
                intf.wait_for_ready()

                self._mt.clear_screen()

                self._arm = YoupiArmController(intf)
                # self._arm.base.set_goal_angle(150)
                # while math.fabs(self._arm.base.get_current_angle() - 150) >= 1:
                #     time.sleep(0.1)
                self.pose_home = dict(self._arm.get_pose())
                self._log.info('Youpi home pose:')
                for n, a in self.pose_home.iteritems():
                    self._log.info('- %-10s : %5.1f', n, a)

        elif self._arm_busname:
            self._log.info("connecting to arm nROS controller '%s'", self._arm_busname)
            bus = dbus.SessionBus()
            self._arm = bus.get_object(self._arm_busname, DEFAULT_SERVICE_OBJECT_PATH)
            self._arm.joints = dict([
                (n, bus.get_object(self._arm_busname, '/joint/%s' % n)) for n in self._arm.get_joint_names()
            ])

        else:
            raise Exception('no arm controller connection was configured')

        title = 'Menu principal'

        while True:
            self._log.info('displaying main menu')
            menu = Menu(
                self._mt,
                title=[title, '-' * len(title)],
                choices=[t[0] for t in self._demos],
                prompt='Votre choix',
                line_skip=2,
                margin_top=1,
                prompt_line=20,
                addit=[(0, 23, ' SOMMAIRE: fin '.center(40, '-'))]
            )

            choice = menu.get_choice()

            if choice:
                label, method = self._demos[choice - 1]
                self._log.info('selected demo : %s', label)
                method()
            else:
                break

        self._mt.clear_all()
        self._mt.display_text(u"Damien & Eric vous disent à bientôt.")