legoEv3/ev3dev2/control/__pycache__/GyroBalancer.cpython-35.pyc
h^[J @ sb d Z d d l Z d d l Z d d l Z d d l Z d d l Z d d l Z d d l Z d d l m Z d d l
m Z d d l m
Z
m Z m Z d d l m Z m Z d d l m Z d d l m Z e j e Z e j d Z d
Z e e Z d
Z e e Z d Z e d d
Z e e Z! d Z" d Z# Gd d d e$ Z% Gd d d e& Z' d S)a Module for a robot that stands on two wheels and uses a gyro sensor.
The robot (eg. BALANC3R) will to keep its balance and move in response to
the remote control. This code was adapted from Laurens Valk's script at
https://github.com/laurensvalk/segway.
N)deque) PowerSupply)
LargeMotorOUTPUT_AOUTPUT_D)
GyroSensor TouchSensor)Sound) OrderedDict g333333?ih < c @ s e Z d Z d Z d d d d d d d d d d
e e d d d
Z d d Z d d Z d d Z d d Z
d d Z d d Z d d Z
d d Z d d d d d! Z d d" d# Z d d$ d% Z d d& d' Z d d( d) Z d* d+ Z d S), GyroBalancerzt
Base class for a robot that stands on two wheels and uses a gyro sensor.
Robot will keep its balance.
i x g @ g?Fc C s | | _ | | _ | | _ | | _ | | _ | | _ | | _ | | _ | | _ |
| _ t
| _ t | _
| j
j | j
_ t | _ t | | _ t | | _ t | _ t | j
j d d | _ t | j j d d | _ t | j j d d | _ t | j j d d | _ t | j j d d | _ t | j j d d | _ t j | _! t" j# | _$ |
| _% t& j& t& j' | j( t& j& t& j) | j* d S)zCreate GyroBalancer.z/value0rbz /positionz/duty_cycle_spwN)+gain_gyro_anglegain_gyro_rategain_motor_anglegain_motor_angular_speed"gain_motor_angle_error_accumulatedpower_voltage_nominalpwr_friction_offset_nomtiming_loop_msecmotor_angle_history_length gyro_drift_compensation_factorr power_supplyr ZgyroZMODE_GYRO_RATEmoder touchr
motor_left motor_rightr soundopen_path gyro_file
touch_fileencoder_left_fileencoder_right_file dc_left_file
dc_right_filequeueZQueue drive_queue threadingZEvent stop_balancedebugsignalSIGINTsignal_int_handlerSIGTERMsignal_term_handler)selfr r r r r r r r r! r" Zleft_motor_portZright_motor_portr5 r< >/usr/lib/python3/dist-packages/ev3dev2/control/GyroBalancer.py__init__U s> zGyroBalancer.__init__c C sy | j j | j j | j j | j j | j j | j j | j j | j
j | j j d S)z+Close all file handles and stop all motors.N) r4 setr& stopr' r+ closer, r- r. r/ r0 )r; r< r< r= shutdown s
zGyroBalancer.shutdownc C s) | j d t | j j j S)z,Function for fast reading from sensor files.r )seekintreaddecodestrip)r; Zinfiler< r< r=
_fast_read s
zGyroBalancer._fast_readc C s4 | j d | j t t | | j d S)z)Function for fast writing to motor files.r N)truncatewritestrrD flush)r; Zoutfilevaluer< r< r= _fast_write s
zGyroBalancer._fast_writec C sk t t | | } | d k r8 t d | | } n | d k rW t d | | } | j | | d S)z-Function to set the duty cycle of the motors.r d Ni)rD roundminmaxrN )r; Zmotor_duty_fileZdutyfriction_offset voltage_compZduty_intr< r< r= _set_duty s zGyroBalancer._set_dutyc C s$ t j d | j t d S)zSignal handler for SIGINT.z"Caught SIGINTN)loginforB GracefulShutdown)r; signumframer< r< r= r8 s
z GyroBalancer.signal_int_handlerc C s$ t j d | j t d S)zSignal handler for SIGTERM.z"Caught SIGTERMN)rV rW rB rX )r; rY rZ r< r< r= r: s
z GyroBalancer.signal_term_handlerc C s# t j d | j } | j d S)z$Run the _balance method as a thread.targetN)r3 ZThread_balancestart)r; Zbalance_threadr< r< r= balance s zGyroBalancer.balancec $
C sy xrd rt| j j rt| j j | j j | j j | j j d } d } d } d } d } d } d } d } d } d }
d } d } d }
t j d | j j d | j
j | j j
} | j | } t t | j | } | j d } d } t d g | j } | j | t } t j d t j d d } x7 t | D]) } |
| j | j }
t j d q]W|
| }
t j d
t |
t j d t j d t j d t j d t j d | j j t j } | j r=t } t } | | d
<t } | | d <d } d \ } } t j } x;| r| j j r| d 7} y& | j j! \ } } | j j" Wn t# j$ k
rYn X| j | j% } | j | j& | j | j' d } | t( } | j | j }
d } x- | | k rFt j | } t j d qWt j | } t j } | j rt j | } |
| | <| d | | <|
|
t } | t) } | | | } | | } | | d | j | } | } | j* | | j+ | | j, | | j- | | j. | | j/ | } | j0 | j1 | | | | | j0 | j2 | | | | | | | } d | |
| |
}
| | | } q^Wt j }! | j3 | j2 d | j3 | j1 d x | j
j4 rt j d qW|! | | }" t j d t |" d d t j d t j d t j d | j r t5 d d }# t6 j7 | |# Wd QRXq Wd S)zMake the robot balance.Tr z0Hold robot upright. Press touch sensor to start.z Press touch sensor to start.i z#-----------------------------------zCalibrating...rO g{Gz?z
gyro_offset: zGO!z Press Touch Sensor to re-start.
loop_times
gyro_readingsFr g @gMbP?g @@z
Loop time:ZmsZSTOPzdata.txtr N)r r )8r4 Zis_setr& resetr' Z
run_directrV rW r( Zspeakr% Z
wait_for_bumpr# Zmeasured_voltsr rD rP r r r r! r" RAD_PER_SEC_PER_RAW_GYRO_UNITrangerH r+ timesleeprK Zbeepr5 r
r2 Z
get_nowaitZ task_doner1 ZEmptyr, r- r. RAD_PER_RAW_MOTOR_UNIT RAD_PER_SEC_PER_PERCENT_SPEEDappendr r r r r rU r0 r/ rN Z
is_pressedr) jsondump)$r; Zmotor_angle_rawZ motor_angleZmotor_angle_refZmotor_angle_errorZmotor_angle_error_accZmotor_angular_speedZmotor_angular_speed_refZmotor_angular_speed_errorZmotor_duty_cycleZ
gyro_rate_rawZ gyro_rateZgyro_est_angleZ gyro_offsetZ voltage_idlerT rS Zloop_time_targetZ
loop_countZmotor_angle_histZgyro_drift_comp_rateZgyro_calibrate_countiZprog_start_timedatar_ r` Z
touch_pressedspeedsteeringZloop_start_timeZ loop_timeZtime_of_sampleZ
prog_end_timeZ
avg_loop_timeZ data_filer< r< r= r\ s
+
zGyroBalancer._balancer Nc C sP | j j | | f | d k r? t j | | j j d | j j d S)z Move robot.Nr )r r )r2 Zputrd re join)r; rm rn secondsr< r< r= _move s
zGyroBalancer._movec C s | j d t d d d | d S)zMove robot forward.rm rn r rp N)rq SPEED_MAX)r; rp r< r< r= move_forward s zGyroBalancer.move_forwardc C s! | j d t d d d | d S)zMove robot backward.rm rn r rp N)rq rr )r; rp r< r< r=
move_backward s zGyroBalancer.move_backwardc C s | j d d d t d | d S)zRotate robot left.rm r rn rp N)rq STEER_MAX)r; rp r< r< r= rotate_left s zGyroBalancer.rotate_leftc C s! | j d d d t d | d S)zRotate robot right.rm r rn rp N)rq ru )r; rp r< r< r= rotate_right s zGyroBalancer.rotate_rightc C s | j d d d d d S)z%Stop robot (balancing will continue).rm r rn N)rq )r; r< r< r= r@ s zGyroBalancer.stop)__name__
__module__ __qualname____doc__r r r> rB rH rN rU r8 r: r^ r\ rq rs rt rv rw r@ r< r< r< r= r N s: @ r c @ s e Z d Z d Z d S)rX z(Custom exception for SIGINT and SIGTERM.N)rx ry rz r{ r< r< r< r= rX s rX )(r{ Zloggingrd ri r1 r3 Zmathr6 collectionsr Z ev3dev.powerr Z ev3dev.motorr r r Zev3dev.sensor.legor r Z ev3dev.soundr r
Z getLoggerrx rV ZpiZ RAD_PER_DEGZ DEG_PER_SEC_PER_RAW_GYRO_UNITrb ZDEG_PER_RAW_MOTOR_UNITrf ZRPM_PER_PERCENT_SPEEDZ DEG_PER_SEC_PER_PERCENT_SPEEDrg rr ru objectr ExceptionrX r< r< r< r= <module> s6