B
    d%                 @   st   d dl mZ d dlZd dlZd dlT d dlmZ d dlm	Z	 d dl
mZ d dlmZ d dlmZ d	d
 Zdd ZdS )    )sleepN)*)turn)locate)locate_g)UturnBK)	distance1c             C   s   | dkr t ddd t ddd | dkr@t ddd t ddd | dkr`t ddd t ddd | dkrt d	dd t ddd | d
krt dd	d t ddd d S )Nr   g        g?	      g333333?g{Gz?   g333333ӿ   )r   )DR r   /home/pi/P21/FXGO2.pymoving   s              r   c          
   C   sN  d}d}d}x:t dD ],}t }|dk rDtddd tddd	 |  \}}|d
krZP t|}t|\}	}
}}}td| t	dd@ t
dkrP |	dkrd}n8|
d |k rd}n&||
d   kr|krn nd}nd}t||	|
|||
d| |	dkr6t|dkr6t|dkr6tdt|t| d}P t| |d7 }qW d S )Nr   g      h@g      |@i     g      ?g        g333333?g?Fimg24r
      qr   r   z DR=F   zSTOP max(ys,WDS)=r	   )ranger   r   readnpcopyr   cv2imshowwaitKeyordprintmaxr   )captZLTXZRTXjdis1retframeimg2NNxsysWDSr   r   r   r   r   FXGO2   s<       
    &  r+   )timer   r   numpyr   PARAMmotormr   ZLOCATE1r   LOCATE_goalr   r   ZDISTr   r   r+   r   r   r   r   <module>   s   