o
|
Ç$i ã @ sr d Z ddlmZ ddlZddlZddlZddlmZ dZdd d Z dddZ
|
ej G dd dZ G dd dZ dS )u*
|
å·®é履带è¿å¨å¦/å¨åå¦æ¨¡åã
|
é )Ú annotationsN)ÚTupleg£:#@ÚvalueÚfloatÚ min_valueÚ max_valueÚreturnc C s t |t|| S ©N)ÚmaxÚmin)r r r © r ú0E:\GIT\Lawnmower_STM32H7\PYTHON\hitl\dynamics.pyÚ_clamp s r Úradc C sF t j}| t jkr| |8 } | t jks| t j k r!| |7 } | t j k s| S )zWrap angle to [-pi, pi).)ÚmathÚtauÚpi)r Zpi2r r r Ú _wrap_angle s
|
|
ÿ ÿr c @ sÀ e Zd ZU dZded< dZded< dZded< dZded< dZded< dZ ded< dZ
|
ded < dZ ded
|
< dZ ded < dZ ded < dZded < dZded< dZded< dZded< dddZdS )ÚDifferentialDriveStateç r ÚeastÚnorthÚupÚheadingÚlinear_velocityÚangular_velocityÚ pitch_degÚroll_degÚ east_velocityÚnorth_velocityÚ up_velocity)r r ç ð¿zTuple[float, float, float]Ú body_accel_g)r r r Ú
|
gyro_deg_sg >@Ú temperature_cr ú'DifferentialDriveState'c C s
|
t | ¡S r )Ú dataclassesÚreplace)Úselfr r r Úcopy. s
|
zDifferentialDriveState.copyN)r r% )Ú__name__Ú
|
__module__Ú __qualname__r Ú__annotations__r r r r r r r r r r r" r# r$ r) r r r r r s
|
r c @ sH e Zd ZdZddde d¡e d¡fd d dZd d ddZd!dd Zd S )"ÚDifferentialDriveModelu6 åéæå·®é履带模åï¼å¯æ¨¡æåå°è½¬åãgé?g @g ø?g ^@g f@Ú track_widthr Úmax_linear_speedÚmax_linear_accelÚmax_angular_speedÚmax_angular_accelc C s6 || _ || _|| _|| _|| _t | _t d¡| _ d S )Né* )
|
r/ r0 r1 r2 r3 r ÚstateÚrandomZRandomÚ_rng)r( r/ r0 r1 r2 r3 r r r Ú__init__5 s z DifferentialDriveModel.__init__r r r r Ú heading_degc C s t |||t |¡d| _d S )N)r r r r )r r Úradiansr5 )r( r r r r9 r r r ÚresetE s z DifferentialDriveModel.resetÚ target_linearÚtarget_angularÚdtr r c C sB |dkr| j S t|| j | j}t|| j | j}|| j j }| j| }t|| |}|| }| j j|7 _| }|| j j }| j| } t|| | }|| }
|
| j j|7 _t| j j | j j| | j _ t
|
| j j ¡} t
|
| j j ¡} | j j | j j| | 7 _ | j j| j j| | 7 _| j jd7 _| j j| | j _| j j| | j _d| j _| j j| j j } |t }| t }d}|| j dd¡ || j dd¡ || j dd¡ f| j _t
|
t
|
|d¡¡| j _t
|
t
|
| d¡¡| j _| j dd¡| j dd¡t
|
| j j¡ | j dd¡ f| j _| j dd¡}| j j |7 _ | j S )Nr r! gü©ñÒMb`?gü©ñÒMb@?g ð?g{®Gáz?g¹?) r5 r r0 r2 r r1 r r3 r r r ÚcosÚsinr r r r r r ÚGRAVITYr7 Zgaussr" ÚdegreesÚatan2r r r# r$ )r( r< r= r> ZdvZmax_dvZ
|
linear_accZtarget_angular_mathZdwZmax_dwZ angular_accZcos_hZsin_hZ lateral_accZax_gZay_gZaz_gZ
|
temp_driftr r r ÚstepH sR
|
|
ý ýzDifferentialDriveModel.stepN)
|
r/ r r0 r r1 r r2 r r3 r )r r r r )r r r r r r r9 r )r< r r= r r> r r r ) r* r+ r, Ú__doc__r r: r8 r; rD r r r r r. 2 s ú r. )r r r r r r r r )r r r r )rE Ú
|
__future__r r& r r6 Útypingr rA r r Ú dataclassr r. r r r r Ú<module> s
|
|
|
|
