§
     ïÞf:R  ã                   ó„   — d dl Z d dlZd dlZd dlZd dlZd dlT d dlT d dlZd dl	T d dl
mZ  G d„ d¦  «        Zedk    rdS dS )é    N)Ú*)ÚCOMMANDc                   óô   — e Zd Zd„ Zd„ Zd„ Zd"d„Zd"d„Zd„ Zd	„ Z	d
„ Z
d„ Zd„ Zd„ Zdddddd ej         ej        d¦  «        ¦  «        fd„Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd#d„Zd„ Zd„ Zd„ Zd„ Zd$d „Zd!S )%ÚControlc                 óD  — t          ¦   «         | _        t          ¦   «         | _        t	          ddd¦  «        | _        d| _        d| _        d| _        d| _	        d| _
        d| _        g d¢| _        g d¢g d¢g d	¢g d	¢g| _        |                      d
¦  «        | _        g d¢g d¢g d¢g d¢g| _        g d¢g d¢g d¢g d¢g| _        d| _        d| _        d| _        t+          j        | j        ¬¦  «        | _        |                      ¦   «          |                      d¦  «         d S )Ng      à?ç        g{®Gázd?é   éc   r   ©Ú r   r   r   r   )r   r
   é
   )r   r
   éöÿÿÿÚpoint)éZ   r   r   )r   r   r   TF©Útarget)ÚIMUÚimuÚServoÚservoÚIncremental_PIDÚpidÚspeedÚheightÚtimeoutÚ	move_flagÚ
move_countÚmove_timeoutÚorderr   ÚreadFromTxtÚcalibration_pointÚangleÚcalibration_angleÚ
relax_flagÚbalance_flagÚattitude_flagÚ	threadingÚThreadÚ	conditionÚThread_conditionaÚcalibrationÚrelax)Úselfs    ú2/home/digit/Desktop/robocat/Code/Server/Control.pyÚ__init__zControl.__init__   s3  € Ý‘”ˆŒÝ‘7”7ˆŒ
Ý" 3 s¨6Ñ2Ô2ˆŒØˆŒ
ØˆŒØˆŒØˆŒØˆŒØˆÔØ%Ð%Ð%ˆŒ
Ø!�k�k ; ; ;°°°¸l¸l¸lÐKˆŒ
Ø!%×!1Ò!1°'Ñ!:Ô!:ˆÔØ�h�h˜x˜x˜x¨¨¨°°°Ð:ˆŒ
Ø '  ¨¨¨°°°¸¸¸Ð@ˆÔØˆŒØˆÔØ ˆÔÝ(Ô/°t´~ÐFÑFÔFˆÔØ×ÒÑÔÐØ�
Š
�4ÑÔÐÐÐó    c                 ó  — t          |dz   d¦  «        }|                     ¦   «         }g }t          t          |¦  «        ¦  «        D ]D}||                              ¦   «                              d¦  «        }|                     |¦  «         ŒEt          t          |¦  «        ¦  «        D ]K}t          t          ||         ¦  «        ¦  «        D ]&}t          ||         |         ¦  «        ||         |<   Œ'ŒL|                     ¦   «          |S )Nú.txtÚrú	)	ÚopenÚ	readlinesÚrangeÚlenÚstripÚsplitÚappendÚintÚclose)r-   ÚfilenameÚfile1Úlist_rowÚlist_sourceÚiÚcolumn_listÚjs           r.   r    zControl.readFromTxt#   sý   € Ý�X Ñ&¨Ñ,Ô,ˆØ—?’?Ñ$Ô$ˆØˆÝ•s˜8‘}”}Ñ%Ô%ð 	,ð 	,ˆAØ" 1œ+×+Ò+Ñ-Ô-×3Ò3°DÑ9Ô9ˆKØ×Ò˜{Ñ+Ô+Ð+Ð+Ý•s˜;Ñ'Ô'Ñ(Ô(ð 	;ð 	;ˆAÝ�3˜{¨1œ~Ñ.Ô.Ñ/Ô/ð ;ð ;�Ý$'¨°A¬°qÔ(9Ñ$:Ô$:�˜A”˜qÑ!Ð!ð;à�Š‰ŒˆØÐr0   c                 óŒ  — t          |dz   d¦  «        }t          t          |¦  «        ¦  «        D ]}t          t          ||         ¦  «        ¦  «        D ]E}|                     t	          ||         |         ¦  «        ¦  «         |                     d¦  «         ŒF|                     d¦  «         Œ€|                     ¦   «          d S )Nr2   Úwr4   ú
)r5   r7   r8   ÚwriteÚstrr=   )r-   Úlistr>   Úfile2rB   rD   s         r.   Ú	saveToTxtzControl.saveToTxt0   s¯   € Ý�X Ñ&¨Ñ,Ô,ˆÝ•s˜4‘y”yÑ!Ô!ð 	ð 	ˆAÝ�3˜t Aœw™<œ<Ñ(Ô(ð "ð "�Ø—’�C  Q¤¨¤
™OœOÑ,Ô,Ð,Ø—’˜DÑ!Ô!Ð!Ð!Ø�KŠK˜ÑÔÐÐØ�Š‰Œˆˆˆr0   é   é7   c                 ó0  — t           j        dz  t          j        ||¦  «        z
  }d}|t          j        |¦  «        z  }	|t          j        |¦  «        z  }
t          j        ||
z
  dz  ||	z
  dz  z   ||z
  dz  z   ¦  «        }||z
  |z  }||z  ||z  z   ||z  z
  d|z  |z  z  }t          j        t          |d¦  «        ¦  «        t          j        t          |d¦  «        ¦  «        z
  }t           j        t          j        t          |dz  |dz  z   |dz  z
  d|z  |z  z  d¦  «        ¦  «        z
  }t          t          j	        |¦  «        ¦  «        }t          t          j	        |¦  «        ¦  «        }t          t          j	        |¦  «        ¦  «        }|||fS )Né   r   )
ÚmathÚpiÚatan2ÚsinÚcosÚsqrtÚasinÚroundÚacosÚdegrees)r-   ÚxÚyÚzÚl1Úl2Úl3ÚaÚx_3Úx_4Úx_5Úl23rF   ÚvÚbÚcs                   r.   ÚcoordinateToAnglezControl.coordinateToAngle9   sb  € Ý
Œ'�!‰)•D”J˜q ‘O”OÑ
#ˆØˆØ�tŒx˜‰{Œ{‰NˆØ�tŒx˜‰{Œ{‰NˆÝŒI�q˜‘u˜q‘j ! C¡%¨!¡Ñ+¨Q¨s©U°Q©JÑ6Ñ7Ô7ˆØˆS‰5�#‰+ˆØˆb‰5��S‘‰=˜˜B™Ñ  2¡ c¡Ñ
*ˆÝ
Œ)•E˜!˜A‘J”JÑ
Ô
¥¤	­%°°!©*¬*Ñ 5Ô 5Ñ
5ˆÝ
Œ'•$”)�E 2 q¡5¨¨Q©¡;¨s°A©vÑ#5¸¸"¹¸R¹Ñ"@ÀÑCÔCÑDÔDÑ
DˆÝ
•”˜Q‘”Ñ
 Ô
 ˆÝ
•”˜Q‘”Ñ
 Ô
 ˆÝ
•”˜Q‘”Ñ
 Ô
 ˆØ��1ˆuˆr0   c                 ó¬  — t           j        dz  |z  }t           j        dz  |z  }t           j        dz  |z  }|t          j        ||z   ¦  «        z  |t          j        |¦  «        z  z   }|t          j        |¦  «        z  t          j        ||z   ¦  «        z  |t          j        |¦  «        z  t          j        |¦  «        z  z   |t          j        |¦  «        z  z   }|t          j        |¦  «        z  t          j        ||z   ¦  «        z  |t          j        |¦  «        z  t          j        |¦  «        z  z   |t          j        |¦  «        z  z   }	|||	fS )Né´   )rQ   rR   rT   rU   )
r-   ra   rg   rh   r^   r_   r`   r[   r\   r]   s
             r.   ÚangleToCoordinatezControl.angleToCoordinateH   s  € Ý
Œ'�#‰+�a‰-ˆÝ
Œ'�#‰+�a‰-ˆÝ
Œ'�#‰+�a‰-ˆØ
�TŒX�a˜‘c‰]Œ]Ñ
˜2�dœh q™kœk™>Ñ
)ˆØ
�TŒX�a‰[Œ[‰.�œ ! A¡#™œÑ
& r­$¬(°1©+¬+¡~µd´h¸q±k´kÑ'AÑ
AÀ"ÅTÄXÈaÁ[Ä[Á.Ñ
PˆØ
�TŒX�a‰[Œ[‰.�œ ! A¡#™œÑ
& r­$¬(°1©+¬+¡~µd´h¸q±k´kÑ'AÑ
AÀ"ÅTÄXÈaÁ[Ä[Á.Ñ
PˆØ��1ˆuˆr0   c                 ó�  — t          d¦  «        D ]z}|                      | j        |         d         | j        |         d         | j        |         d         ¦  «        \  | j        |         d<   | j        |         d<   | j        |         d<   Œ{t          d¦  «        D ]z}|                      | j        |         d         | j        |         d         | j        |         d         ¦  «        \  | j        |         d<   | j        |         d<   | j        |         d<   Œ{t          d¦  «        D ]¡}| j        |         d         | j        |         d         z
  | j        |         d<   | j        |         d         | j        |         d         z
  | j        |         d<   | j        |         d         | j        |         d         z
  | j        |         d<   Œ¢d S )Né   r   é   rP   )r7   ri   r!   r#   r   r"   )r-   rB   s     r.   r+   zControl.calibrationQ   s%  € Ý�q‘”ð 	Xð 	XˆAØcg×cyÒcyÐz~ô  {Qð  RSô  {Tð  UVô  {WØz~ô  {Qð  RSô  {Tð  UVô  {WØz~ô  {Qð  RSô  {Tð  UVô  {WñdXô dXÑbˆDÔ" 1Ô% aÑ(¨Ô)?ÀÔ)BÀ1Ñ)EÀdÔF\Ð]^ÔF_Ð`aÑFbÐFbõ �q‘”ð 	hð 	hˆAØ?C×?UÒ?UÐVZÔV`ÐabÔVcÐdeÔVfØVZÔV`ÐabÔVcÐdeÔVfØVZÔV`ÐabÔVcÐdeÔVfñ@hô @hÑ>ˆDŒJ�qŒM˜!Ñ˜TœZ¨œ]¨1Ñ-¨d¬j¸¬m¸AÑ.>Ð.>õ �q‘”ð 	Wð 	WˆAØ)-Ô)?ÀÔ)BÀ1Ô)EÀdÄjÐQRÄmÐTUÔFVÑ)VˆDÔ" 1Ô% aÑ(Ø)-Ô)?ÀÔ)BÀ1Ô)EÀdÄjÐQRÄmÐTUÔFVÑ)VˆDÔ" 1Ô% aÑ(Ø)-Ô)?ÀÔ)BÀ1Ô)EÀdÄjÐQRÄmÐTUÔFVÑ)VˆDÔ" 1Ô% aÑ(Ð(ð	Wð 	Wr0   c                 ó  — |                       ¦   «         �râ	 t          d¦  «        D ]z}|                      | j        |         d         | j        |         d         | j        |         d         ¦  «        \  | j        |         d<   | j        |         d<   | j        |         d<   Œ{t          d¦  «        D �]ã}|                      | j        |         d         | j        |         d         z   dd¦  «        | j        |         d<   |                      d| j        |         d         | j        |         d         z   z
  dd¦  «        | j        |         d<   |                      | j        |         d         | j        |         d         z   dd¦  «        | j        |         d<   |                      | j        |dz            d         | j        |dz            d         z   dd¦  «        | j        |dz            d<   |                      d| j        |dz            d         z   | j        |dz            d         z   dd¦  «        | j        |dz            d<   |                      d| j        |dz            d         | j        |dz            d         z   z
  dd¦  «        | j        |dz            d<   �Œåt          d¦  «        D �]8}| j                             d|dz  z   | j        |         d         ¦  «         | j                             d|dz  z   | j        |         d         ¦  «         | j                             d|dz  z   | j        |         d         ¦  «         | j                             d|dz  z   | j        |dz            d         ¦  «         | j                             d	|dz  z   | j        |dz            d         ¦  «         | j                             d
|dz  z   | j        |dz            d         ¦  «         �Œ:d S # t          $ r}Y d }~d S d }~ww xY wt          d¦  «         d S )Nrn   r   ro   rP   rk   r   é   r	   é	   r   z0This coordinate point is out of the active range)Ú
checkPointr7   ri   r   r"   Úrestrictionr#   r   ÚsetServoAngleÚ	ExceptionÚprint)r-   rB   Úes      r.   ÚrunzControl.run^   s  € Ø�?Š?ÑÔñ 	FðÝ˜q™œð pð p�AØGK×G]ÒG]Ð^bÔ^hÐijÔ^kÐlmÔ^nØ^bÔ^hÐijÔ^kÐlmÔ^nØ^bÔ^hÐijÔ^kÐlmÔ^nñHpô HpÑF�D”J˜q”M !Ñ$ T¤Z°¤]°1Ñ%5°d´jÀ´mÀAÑ6FÐ6Fõ ˜q™œð wñ w�AØ%)×%5Ò%5°d´jÀ´mÀAÔ6FÀtÔG]Ð^_ÔG`ÐabÔGcÑ6cÐdeÐfiÑ%jÔ%j�D”J˜q”M !Ñ$Ø%)×%5Ò%5°b¸$¼*ÀQ¼-ÈÔ:JÈ4ÔKaÐbcÔKdÐefÔKgÑ:gÑ6hÐijÐknÑ%oÔ%o�D”J˜q”M !Ñ$Ø%)×%5Ò%5°d´jÀ´mÀAÔ6FÀtÔG]Ð^_ÔG`ÐabÔGcÑ6cÐdeÐfiÑ%jÔ%j�D”J˜q”M !Ñ$Ø'+×'7Ò'7¸¼
À1ÀQÁ3¼ÈÔ8JÈ4ÔKaÐbcÐdeÑbeÔKfÐghÔKiÑ8iÐjkÐloÑ'pÔ'p�D”J˜q ™s”O AÑ&Ø'+×'7Ò'7¸¸4¼:ÀaÈÁc¼?È1Ô;MÑ8MÈdÔNdÐefÐghÑehÔNiÐjkÔNlÑ8lÐmnÐorÑ'sÔ's�D”J˜q ™s”O AÑ&Ø'+×'7Ò'7¸¸T¼ZÈÈ!É¼_ÈQÔ=OÐPTÔPfÐghÐijÑgjÔPkÐlmÔPnÑ=nÑ8oÐpqÐruÑ'vÔ'v�D”J˜q ™s”O AÑ&Ñ&Ý˜q™œð Hñ H�AØ”J×,Ò,¨Q¨q°©s©U°4´:¸a´=ÀÔ3CÑDÔDÐDØ”J×,Ò,¨Q¨q°©s©U°4´:¸a´=ÀÔ3CÑDÔDÐDØ”J×,Ò,¨Q¨q°©s©U°4´:¸a´=ÀÔ3CÑDÔDÐDØ”J×,Ò,¨Q¨q°©s©U°4´:¸aÀ¹c´?À1Ô3EÑFÔFÐFØ”J×,Ò,¨Q¨q°©s©U°4´:¸aÀ¹c´?À1Ô3EÑFÔFÐFØ”J×,Ò,¨R°°!±©V°D´J¸qÀ¹s´OÀAÔ4FÑGÔGÐGÑGðHð Høõ ð ð ð Ø�����øøøøðøøøõ ÐDÑEÔEÐEÐEÐEs   —OO  Ï 
O5Ï0O5c                 ó@  — d}g d¢}t          d¦  «        D ][}t          j        | j        |         d         dz  | j        |         d         dz  z   | j        |         d         dz  z   ¦  «        ||<   Œ\t          d¦  «        D ]}||         dk    s||         dk     rd	}Œ|S )
NT)r   r   r   r   r   r   rn   r   rP   ro   é‚   é   F)r7   rQ   rV   r   )r-   ÚflagÚ
leg_lenghtrB   s       r.   rs   zControl.checkPointw   s¸   € ØˆØ �=�=ˆ
Ý�q‘”ð 	_ð 	_ˆAÝœ	 $¤*¨Q¤-°Ô"2°AÑ"5°d´jÀ´mÀAÔ6FÈÑ6IÑ"IÈ$Ì*ÐUVÌ-ÐXYÔJZÐ\]ÑJ]Ñ"]Ñ^Ô^ˆ*�Q‰-ˆ-Ý�qÑ"Ô"ð 	ð 	ˆAØ˜Œ]˜SÒ Ð  J¨q¤M°BÒ$6Ð$6ØˆDøØˆr0   c                 óî  — 	 	 t          j         ¦   «         | j        z
  dk    r5| j        dk    r*| j        dk    rd| _        t          j         ¦   «         | _        | j        dk     �r�t          j         ¦   «         | j        z
  dk    rd| j        dk    rY| j        dk    rN| j        d         dk    r=t          j         ¦   «         | _        d| _        |                      d¦  «         g d¢| _        | j        dk    rH| j        d         dk    r7| j        d         t          j        k    r|                      d¦  «         d| _        | j	        dk    rG| j        d         t          j
        k    r,| j        d         dk    r|                      ¦   «          d| _	        | j        dk    rA| xj        t          j         ¦   «         | j        z
  z  c_        t          j         ¦   «         | _        | j        d         t          j        k    rg d¢| _        |                      ¦   «          �n=| j        d         t          j        k    r5t          | j        d	         ¦  «        | _        |                      ¦   «          �ní| j        d         t          j        k    r5t          | j        d	         ¦  «        | _        |                      ¦   «          �n�| j        d         t          j        k    r5t          | j        d	         ¦  «        | _        |                      ¦   «          �nM| j        d         t          j        k    r5t          | j        d	         ¦  «        | _        |                      ¦   «          �ný| j        d         t          j        k    r5t          | j        d	         ¦  «        | _        |                      ¦   «          �n­| j        d         t          j        k    r5t          | j        d	         ¦  «        | _        |                      ¦   «          �n]| j        d         t          j        k    rK| j        rd| _        |                      d¦  «         nd| _        |                      d¦  «         g d¢| _        �n÷| j        d         t          j        k    r8|                      t          | j        d	         ¦  «        ¦  «         g d¢| _        �n¤| j        d         t          j        k    r8|                      t          | j        d	         ¦  «        ¦  «         g d¢| _        �nQ| j        d         t          j
        k    rAd| _	        |                      | j        d	         | j        d
         | j        d         ¦  «         �nõ| j        d         t          j         k    �rd| _        | j        d	         dk    r¢t          | j        d
         ¦  «        | j!        d         d<   t          | j        d         ¦  «        | j!        d         d	<   t          | j        d         ¦  «        | j!        d         d
<   |  "                    ¦   «          |  #                    ¦   «          �n| j        d	         dk    r¢t          | j        d
         ¦  «        | j!        d	         d<   t          | j        d         ¦  «        | j!        d	         d	<   t          | j        d         ¦  «        | j!        d	         d
<   |  "                    ¦   «          |  #                    ¦   «          �nl| j        d	         dk    r¢t          | j        d
         ¦  «        | j!        d
         d<   t          | j        d         ¦  «        | j!        d
         d	<   t          | j        d         ¦  «        | j!        d
         d
<   |  "                    ¦   «          |  #                    ¦   «          �n¹| j        d	         dk    r¢t          | j        d
         ¦  «        | j!        d         d<   t          | j        d         ¦  «        | j!        d         d	<   t          | j        d         ¦  «        | j!        d         d
<   |  "                    ¦   «          |  #                    ¦   «          �n| j        d	         dk    r/|  $                    | j!        d¦  «         |                      ¦   «          nÅ| j        d         t          j%        k    rN| j        d	         dk    r=tM          j'        |  (                    ¦   «         ¬¦  «        }| )                    ¦   «          d S n[| j        dk    rPd| _        |                      d¦  «         | j*        d	k    rd	| _*        | j        dk    rd| _        d| _*        g d¢| _        n&# tV          $ r}tY          |¦  «         Y d }~nd }~ww xY w�	Œu)NTé<   r   rk   r   Fr   r   ro   rP   rq   Úonern   ÚtwoÚthreeÚfourÚsaver   Ú1r   éð   )-Útimer   r$   r   r   r   r,   ÚcmdÚ	CMD_RELAXr&   ÚCMD_ATTITUDEÚstopÚCMD_MOVE_STOPÚCMD_MOVE_FORWARDr<   r   ÚforWardÚCMD_MOVE_BACKWARDÚbackWardÚCMD_MOVE_LEFTÚsetpLeftÚCMD_MOVE_RIGHTÚ	setpRightÚCMD_TURN_LEFTÚturnLeftÚCMD_TURN_RIGHTÚ	turnRightÚ
CMD_HEIGHTÚ	upAndDownÚCMD_HORIZONÚbeforeAndAfterÚattitudeÚCMD_CALIBRATIONr!   r+   ry   rL   ÚCMD_BALANCEr'   r(   ÚIMU6050Ústartr   rv   rw   )r-   Ú
Thread_IMUrx   s      r.   r)   zControl.condition�   sî  € ðe	ðdÝ”9‘;”;˜tÔ0Ñ0°2Ò5Ð5¸$Ô:KÈQÒ:NÐ:NÐSWÔSbÐdhÒShÐShØ$%�D”OÝ&*¤i¡k¤k�DÔ%Ø”? SÒ(Ñ(Ýœ	™œ D¤LÑ0°"Ò4Ð4¸¼Àqº¸ÈTÌ_Ð^cÒMcÐMcÐhlÔhrÐstÔhuÐy{Òh{Ðh{Ý%)¤Y¡[¤[˜œØ(,˜œØŸ
š
 4Ñ(Ô(Ð(Ø#3Ð#3Ð#3˜œ
Ø”¨Ò,Ð,°´¸A´À"Ò1DÐ1DÈ$Ì*ÐUVÌ-ÕZ]ÔZgÒJgÐJgØŸ
š
 5Ñ)Ô)Ð)Ø(-˜œØÔ)¨4Ò/Ð/°D´J¸q´MÅCÔDTÒ4TÐ4TÐY]ÔYcÐdeÔYfÐjlÒYlÐYlØŸ	š	™œ˜Ø+0˜Ô*Ø”¨Ò-Ð-Ø˜œ­¬©¬°TÔ5FÑ)FÑF˜œÝ*.¬)©+¬+˜Ô)Ø”z !”}¥cÔ&7Ò7Ð7Ø#3Ð#3Ð#3˜œ
ØŸ	š	™œ˜™Øœ Aœ­Ô(<Ò<Ð<Ý#& t¤z°!¤}Ñ#5Ô#5˜œ
ØŸš™œ˜™Øœ Aœ­Ô(=Ò=Ð=Ý#& t¤z°!¤}Ñ#5Ô#5˜œ
ØŸš™œ˜™Øœ Aœ­Ô(9Ò9Ð9Ý#& t¤z°!¤}Ñ#5Ô#5˜œ
ØŸš™œ˜™Øœ Aœ­Ô(:Ò:Ð:Ý#& t¤z°!¤}Ñ#5Ô#5˜œ
ØŸšÑ(Ô(Ð(Ñ(Øœ Aœ­Ô(9Ò9Ð9Ý#& t¤z°!¤}Ñ#5Ô#5˜œ
ØŸš™œ˜™Øœ Aœ­Ô(:Ò:Ð:Ý#& t¤z°!¤}Ñ#5Ô#5˜œ
ØŸšÑ(Ô(Ð(Ñ(Øœ Aœ­¬Ò5Ð5Øœ?ð -Ø,1˜DœOØ ŸJšJ uÑ-Ô-Ð-Ð-à,0˜DœOØ ŸJšJ tÑ,Ô,Ð,Ø#3Ð#3Ð#3˜œ
™
Øœ Aœ­¬Ò6Ð6ØŸš¥s¨4¬:°a¬=Ñ'9Ô'9Ñ:Ô:Ð:Ø#3Ð#3Ð#3˜œ
™
Øœ Aœ­¬Ò7Ð7Ø×+Ò+­C°´
¸1´Ñ,>Ô,>Ñ?Ô?Ð?Ø#3Ð#3Ð#3˜œ
™
Øœ Aœ­Ô(8Ò8Ð8Ø+/˜Ô*ØŸš d¤j°¤m°D´J¸q´MÀ$Ä*ÈQÄ-ÑPÔPÐPÑPØœ Aœ­Ô(;Ò;Ñ;Ø()˜œØœ: aœ=¨%Ò/Ð/Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ø ×,Ò,Ñ.Ô.Ð.Ø ŸHšH™JœJ˜J™JØ!œZ¨œ]¨EÒ1Ð1Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ø ×,Ò,Ñ.Ô.Ð.Ø ŸHšH™JœJ˜J™JØ!œZ¨œ]¨GÒ3Ð3Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ø ×,Ò,Ñ.Ô.Ð.Ø ŸHšH™JœJ˜J™JØ!œZ¨œ]¨FÒ2Ð2Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ý9<¸T¼ZÈ¼]Ñ9KÔ9K˜DÔ2°1Ô5°aÑ8Ø ×,Ò,Ñ.Ô.Ð.Ø ŸHšH™JœJ˜J™JØ!œZ¨œ]¨FÒ2Ð2Ø ŸNšN¨4Ô+AÀ'ÑJÔJÐJØ ŸIšI™KœK˜KøØœ Aœ­¬Ò7Ð7¸D¼JÀq¼MÈ3Ò<NÐ<NÝ#,Ô#3¸4¿<º<¹>¼>Ð#JÑ#JÔ#J˜
Ø"×(Ò(Ñ*Ô*Ð*Ø˜øØ”_ sÒ*Ð*Ø$(�D”OØ—J’J˜tÑ$Ô$Ð$Ø”~ qÒ(Ð(Ø'(˜œØœ¨#Ò-Ð-Ø()˜œØ'(˜œØ/Ð/Ð/�D”JøøÝð ð ð Ý�a‘”������øøøøðøøøñIe	s   ƒc-e ã2Ae å
e2åe-å-e2c                 ó&   — ||k     r|S ||k    r|S |S ©N© )r-   ÚvarÚv_minÚv_maxs       r.   rt   zControl.restrictionè   s#   € Ø�Š;ˆ;ØˆLØ�5Š[ˆ[ØˆLàˆJr0   c                 ó*   — ||z
  ||z
  z  ||z
  z  |z   S r¥   r¦   )r-   ÚvalueÚfromLowÚfromHighÚtoLowÚtoHighs         r.   ÚmapzControl.mapï   s$   € Ø�u‘˜u W™}Ñ-°¸'Ñ1AÑBÀUÑJÐJr0   r   é`   ©rq   rn   c	                 ó.  — |dk    rÈt          d¦  «        D ]¶}	dd|	z   z  |z  dz   | j        d|	z           d<   || j        d|	z           d<   d|	z  |z  d|	z  dz  z   | j        d|	z           d<   dd|	z   z  |z  dz   | j        dd|	z  z            d<   || j        dd|	z  z            d<   dd|	z   z  |z  d|	z  dz  z   | j        dd|	z  z            d<   Œ·�n²|dk    rÂt          d¦  «        D ]°}	d|	z  |z  dz   | j        d|	z           d<   || j        d|	z           d<   dd|	z   z  |z  d|	z  dz  z   | j        d|	z           d<   d|	z  |z  dz   | j        dd|	z  z            d<   || j        dd|	z  z            d<   d|	z  |z  d|	z  dz  z   | j        dd|	z  z            d<   Œ±�nê|dk    s|d	k    rft          d¦  «        D ]T}	|dz   | j        d
|	z           d<   || j        d
|	z           d<   |dz   | j        d|	z            d<   || j        d|	z            d<   ŒU�nx|dk    rÍt          d¦  «        D ]¼}	|ddd|	z  z   f         dz   | j        d
|	z
           d<   |ddd|	z  z   f         | j        d
|	z
           d<   |ddd|	z  z   f         | j        d
|	z
           d<   |dd|	z  f         dz   | j        |	         d<   |dd|	z  f         | j        |	         d<   |dd|	z  f         | j        |	         d<   Œ½n¥t          d¦  «        D ]•}	|dz   | j        |	dz           d<   || j        |	dz           d<   |dz   | j        |	dz  dz            d<   || j        |	dz  dz            d<   |d|	z  dz  z   | j        |	dz           d<   |d|	z  dz  z   | j        |	dz  dz            d<   Œ–|                      ¦   «          d S )Nr—   rP   éÿÿÿÿro   r   r   r™   r   Úhorizonrq   úAttitude Angle)r7   r   ry   )
r-   Ú
move_orderÚX1ÚY1ÚZ1ÚX2ÚY2ÚZ2ÚposrB   s
             r.   ÚchangeCoordinateszControl.changeCoordinatesñ   sU  € Ø˜Ò#Ð#Ý˜1‘X”Xð Cð C�Ø%'¨1¨Q©3¡K°Ñ#3°BÑ#6�”
˜1˜Q™3” Ñ"Ø#%�”
˜1˜Q™3” Ñ"Ø%'¨1¡I¨r¡>°B¸±7¸B±,Ñ#>�”
˜1˜Q™3” Ñ"Ø')¨Q¨q©S¡k°2Ñ%5°bÑ%8�”
˜1˜Q˜q™S™5Ô! !Ñ$Ø%'�”
˜1˜Q˜q™S™5Ô! !Ñ$Ø')¨Q¨q©S¡k°2Ñ%5¸¸Q±wÀ±lÑ%B�”
˜1˜Q˜q™S™5Ô! !Ñ$Ð$ñCð ˜;Ò&Ð&Ý˜1‘X”Xð Að A�Ø%'¨1¡I¨r¡>°"Ñ#4�”
˜1˜Q™3” Ñ"Ø#%�”
˜1˜Q™3” Ñ"Ø%'¨1¨Q©3¡K°Ñ#3°b¸1±W¸b±LÑ#@�”
˜1˜Q™3” Ñ"Ø')¨Q¡i°¡^°BÑ%6�”
˜1˜Q˜q™S™5Ô! !Ñ$Ø%'�”
˜1˜Q˜q™S™5Ô! !Ñ$Ø')¨Q¡i°¡^°b¸1±W¸b±LÑ%@�”
˜1˜Q˜q™S™5Ô! !Ñ$Ð$ñAð ˜HÒ$Ð$¨*¸	Ò*AÐ*AÝ˜1‘X”Xð &ð &�Ø#% b¡5�”
˜1˜Q™3” Ñ"Ø#%�”
˜1˜Q™3” Ñ"Ø#% b¡5�”
˜1˜Q™3” Ñ"Ø#%�”
˜1˜Q™3” Ñ"Ð"ñ	&ð
 Ð+Ò+Ð+Ý˜1‘X”Xð ,ð ,�Ø#& q¨¨1¨Q©3© w¤<°¡?�”
˜1˜Q™3” Ñ"Ø#& q¨¨1¨Q©3© w¤<�”
˜1˜Q™3” Ñ"Ø#& q¨¨1¨Q©3© w¤<�”
˜1˜Q™3” Ñ"Ø!$ Q q¨¡s U¤¨B¡�”
˜1”˜aÑ Ø!$ Q q¨¡s U¤�”
˜1”˜aÑ Ø!$ Q q¨¡s U¤�”
˜1”˜aÑ Ð ð,õ ˜1‘X”Xð 5ð 5�Ø#% b¡5�”
˜1˜Q™3” Ñ"Ø#%�”
˜1˜Q™3” Ñ"Ø%'¨¡U�”
˜1˜Q™3˜q™5Ô! !Ñ$Ø%'�”
˜1˜Q™3˜q™5Ô! !Ñ$Ø#%¨¨Q¡w°¡l¡?�”
˜1˜Q™3” Ñ"Ø%'¨"¨q©°"©¡_�”
˜1˜Q™3˜q™5Ô! !Ñ$Ð$Ø�Š‰
Œ
ˆ
ˆ
ˆ
r0   c           
      ó  — t          dd| j         ¦  «        D ]ó}dt          j        |t          j        z  dz  ¦  «        z  }dt          j        |t          j        z  dz  ¦  «        z  | j        z   }dt          j        |dz   t          j        z  dz  ¦  «        z  }dt          j        |dz   t          j        z  dz  ¦  «        z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|                      d||d||d¦  «         Œôd S )NéÂ  éY   é   rk   é   r‘   r   ©r7   r   rQ   rU   rR   rT   r   r¿   ©r-   rB   r¸   r¹   r»   r¼   s         r.   r‘   zControl.backWard  s  € Ý�s˜2˜tœz˜kÑ*Ô*ð 		?ð 		?ˆAØ•$”(˜1�TœW™9 S™=Ñ)Ô)Ñ)ˆBØ•”˜!�DœG™) C™-Ñ(Ô(Ñ(¨¬Ñ4ˆBØ•$”(˜A˜c™E¥4¤7™?¨3Ñ.Ñ/Ô/Ñ/ˆBØ•”˜1˜S™5¥$¤'™/¨#Ñ-Ñ.Ô.Ñ.¨t¬{Ñ:ˆBØ�D”KÒÐØ”;�Ø�D”KÒÐØ”;�Ø×"Ò" :¨b°°A°b¸¸AÑ>Ô>Ð>Ð>ð		?ð 		?r0   c           
      ó  — t          dd| j        ¦  «        D ]ó}dt          j        |t          j        z  dz  ¦  «        z  }dt          j        |t          j        z  dz  ¦  «        z  | j        z   }dt          j        |dz   t          j        z  dz  ¦  «        z  }dt          j        |dz   t          j        z  dz  ¦  «        z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|                      d||d||d¦  «         Œôd S )Nr   éÃ  rÃ   rk   rÄ   r�   r   rÅ   rÆ   s         r.   r�   zControl.forWard%  s  € Ý�r˜#˜dœjÑ)Ô)ð 		>ð 		>ˆAØ•$”(˜1�TœW™9 S™=Ñ)Ô)Ñ)ˆBØ•”˜!�DœG™) C™-Ñ(Ô(Ñ(¨¬Ñ4ˆBØ•$”(˜A˜c™E¥4¤7™?¨3Ñ.Ñ/Ô/Ñ/ˆBØ•”˜1˜S™5¥$¤'™/¨#Ñ-Ñ.Ô.Ñ.¨t¬{Ñ:ˆBØ�D”KÒÐØ”;�Ø�D”KÒÐØ”;�Ø×"Ò" 9¨R°°1°R¸¸1Ñ=Ô=Ð=Ð=ð		>ð 		>r0   c           
      ó"  — t          dd| j        ¦  «        D ]÷}dt          j        |t          j        z  dz  ¦  «        z  }dt          j        |t          j        z  dz  ¦  «        z  | j        z   }dt          j        |dz   t          j        z  dz  ¦  «        z  }dt          j        |dz   t          j        z  dz  ¦  «        z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|}|}|                      d||||||¦  «         Œød S )Nr   éi  rq   rk   r	   r—   rÅ   ©r-   rB   r¸   r¹   r»   r¼   rº   r½   s           r.   r—   zControl.turnLeft1  s  € Ý�q˜˜TœZÑ(Ô(ð 	Að 	AˆAØ•”˜!�DœG™) C™-Ñ(Ô(Ñ(ˆBØ•”˜!�DœG™) C™-Ñ(Ô(Ñ(¨¬Ñ4ˆBØ•”˜1˜S™5¥$¤'™/¨#Ñ-Ñ.Ô.Ñ.ˆBØ•”˜1˜S™5¥$¤'™/¨#Ñ-Ñ.Ô.Ñ.¨t¬{Ñ:ˆBØ�D”KÒÐØ”;�Ø�D”KÒÐØ”;�ØˆBØˆBØ×"Ò" :¨b°°B°r¸"¸RÑ@Ô@Ð@Ð@ð	Að 	Ar0   c           
      ó"  — t          dd| j        ¦  «        D ]÷}dt          j        |t          j        z  dz  ¦  «        z  }dt          j        |t          j        z  dz  ¦  «        z  | j        z   }dt          j        |dz   t          j        z  dz  ¦  «        z  }dt          j        |dz   t          j        z  dz  ¦  «        z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|}|}|                      d||||||¦  «         Œød S )Nr   rÊ   rq   rk   r	   r™   rÅ   rË   s           r.   r™   zControl.turnRight@  s  € Ý˜˜#˜dœjÑ)Ô)ð 
Bð 
BˆQØ•”˜!�DœG™) C™-Ñ(Ô(Ñ(ˆBØ•”˜!�DœG™) C™-Ñ(Ô(Ñ(¨¬Ñ4ˆBØ•”˜1˜S™5¥$¤'™/¨#Ñ-Ñ.Ô.Ñ.ˆBØ•”˜1˜S™5¥$¤'™/¨#Ñ-Ñ.Ô.Ñ.¨t¬{Ñ:ˆBØ�D”KÒÐØ”;�Ø�D”KÒÐØ”;�ØˆBØˆBØ×"Ò" ;¨r°"°R¸¸2¸bÑAÔAÐAÐAð
Bð 
Br0   c                 óä  — d| j         dgd| j         dgd| j         dgd| j         dgg}t          d¦  «        D ]Œ}||         d         | j        |         d         z
  dz  ||         d<   ||         d         | j        |         d         z
  dz  ||         d<   ||         d         | j        |         d         z
  dz  ||         d<   Œ�t          d¦  «        D ]�}t          d¦  «        D ]w}| j        |         dxx         ||         d         z  cc<   | j        |         dxx         ||         d         z  cc<   | j        |         dxx         ||         d         z  cc<   Œx|                      ¦   «          Œžd S )Nr   r   rn   r   é2   ro   rP   )r   r7   r   ry   )r-   ÚprB   rD   s       r.   rŒ   zControl.stopN  s‘  € Ø�”˜RÐ  2 t¤{°BÐ"7¸"¸d¼kÈ3Ð9OÐRTÐVZÔVaÐcfÐQgÐ
hˆÝ�q‘”ð 	2ð 	2ˆAØ�q”T˜!”W˜TœZ¨œ]¨1Ô-Ñ-¨rÑ1ˆAˆaŒD�‰GØ�q”T˜!”W˜TœZ¨œ]¨1Ô-Ñ-¨rÑ1ˆAˆaŒD�‰GØ�q”T˜!”W˜TœZ¨œ]¨1Ô-Ñ-¨rÑ1ˆAˆaŒD�‰GˆGÝ�r‘”ð 	ð 	ˆAÝ˜1‘X”Xð *ð *�Ø”
˜1”˜aÐ Ð Ô  ! A¤$ q¤'Ñ)Ð Ð Ñ Ø”
˜1”˜aÐ Ð Ô  ! A¤$ q¤'Ñ)Ð Ð Ñ Ø”
˜1”˜aÐ Ð Ô  ! A¤$ q¤'Ñ)Ð Ð Ñ Ð Ø�HŠH‰JŒJˆJˆJð	ð 	r0   c           
      ó  — t          dd| j        ¦  «        D ]ó}dt          j        |t          j        z  dz  ¦  «        z  }dt          j        |t          j        z  dz  ¦  «        z  | j        z   }dt          j        |dz   t          j        z  dz  ¦  «        z  }dt          j        |dz   t          j        z  dz  ¦  «        z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|                      dd||d||¦  «         Œôd S )Nr   rÈ   r   rk   é   r“   r   rÅ   ©r-   rB   rº   r¹   r½   r¼   s         r.   r“   zControl.setpLeftZ  s  € Ý�r˜#˜dœjÑ)Ô)ð 		?ð 		?ˆAØ•$”(˜1�TœW™9 S™=Ñ)Ô)Ñ)ˆBØ•”˜!�DœG™) C™-Ñ(Ô(Ñ(¨¬Ñ4ˆBØ•$”(˜A˜c™E¥4¤7™?¨3Ñ.Ñ/Ô/Ñ/ˆBØ•”˜1˜S™5¥$¤'™/¨#Ñ-Ñ.Ô.Ñ.¨t¬{Ñ:ˆBØ�D”KÒÐØ”;�Ø�D”KÒÐØ”;�Ø×"Ò" :¨a°°2°a¸¸2Ñ>Ô>Ð>Ð>ð		?ð 		?r0   c           
      ó  — t          dd| j         ¦  «        D ]ó}dt          j        |t          j        z  dz  ¦  «        z  }dt          j        |t          j        z  dz  ¦  «        z  | j        z   }dt          j        |dz   t          j        z  dz  ¦  «        z  }dt          j        |dz   t          j        z  dz  ¦  «        z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|                      dd||d||¦  «         Œôd S )NrÁ   rÂ   r   rk   rÑ   r•   r   rÅ   rÒ   s         r.   r•   zControl.setpRightf  s
  € Ý�s˜2˜tœz˜kÑ*Ô*ð 		@ð 		@ˆAØ•$”(˜1�TœW™9 S™=Ñ)Ô)Ñ)ˆBØ•”˜!�DœG™) C™-Ñ(Ô(Ñ(¨¬Ñ4ˆBØ•$”(˜A˜c™E¥4¤7™?¨3Ñ.Ñ/Ô/Ñ/ˆBØ•”˜1˜S™5¥$¤'™/¨#Ñ-Ñ.Ô.Ñ.¨t¬{Ñ:ˆBØ�D”KÒÐØ”;�Ø�D”KÒÐØ”;�Ø×"Ò" ;¨q°°B°q¸¸BÑ?Ô?Ð?Ð?ð		@ð 		@r0   Fc                 ó¼  — |dk    �r¨g d¢g d¢g d¢g d¢g}t          d¦  «        D ]Œ}| j        |         d         ||         d         z
  dz  ||         d<   | j        |         d         ||         d         z
  dz  ||         d<   | j        |         d         ||         d         z
  dz  ||         d<   Œ�t          dd¦  «        D ]�}t          d¦  «        D ]w}| j        |         dxx         ||         d         z  cc<   | j        |         dxx         ||         d         z  cc<   | j        |         dxx         ||         d         z  cc<   Œx|                      ¦   «          Œž| j        dk    rC| xj        t          j        ¦   «         | j        z
  z  c_        t          j        ¦   «         | _        d S d S |                      ¦   «          t          j        ¦   «         | _        d S )	NT)rN   éN   r   rn   r   rÎ   ro   rP   é3   )r7   r   ry   r   r   rˆ   rŒ   )r-   r}   rÏ   rB   rD   s        r.   r,   zControl.relaxr  så  € Ø�Š:‰:Øˆ{ˆ{˜K˜K˜K¨¨¨°k°k°kÐBˆAÝ˜1‘X”Xð 6ð 6�Øœ Aœ qÔ)¨!¨A¬$¨q¬'Ñ1°2Ñ5��!”�Q‘Øœ Aœ qÔ)¨!¨A¬$¨q¬'Ñ1°2Ñ5��!”�Q‘Øœ Aœ qÔ)¨!¨A¬$¨q¬'Ñ1°2Ñ5��!”�Q‘�Ý˜1˜R‘[”[ð ð �Ý˜q™œð .ð .�AØ”J˜q”M !Ð$Ð$Ô$ a¨¤d¨1¤gÑ-Ð$Ð$Ñ$Ø”J˜q”M !Ð$Ð$Ô$ a¨¤d¨1¤gÑ-Ð$Ð$Ñ$Ø”J˜q”M !Ð$Ð$Ô$ a¨¤d¨1¤gÑ-Ð$Ð$Ñ$Ð$Ø—’‘
”
�
�
ØÔ  !Ò#Ð#Ø�”¥¤¡¤¨TÔ->Ñ!>Ñ>�”Ý"&¤)¡+¤+�Ô!Ð!Ð!ð $Ð#ð �IŠI‰KŒKˆKÝ"œi™kœkˆDÔÐÐr0   c           	      ód   — |dz   | _         |                      dd| j         dd| j         d¦  «         d S )Nr
   r   r   )r   r¿   ©r-   r§   s     r.   r›   zControl.upAndDown…  s7   € Ø˜‘FˆŒØ×Ò˜x¨¨$¬+°a¸¸$¼+ÀaÑHÔHÐHÐHÐHr0   c           	      óP   — |                       d|| j        d|| j        d¦  «         d S )Nrµ   r   )r¿   r   rØ   s     r.   r�   zControl.beforeAndAfterˆ  s,   € Ø×Ò˜y¨¨T¬[¸¸3¸t¼{È1ÑMÔMÐMÐMÐMr0   c                 óH  — |                       t          |¦  «        dddd¦  «        }|                       t          |¦  «        dddd¦  «        }|                       t          |¦  «        dddd¦  «        }|                      |||d¦  «        }|                      d|¬¦  «         d S )Niìÿÿÿé   r   r   r   r¶   ©r¾   )r°   r<   ÚpostureBalancer¿   )r-   r3   rÏ   r\   r¾   s        r.   rž   zControl.attitudeŠ  s˜   € Ø
�(Š(•3�q‘6”6˜#˜b  RÑ
(Ô
(ˆØ
�(Š(•3�q‘6”6˜#˜b  RÑ
(Ô
(ˆØ
�(Š(•3�q‘6”6˜#˜b  RÑ
(Ô
(ˆØ×Ò  ! A aÑ(Ô(ˆØ×ÒÐ/°CÐÑ8Ô8Ð8Ð8Ð8r0   c                 óÐ  — d| _         g d¢| _        |                      ddd¦  «        }|                      d|¬¦  «         t	          j        d¦  «         | j                             ¦   «         \  | j        _        | j        _	        t	          j        d¦  «         	 | xj
        t	          j        ¦   «         | j        z
  z  c_
        t	          j        ¦   «         | _        | j                             ¦   «         \  }}}| j                             |¦  «        }| j                             |¦  «        }|                      ||d¦  «        }|                      d|¬¦  «         | j        d         t          j        k    r| j        d         dk    s'| j         dk    r| j        d         d	k    s| j
        d
k    r;t#          j        | j        ¬¦  «        }|                     ¦   «          | j         dk     d S �ŒP)NTr   r   r¶   rÜ   rP   ro   Ú0r   rk   r   F)r%   r   rÝ   r¿   rˆ   Úsleepr   Úaverage_filterÚError_value_accel_dataÚError_value_gyro_datar   r   Ú	imuUpdater   ÚPID_computer‰   r    r'   r(   r)   r¢   )r-   r¾   r3   rÏ   r\   r*   s         r.   r¡   zControl.IMU6050�  sÎ  € ØˆÔØ#Ð#Ð#ˆŒ
Ø×Ò  ! AÑ&Ô&ˆØ×ÒÐ/°CÐÑ8Ô8Ð8ÝŒ
�1‰ŒˆØGKÄx×G^ÒG^ÑG`ÔG`ÑFˆŒÔ'¨¬Ô(FÝŒ
�1‰Œˆð	ØˆOŒO�TœY™[œ[¨Ô):Ñ:Ñ:ˆOŒOÝ"œi™kœkˆDÔØ”(×$Ò$Ñ&Ô&‰EˆAˆa�ØŒh×"Ò" 1Ñ%Ô%ˆAØŒh×"Ò" 1Ñ%Ô%ˆAØ×#Ò# A a¨Ñ*Ô*ˆCØ×"Ò"Ð#3¸Ð"Ñ<Ô<Ð<Ø”˜A”¥¤Ò/Ð/°D´J¸q´MÀ3Ò4FÐ4FÈ$ÔJ[Ð]aÒJaÐJaÐfjÔfpÐqrÔfsÐuwÒfwÐfwÐ{ô  |Kð  LOò  |Oð  |OÝ"+Ô"2¸$¼.Ð"IÑ"IÔ"IÐ!Ø!×'Ò'Ñ)Ô)Ð)ØÔ! 5Ò(Ð(Ø�ñ	r0   ro   c           	      óŠ  — d}d}d}|dk    r| j         }t          j        dd|g¦  «        j        }t          j        |||g¦  «        t
          j        z  dz  }	|	d         |	d         |	d         }}}
t          j        g d¢dt          j        |
¦  «        t          j        |
¦  «         gdt          j        |
¦  «        t          j        |
¦  «        gg¦  «        }t          j        t          j        |¦  «        dt          j        |¦  «         gg d	¢t          j        |¦  «        dt          j        |¦  «        gg¦  «        }t          j        t          j        |¦  «        t          j        |¦  «         dgt          j        |¦  «        t          j        |¦  «        dgg d
¢g¦  «        }||z  |z  }t          j        |dz  |dz  dg|dz  | dz  dg| dz  |dz  dg| dz  | dz  dgg¦  «        j        }t          j        |dz  |dz  dz   | j         |z
  g|dz  | dz  dz
  | j         |z
  g| dz  |dz  dz   | j         |z
  g| dz  | dz  dz
  | j         |z
  gg¦  «        j        }t          j        t          j	        d¦  «        ¦  «        }t          d¦  «        D ](}|||d d …|f         z  z   |d d …|f         z
  |d d …|f<   Œ)|S )NéL   éˆ   r   r   rk   ro   rP   )ro   r   r   )r   ro   r   )r   r   ro   r   r²   rn   )r   ÚnpÚmatÚTÚarrayrQ   rR   rU   rT   Úzerosr7   )r-   r3   rÏ   r\   Úhrg   rF   Úlr¾   ÚrpyÚRÚPÚYÚrotxÚrotyÚrotzÚrot_matÚ
body_strucÚfootpoint_strucÚABrB   s                        r.   rÝ   zControl.postureBalance¥  s  € ØˆØˆØˆØˆaŠ4ˆ4ØŒkˆAÝŒf�c˜C !Ð%Ñ&Ô&Ô(ˆÝŒh˜˜A �{Ñ#Ô#¥d¤gÑ-°Ñ3ˆØ�a”&˜#˜aœ& # a¤&ˆaˆ1ˆÝŒvÐ<Ð<Ð<Ø¥¤¨¡¤­d¬h°q©k¬k¨\Ð:Ø¥¤¨¡¤­d¬h°q©k¬kÐ:ð<ñ =ô =ˆõ Œv�œ !™œ qµ´¸±´¨|Ð<Ø:Ð:Ð:Ý”x ‘{”{ Q­d¬h°q©k¬kÐ:ð<ñ =ô =ˆõ Œv�œ !™œ¥t¤x°¡{¤{ l°QÐ<Ý”x ‘{”{¥T¤X¨a¡[¤[°1Ð:Ø:Ð:Ð:ð<ñ =ô =ˆð ˜‘+ Ñ$ˆÝ”V˜q 1™u q¨1¡u¨qÐ1Ø ™U Q B¨¡F¨qÐ1Ø˜B ™F Q¨¡U¨qÐ1Ø˜B ™F Q B¨¡F¨qÐ1ð3ñ 4ô 4ô 56ð 	õ œ& A¨¡E¨a°!©e°R©Z¸$¼+Àa¹-Ð"HØ#$ q¡5¨Q¨B°©F°B©;¸4¼;Àq¹=Ð IØ#$ " q¡&¨Q°©U°B©J¸4¼;Àq¹=Ð IØ#$ " q¡&¨Q¨B°©F°B©;¸4¼;Àq¹=Ð Ið"Kñ Lô Lô MNð 	õ ŒV•B”H˜VÑ$Ô$Ñ%Ô%ˆÝ�q‘”ð 	Pð 	PˆAØ˜W °q°q°q¸!°tÔ'<Ñ<Ñ<¸zÈ!È!È!ÈQÈ$Ô?OÑOˆBˆqˆqˆq�!ˆt‰HˆHØˆr0   N)rM   rN   rN   )F)ro   )Ú__name__Ú
__module__Ú__qualname__r/   r    rL   ri   rl   r+   ry   rs   r)   rt   r°   ré   rê   rí   r¿   r‘   r�   r—   r™   rŒ   r“   r•   r,   r›   r�   rž   r¡   rÝ   r¦   r0   r.   r   r      sì  € € € € € ðð ð ð*ð ð ðð ð ðð ð ð ðð ð ð ðWð Wð WðFð Fð Fð2ð ð ðfð fð fðNð ð ðKð Kð Kà-.°"¸¸QÀ"ÈÈfÈbÌfÐU]ÐUWÔU]Ð^dÑUeÔUeÑNfÔNfð 'ð 'ð 'ð 'ðP
?ð 
?ð 
?ð
>ð 
>ð 
>ðAð Að AðBð Bð Bðð ð ð
?ð 
?ð 
?ð
@ð 
@ð 
@ð*ð *ð *ð *ð&Ið Ið IðNð Nð Nð9ð 9ð 9ðð ð ð*ð ð ð ð ð r0   r   Ú__main__)rˆ   rQ   ÚsmbusÚcopyr'   r   ÚPIDÚnumpyré   r   ÚCommandr   r‰   r   rû   r¦   r0   r.   ú<module>r     sÂ   ðà €€€Ø €€€Ø €€€Ø €€€Ø Ð Ð Ð Ø Ð Ð Ð Ø Ð Ð Ð Ø Ð Ð Ð Ø Ð Ð Ð Ø "Ð "Ð "Ð "Ð "Ð "ðvð vð vð vð vñ vô vð vðp ˆZÒÐØ€Dð Ðr0   