
    j,                     R    d Z ddlZddlmZ ddlmZmZmZmZ  G d de          Z	dS )z9Leader-follower teleoperation using YAM teaching handles.    N)TeleopInterface)
PEDAL_LEFTPEDAL_MIDDLEPEDAL_RIGHTtry_open_footpedalc                       e Zd ZdZedefd            Zedefd            ZddZ	ddZ
ddZdd	Zdd
ZdefdZdefdZdefdZedefd            ZdS )YAMInterfacez.Follower arms mirror leader arms in real time.returnc                     dS )Nleader selfs    9/home/robot-lab/cameron/raiden_fork/raiden/control/yam.pynamezYAMInterface.name   s    x    c                     dS )NTr   r   s    r   uses_leaderszYAMInterface.uses_leaders   s    tr   Nc                 r    t          j                     _        t          j                     _        t          j                     _        t                       _         j        Sdt          dd f fd} j                            |            j        	                                 t          d           d S d S )Ncoder
   c                 >   | t           k    rDt          dd           }||                                 d S j                                         d S | t
          k    rj                                         d S | t          k    rj                                         d S d S )N_recording_controller)	r   getattr
soft_pause_pedal_triggersetr   _pedal_successr   _pedal_failure)r   rcr   s     r   _cbzYAMInterface.open.<locals>._cb$   s    :%% '>EEB~+//11111\))'++-----[(('++----- )(r   uH     ✓ FootPedal ready: left=trigger/pause  middle=success  right=failure)	threadingEventr   r   r   r   
_footpedalinton_pressstartprint)r   r    s   ` r   openzYAMInterface.open   s    'o//'o//'o//,..?&
.# 
.$ 
. 
. 
. 
. 
. 
. O$$S)))O!!###Z    ! '&r   c                 l    t          | dd           "| j                                         d | _        d S d S )Nr#   )r   r#   closer   s    r   r*   zYAMInterface.close6   s;    4t,,8O!!###"DOOO 98r   c                     d S Nr   r   robot_controllers     r   setupzYAMInterface.setup?   s    r   c                 .    |                                  d S r,   )start_teleoperationr-   s     r   r&   zYAMInterface.startB   s    ,,.....r   c                 .    |                                  d S r,   )stop_teleoperationr-   s     r   stopzYAMInterface.stopE   s    ++-----r   c                     |                                 dS t          | dd           }|*|                                r|                                 dS dS )NTr   F)check_button_pressr   is_setclear)r   r.   triggers      r   pollzYAMInterface.pollL   sW    ..00<4$ 0$777>>#3#3MMOOO4ur   c                     t          | dd           }|*|                                r|                                 dS dS )Nr   TF)r   r7   r8   r   r.   evs      r   poll_successzYAMInterface.poll_successU   s<    T+T22>biikk>HHJJJ4ur   c                     |                                 rdS t          | dd           }|*|                                r|                                 dS dS )NTr   F)check_failure_buttonr   r7   r8   r<   s      r   poll_failurezYAMInterface.poll_failure\   sU    0022 	4T+T22>biikk>HHJJJ4ur   c                     	 dS )Na[  
============================================================
  BIMANUAL TELEOPERATION ACTIVE
============================================================

  Press the button on any leader arm to return ALL arms to home
  Press Ctrl+C for EMERGENCY STOP (stops all motion immediately)

============================================================
r   r   s    r   bannerzYAMInterface.bannere   s    	
 	
r   )r
   N)__name__
__module____qualname____doc__propertystrr   boolr   r(   r*   r/   r&   r4   r:   r>   rA   rC   r   r   r   r	   r	      sB       88c    X d    X   2# # # #   / / / /. . . .             
 
 
 
 X
 
 
r   r	   )
rG   r!   raiden.control.baser   raiden.robot.footpedalr   r   r   r   r	   r   r   r   <module>rM      s    ? ?     / / / / / /           `
 `
 `
 `
 `
? `
 `
 `
 `
 `
r   