+
    Rj                     t    R t ^ RIt^ RIt^ RIt^ RIt^ RIHtHt ^ RIH	t	H
t
 ^(tR t]R8X  d
   ]! 4        R# R# )a  Zero + direction calibration for ONE Feetech servo, done by hand.

Two captures:
  1) Move the servo to its URDF q=0 pose, press Enter -> INST_OFSCAL sets that
     physical position as tick 2048 (the joint zero), persisted in the servo EEPROM.
  2) Move the servo in the URDF-NEGATIVE direction for that joint, press Enter ->
     the tool reads which way the tick moved and records the sign (+1/-1), so later
     `ticks_to_rad(pos, sign)` maps encoder ticks to URDF radians.

Torque is disabled up front so you can move the servo freely. Result is merged into
calib/<config>.json keyed by servo id (run once per servo to build up a whole robot).

Usage:
  python calibrate_servo.py <servo_id> --config gripper [--joint jaw] [--port /dev/tty...]
N)BusCENTER_TICK)CONFIGS
calib_pathc                  h   \         P                  ! 4       p V P                  R \        R7       V P                  RRR7       V P                  RRRR7       V P                  R	RR7       V P	                  4       p\
        P                  ! VP                  / 4      pVP                  ;'       gE    VP                  R
/ 4      P                  VP                  4      ;'       g    VP                  R4      pVf2   \        P                  ! RVP                   RVP                   R24       VP                  '       d   \        VP                  4      M	\        4       p VP                  VP                  4      '       g%   \        P                  ! RVP                   R24       VP                  VP                  R4       \!        RVP                   R24       \#        RVP                   RV R24       VP%                  VP                  4      pVP'                  VP                  4      '       g   \        P                  ! R4       VP)                  VP                  4      p\!        RV RV R\*         R24        \#        RVP                   RV R24       VP%                  VP                  4      pVf   \!        R4       KI  V\*        ,
          p\-        V4      \.        8  d   \!        R VR! R"24       K|   T^ 8  d   ^MR3p	\!        R#T R$TR! R%T	R! 24       \1        TP                  4      p
\2        P4                  ! \2        P6                  P9                  T
4      RR&7       \2        P6                  P;                  T
4      '       d    \<        P>                  ! \A        T
4      4      M/ pR'TR(T	R)\*        R*TR+T/T\C        TP                  4      &   \<        PD                  ! T\A        T
R,4      ^R-7       \!        R.T
 R/TP                   R0T R1T	R! R22	4       TPG                  4        R#   TPG                  4        i ; i)4servo_id)typez--configgripper)defaultz--jointNzBURDF joint this servo drives (default: the config's default_joint))r
   helpz--port	joint_mapdefault_jointzNo --joint given and config 'z' has no mapping for servo .zServo z not responding on the bus.Fu)    torque OFF — you can move it by hand.
z1) Move servo z! to its ZERO pose (URDF q=0 for 'z'), then press Enter... u5   INST_OFSCAL failed — servo did not accept the zero.z   zeroed: was z, now reads z	 (target z).
Tz2) Move servo z in the NEGATIVE 'z4' direction (per the URDF/sim), then press Enter... z   read failed, try again.z   only z+duU    ticks of motion — move it further in the negative direction and press Enter again.z   negative pose tick=z (delta z)  ->  sign = )exist_okjointsign	zero_tickraw_zero_before_calneg_sample_tickw)indentz

Saved -> z	
  servo z	: joint='z'  sign=z  (zero stored in servo EEPROM))$argparseArgumentParseradd_argumentint
parse_argsr   getconfigr   r   sysexitportr   pingtorqueprintinputread_posset_zero_hereread_pos_settler   absMIN_DELTA_TICKSr   osmakedirspathdirnameexistsjsonloadopenstrdumpclose)apacfgr   busrawztdr   r-   datas               calibrate_servo.pymainr@      s^   		 	 	"BOOJSO)OOJ	O2OOIt]  _OOHdO+
A
++ahh
#CGG[[sww{B/33AJJ?[[377?C[E}0
:UVWV`V`Uaabcd#aff+SUC)xx

##HHvajj\)DEF

1::u%qzzl"LMNqzzl*KE7Rjklll1::&  ,,HHLM

+uL9[MNON1::,.@ H) * +QZZ(Ay23KA1v'2 '9 : ;Ur&qc!B~d2YOP!((#
BGGOOD)D9(*t(<(<tyyd$"UFD+{!3(91!
S_ 			$T32D6AJJ<yxPTUWy Y. / 	0 					s    P 8JP P1__main__)__doc__r   r0   r+   r   feetech_busr   r   configsr   r   r*   r@   __name__     r?   <module>rH      s=      	 
 ( '8v zF rG   