Lawnmower_STM32H7.git

o  
6ÖiùÂã@sdZddlmZmZmZmZmZmZddlZddZ    GdddZ
edkrddlZddgd    dgd
dggZ e
e Ze dd¡ed D]FZe ¡ZeeeeddejZejeddejdZejdeeedZejdeeedZe eefe¡qBdSdS)u
MowerController
 
æ§å¶å¨åè®¾ä¸å¥çº¦ï¼
- åæ ç³»ï¼XY å¹³é¢åä½ä¸ºç±³ï¼RTK æä¾ï¼ï¼Z å¿½ç¥ç¨äºå¹³é¢è·è¸ªã
- èªåï¼headingï¼ï¼å¼§åº¦ï¼èå´ä»»æï¼æ§å¶å¨ä¼å¤çç¯ç»ã
- GPS é¢çï¼ä½ç½®/éåº¦/heading/attitudeï¼çº¦ 10Hzï¼IMUï¼å éåº¦ãè§éåº¦ï¼100Hzã
- æ§å¶è¾åºé¢çï¼74Hzï¼è¿åä¸¤ä¸ªä¿¡å·ï¼forwardï¼-100..100ï¼å¯¹åº -1..+1 m/sï¼å turnï¼-100..100ï¼å¯¹åº -pi/2..+pi/2 rad/sï¼æ£ä¸ºå·¦è½¬ï¼ã
- è·¯å¾æä»¶ï¼JSON åè¡¨ [[x,y],[x,y],...]
 
è®¾è®¡è¯´æï¼
- ä½¿ç¨çº¯è¿½è¸ªï¼pure pursuitï¼æ¹æ³éååè§ç¹ï¼å¹¶åºäºè§åº¦è¯¯å·®ä½¿ç¨ P æ§å¶è®¡ç®ææåèªéçï¼rad/sï¼ã
- åè¿éåº¦ä¸åèªè¯¯å·®è¦åï¼åèªè¯¯å·®å¤§æ¶éä½åè¿éåº¦ä»¥ä¿è¯å¯è½¬å¼¯è½åã
 
æ¥å£ï¼
- controller = MowerController(path_points, params)
- controller.update_gps(pos_xy, heading, vel_xy, timestamp)
- controller.update_imu(accel3, gyro3, timestamp)
- controller.compute_control(timestamp) -> {'forward': int, 'turn': int, 'info': {...}}
 
é)Úatan2ÚhypotÚsinÚcosÚpiÚacosNcCs@|tkr|dt8}|tks|tkr|dt7}|tks|S)Né)r)Úa©r
ú=D:\Users\zhyin\PycharmProjects\Lowanlower\mower_controller.pyÚ
wrap_anglesÿ
 
ÿrc@steZdZdddZdddZdddZdd    Zd
dZdd ZddZ    ddZ
dd ZddZ    ddZdddZdS)ÚMowerControllerNcCsvdd|D}t|dkrtd||_|pi}| dd¡}g}tt|dD]Q}||\}}    ||d\}
}| ||    f¡|
|}||    } t|| }||kry|dkryt||}td|dD]}||d}| ||||    | |f¡qaq(| |d    ¡||_t|j|_    g|_
d
g|_t|j    dD],}|j|\}}|j|d\}}t||||}|j
 |¡|j |jd    |¡q|jrÍ|jd    nd
|_d
|_ |pÖi}| dd¡|_| dd ¡|_| dtd¡|_| dd ¡|_d
|_d
|_| dd¡|_| dd¡|_| dd¡|_| dd ¡|_| dd¡|_| dd¡|_| dd¡|_| dd¡|_| d d!¡|_| d"d¡|_| d#d$¡|_| d%d&¡|_ | d'd&¡|_!| d(d¡|_"| d)d¡|_#d
|_$| d*d+¡|_%d
|_&d|_'d
|_(d
|_)d
|_*d|_+| d,d-¡|_,| d.d-¡|_-d|_.| d/d¡|_/d0|_0d|_1d|_2d1|_3d|_4d|_5d2|_6d3|_7|j6|_8| d4d5¡|_9| d6d-¡|_:| d7d¡|_;| d8d¡|_<| d9d¡|_=| d:td;¡|_>| d<d=¡|_?d>|_@| d?d¡|_A| d@d$¡|_B| dAdB¡|_Cd|_Dd|_Ed>|_FdC|_Gd>|_Hd>|_Id|_J| dDd5¡|_K| dEd$¡|_L| dFdG¡|_MtN|_OtPdHdI|_Q|jQ RdJ¡dS)KNcSsg|]}t|qSr
)Útuple)Ú.0Úpr
r
rÚ
<listcomp>'sz,MowerController.__init__.<locals>.<listcomp>rzpath_points cannot be emptyZmax_segment_lengthç333333ã?éçíµ ÷Æ°>éÿÿÿÿçÚmax_forward_mpsÚmax_reverse_mpsçÉ?Úmax_yawrateéÚaccelÚ    k_headinggÍÌÌÌÌÌü?Úk_heading_dgÐ?Úk_xteç333333ó?Úk_curvÚ
base_speedçà?Úlookahead_timeç@Ú lookahead_mingffffffÖ?Ú lookahead_maxçø?Úgoal_toleranceç333333Ó?Ú    slow_zoneÚfinal_approach_distçé?Úmax_xteçð?Úxte_gain_scaleÚcurv_gain_scaleÚheading_err_gain_scaleÚ yawrate_alphagÍÌÌÌÌÌì?Úk_heading_ig{®Gáz?Úk_xte_iÚlarge_err_thresholdé
)rrZ
goto_startZfollow_pathÚstart_toleranceg¸ëQ¸¾?Ústart_heading_toleranceÚstart_approach_distÚstart_slow_speedÚstart_min_speedÚstart_turn_first_headingé2Úmin_follow_speedg{®Gáz´?FÚ recovery_distÚrecovery_exit_distÚrecovery_speedgá?ÚrotateÚwaypoint_turn_toleranceÚwaypoint_approach_distÚwaypoint_turn_heading_tolç¹?zcontroller_log.csvÚwzgt,x,y,heading,target_x,target_y,target_dist,target_idx,xte,heading_err,forward,turn,speed,stage,status
)SÚlenÚ
ValueErrorÚ original_pathÚgetÚrangeÚappendrÚintÚpathÚnZsegment_lengthsZcum_lengthsZtotal_lengthZ last_progressrrrrrZcurrent_speed_targetÚ current_speedrrrr!r"r$r&r'r)r+r,r.r0r1r2Úyawrate_filterr3Úlast_heading_errÚ    last_timeZ    last_curvÚheading_err_sumÚxte_sumÚmax_sumr4r5Úconsecutive_large_errr6Úmax_large_err_countÚposÚheadingÚvelÚ    imu_accelÚimu_gyroÚSTAGE_GOTO_STARTÚSTAGE_FOLLOW_PATHÚstager8r9r:r;r<r=r?Úin_recoveryr@rArBÚcurrent_target_idxÚnearest_path_idxÚfinishedÚ goto_substageÚwaypoint_approach_modeÚwaypoint_turn_modeÚwaypoint_turn_target_idxrDrErFÚsetÚwaypoint_turnedÚopenÚlog_fileÚwrite)ÚselfZpath_pointsÚparamsrKZp_paramsZmax_seg_lenZ    densifiedÚiÚx1Úy1Úx2Úy2ÚdxÚdyZseg_lenÚstepsÚkÚratioÚaxÚayÚbxÚbyÚLrr
r
rÚ__init__$sÈ
 zMowerController.__init__cCs.t||_t||_|durt||_dSdS)z¤Update GPS-like measurements at ~10Hz.
 
        pos_xy: (x,y) in meters
        heading: radians
        vel_xy: (vx, vy) in m/s in global frame (optional)
        N)rr[Úfloatr\r])rpZpos_xyr\Zvel_xyÚ    timestampr
r
rÚ
update_gps¹s
 
 
ÿzMowerController.update_gpscCst||_t||_dS)z:Update IMU (100Hz). accel3 and gyro3 are 3-element tuples.N)rr^r_)rpZaccel3Zgyro3rr
r
rÚ
update_imuÅs
zMowerController.update_imucCsø|\}}|\}}|\}}    ||}
|    |}|
|
||}|dkr)t||||S|||
||||} | dkrDt||||S| dkrQt||||    S|| |
}|| |}t||||}||||||
dkrvdnd}||S)zDCalculate cross track error (perpendicular distance to line segment)rrr)r)rpÚp1Úp2r[ÚxÚyrsrtrurvrwrxÚl2ÚtÚpxÚpyÚxteÚsignr
r
rÚ    _calc_xteÊs$$zMowerController._calc_xtecCs´|dks |t|jdkrdS|j|d}|j|}|j|d}|d|d}|d|d}|d|d}|d|d}t||}    t||}
tt|
|    }|tdkS)u
        æ£æµè·¯å¾ç¹idxæ¯å¦æ¯æç¹ï¼éè¦åå°è½¬åï¼
        
        æç¹å®ä¹ï¼ååä¸¤æ®µè·¯å¾æ¹åååè¶è¿45åº¦
        rrFr)rIrPrÚabsrr)rpÚidxÚp_prevÚp_currÚp_nextÚdx1Údy1Údx2Údy2Úheading1Úheading2Ú
angle_diffr
r
rÚ_is_waypoint_turn_neededçs
 
 
z(MowerController._is_waypoint_turn_neededcCs|jdurdS|j\}}d}td}tdt|jdD]b}|j|d}|j|}|j|d}|d|d}    |d|d}
|d|d}|d|d}t|
|    } t||}tt|| }|tdkr~t    |d||d|}||kr~|}|}q||fS)u
        å¨åå§è·¯å¾ä¸æ¾å°ææ¥è¿å½åä½ç½®çæç¹
        
        Returns: (waypoint_idx, distance) æ (None, None)
        N©NNÚinfrrr)
r[rrMrIrKrrrrr)rprrZnearest_waypoint_idxÚnearest_distrrrrrrrrrrrrÚdistr
r
rÚ_find_nearest_original_waypoints.
 
 
 
 
z/MowerController._find_nearest_original_waypointcCs²|t|jdkr4|dkr2|j|d|j|dd}|j|d|j|dd}t||SdS|j|dd|j|d}|j|dd|j|d}t||S)uBè·ååå§è·¯å¾ç¹idxå¤çç®æ èªåï¼ä¸ä¸æ®µçæ¹åï¼rrr©rIrKr)rprrwrxr
r
rÚ_get_target_heading_at_waypoint*s  
  
z/MowerController._get_target_heading_at_waypointcCs`|jdur|jddddfS|j\}}|j|j\}}t||||}|dkr0d}t|j}ntd|jd}tt|j|jd}|j}    td}
t||D]}|j|\}}t||||}||
kri|}
|}    qO|    |_|    } t|    t|jD]}|j|\}}t||||}||kr|} nqwt|jd} d}d|    kr«t|jdkr'nnz|j|    d}|j|    }|j|    d}|d|d|d|df}|d|d|d|df}t|d|d}t|d|d}|d    kr'|d    kr'|d|d|d|d}td
td|||}t    t
t|}|t|d    }|j| || |    fS)u
        ç®åçPure Pursuitåè§ç¹æ¥æ¾ - åªä½¿ç¨ç¹ç´¢å¼
        
        ä»nearest_path_idxå¼å§ååæç´¢ï¼æ¾å°ç¬¬ä¸ä¸ªè·ç¦»>=lookahead_distçç¹
        
        Returns: (lookahead_xy, curvature, lookahead_idx, nearest_idx)
        Nrrr/éérrrçð¿)r[rPrerrIÚmaxÚminrrMrrr)rpZlookahead_distrrrrZcurrent_nearest_distZsearch_startZ
search_endÚnearest_idxr rrÚdÚ lookahead_idxZ    curvaturerrZp3Úv1Úv2Úl1rÚdotÚ    cos_angleÚangle_changer
r
rÚ_find_lookahead_point9sX
 
þ$
   z%MowerController._find_lookahead_pointcCst|drt|jdkrdtdfS|j\}}tdt|jdD]f}||jvr)q!|j|d}|j|}|j|d}|d|d}|d|d}|d|d}    |d|d}
t||}t|
|    }tt    ||} | t
dkrt|d||d|}||fSq!dtdfS)uÈ
        å¨åå§è·¯å¾ä¸æ¥æ¾ä¸ä¸ä¸ªæªå°è¾¾çæç¹
        æç§è·¯å¾é¡ºåºæ¥æ¾ï¼ä¸è·³è¿
        
        Returns: (waypoint_idx, distance) æ (None, inf) å¦ææ²¡ææ¾å°
        rKéNrrrr)ÚhasattrrIrKrr[rMrlrrrrr)rprrrrÚprev_ptÚcurr_ptÚnext_ptrrrrZangle1Zangle2r²r¡r
r
rr¢s*
 
 
 
 
ýcCs¦|durdS|t|jdkr2|dkr0|j|d}|j|}t|d|d|d|dSdS|j|}|j|d}|d|d}|d|d}t||S)uÔ
        è·åæç¹å¤çç®æ èªåï¼ä¸ä¸æ®µè·¯å¾çæ¹åï¼
        
        Args:
            waypoint_idx: æç¹å¨åå§è·¯å¾ä¸çç´¢å¼
        
        Returns: ç®æ èªåï¼å¼§åº¦ï¼
        Nrrrr£)rpÚwaypoint_idxr¶r·r¸rwrxr
r
rr¤«s    
"
 
cCs|\}}|j}|j|\}}|t|jdkry|j|d\}}||}    ||}
|    |    |
|
}|dkry|||    |||
|}tdtd|}|||    } |||
}|| }||}t||}||
||    }|dkrq|n|}|| |fSt||||}|||fS)uo
        ç®åçæ¨ªåè¯¯å·®è®¡ç® - åºäºæè¿ç¹
        
        Returns: (xte, proj_x, proj_y)
        rgÖ&è.>rr/r)rerPrIr¨r©r)rpr[rrrªrrÚnxÚnyrwrxZseg_len2rÚproj_xÚproj_yZ    xte_vec_xZ    xte_vec_yÚxte_magÚcrossrr
r
rÚ_compute_xte_simpleÈs*
 
 
z#MowerController._compute_xte_simplecQ CsR|jrddddidS|jdus|jdurddddidS|dur%t ¡}|jdur-d}n    |r4||jnd}|r:|n    |jrB|j|nd|_|j\}}|j|jkr|jd}|d|}|d    |}t||}t    ||}    t
|    |j}
t|d
r}|jdurd|_|jdkrt |
|jkrd|_np|j|
d }tt||jd}tdtd|}d} |\}}t||||}d}|j |dd|dd|dd|jdd|dd|dd|dd|d|
dd| d|d|jd¡|j ¡| |d|j|
|ddS|jdkræ|    }t
||j}||jkrd|_nÌ||jkr7||j}dd|}|j|j|j|}n
d}t|jdd}t |t}ddd|}t|jd|||}t||jd}|j|}tt||jd} tdtd| } tt||jd}tdtd|}|\}}t||||}d}|j |dd|dd|dd|jdd|dd|dd|dd|d|dd| d|d|dd|jd¡|j ¡| |d|j||d dS|jdkrt|jd    kr|jd    d|jdd}|jd    d    |jdd    }t    ||}nd}t
||j}||jkr t ||j kr |j!|_d|_d|_"|j|_#tt|j|jd}|jd\}}t||||}d}|j |dd|dd|dd|jdd|dd|dd|dd|d|dd|d!|jdd|jd"¡|j ¡|dd#|j|j"d$dS|dkr§d    nd%}|t|jdtd&t |d}tt||jd}tdtd|}|\}}t||||}d}|j |dd|dd|dd|jdd|dd|dd|dd|d|dd!|dddd|jd'¡|j ¡d|d(|d)dS|jdur(d}n
|r0||jnd}|r7|n
|jr@|j|nd|_|j\}}| $|j¡\} }!}"t|jd%d||jd%d    |}#|j%sÅ|j&sÅ| '¡\}$}%|$durÅ|%d*krÅ|$|j(vrÅd+|_%|$|_)|j |dd|dd|dd|jdd|j*|$ddd|j*|$d    dd|%dd|$d| dd,|jd-¡|j ¡|j%r×|j*|j)\}&}'|&|}|'|}t||}%|%|j+krd.|_%d+|_&|j |dd|dd|dd|jdd|&dd|'dd|%dd|j)d| dd,|jd/¡|j ¡n¸t    ||}(t
|(|j})|%dkr>|%d}*|jdd|*}n|jd0}t |)t}ddd|}||}t|jd|}|j|)d }tt||jd} tdtd| } tt||jd}tdtd|}|j |dd|dd|dd|jdd|&dd|'dd|%dd|j)d| dd|)dd| d|d|dd|jd1¡|j ¡| |d2|%|)|d3dS|j&r¢| ,|j)¡}+t
|+|j}t ||j-kr;|j( .|j)¡d.|_&|j |dd|dd|dd|jdd|j*|j)ddd|j*|j)d    dd|j)d| dd|dd4|jd5¡|j ¡ng|j|d}tt||jd}tdtd|}|j |dd|dd|dd|jdd|j*|j)ddd|j*|j)d    dd|j)d| dd|dd!|d|jd6¡|j ¡d|d7||+d8dSt |j#},t | }-|j/|j0|,d9t|-d}.t|j/t|j1|.}/t | }0|j2s    |0|j3kr    d+|_2|j |dd|dd|dd|jdd|!dd|"dd|0dd|j4d| dd,|jd:¡|j ¡|j2rJ|0|j5krJd.|_2|j |dd|dd|dd|jdd|!dd|"dd|0dd|j4d| dd,|jd;¡|j ¡| 6|/¡\\}1}2}3}4}5|4|_"t    |2||1|}6td<td| |j7}7|j8|7|j9}8t
|6|8}t
||j}|dkr||j:|nd}9||_:|j;||7_;|j<| |7_<t|j=t|j=|j;|_;t|j=t|j=|j<|_<|j||j>|9|j?|j;|j@|j<}ddt |3|jA}:t |};dd|;|jB}t | }<dd|<|j9}=|:||=d=}>t|:||=}?d|>d>|?}@|j|@}|jC|d    |jC|jD|_D|j}At|At|A|jD}|j2rft    |"||!|}6td<td| |j7}7|j8|7|j9}8t
|6|8}t
||j}|j|}|jE}|#|jFk}B|#|jGk}Ct | |jHdkst |td?kr|jId    7_I|jI|jJkrd|_Id|_;d|_<nd|_I|j2sÜ|BrÁtd9td|#|jF}D|Cr½|Dtd|#|jG9}D||D9}|CrÜ|d9}t | d@ksØt |tdAkrÜ|d09}|j|j!krJ|j2sJ|Brí|jKn|jKd}Et |dBkrJ||EkrJ|1|2}}t||||}|j |dd|dd|dd|jdd|dd|dd|dd|j"d| dd|ddC|Edd|jdD¡|j ¡|E}|jL|BrQdnd}F||j#}G|F|}H|j#t|Ht|H|G7_#|j|jkrt |dEkr|jMd|_#n"t |dEkr|j2s|jMd|_#|j#dkr£t |d?kr£d|_#|#|jNkr´d+|_ddddidS|#|jNdkrÞ|j"|jOdFkrÞt |d9krÞt | d>krÞd+|_ddddidS|durå|nt ¡}I|j#}J|1|2}}t||||}|jdk    rtt|J|jdnd}Ktdtd|K}K|jdk    r&tt||jdnd}Ltdtd|L}L|j |Idd|dd|dd|jdd|dd|dd|dd|j"d| dd|dd|Kd|Ld|Jdd|jdG¡|j ¡|jdk    rtt|j#|jdnd} tdtd| } | dk
r|j
st |dBk
r|jdk    r°tt|jK|jdnd}M|Mdk
r|1|2}}t||||}|j |Idd|dd|dd|jdd|dd|dd|dd|j"d| dd|dd|Mdtt||jdd|jKdd|jdH¡|j ¡|M} |jK|_#|jdk
r(tt||jdnd}tdtd|}|j
r8dndI}N|N||f|j|||j#||1|2f|3|#dJ
}O|I}P|1|2}}t||||}|j |Pdd|dd|dd|jdd|dd|dd|dd|j"d| dd|dd| d|d|j#dd|jd|NdK¡|j ¡| ||OdS)LzCompute forward and turn outputs mapped to [-100,100].
        
        Returns dict with keys: forward(int), turn(int), info(dict)
        rÚstatusrf)ÚforwardÚturnÚinfoNZno_stategyé&1¬?rrrgrCÚmovegÍÌÌÌÌÌô?édiÿÿÿz.3fú,z,0.000,z,rotate_to_start
Zrotate_to_start)rÁrbÚheading_errÚ dist_to_startÚaligngÙ?rr/r-r#r(r z,moving_to_start
Zmoving_to_start)rÁrbrÉrÈz,0,z,reached_start
Z reached_start)rÁrbZadvanced_idxrg©?z,align_at_start
Zalign_at_start)rÁrÈr%Tz,0.000,0,0,0.000,z,enter_waypoint_approach
Fz,reached_waypoint
gffffffæ?z,approaching_waypoint
Zapproaching_waypoint)rÁÚ waypoint_distrÈÚtarget_speedz,0,0,0.000,z,exit_waypoint_turn
z,waypoint_turning
Zwaypoint_turning)rÁrÈÚtarget_headingr*z,enter_recovery
z,exit_recovery
r§gUUUUUUÕ?rg@g333333Ã?r7g@z,0,0,z,enforce_min_speed
g333333@rz    ,pre_map
z,force_min_forward
Zrunning)
rÁr[r\Údesired_headingrÈrRÚyawrate_cmdZ    target_xyÚpath_curvatureÚ
final_distÚ
)Prfr[r\ÚtimerUrbr`rPrrrrµrgrr=rrOÚroundrr¨r©rnroÚflushr8r:r;r"rr<rrIr9rardrRrÀrhrir¢rlrjrKrDr¤rFÚaddr&r$r'rcr@rerAr³r.rr0rTrVrWrXrr4r5r1r2r3rSrBr+r,r6rYrZr?rrr)rQ)QrprÚdtrrZstart_pointrwrxrÉZdesired_heading_to_startZheading_err_to_startrÏZturn_signalZforward_signalZtarget_xZtarget_yZtarget_distZ
target_idxrÎrÈZ
dist_ratioZspeed_scaleZbase_target_speedZ heading_ratioZ    err_scalerÌZpath_dxZpath_dyZdesired_start_headingZheading_err_finalÚf_sigrrr¼r½rÑr¹rËZ
waypoint_xZ
waypoint_yZdesired_heading_to_waypointZheading_err_to_waypointZspeed_ratiorÍZactual_speedr¾Zbase_lookaheadZ    lookaheadZdist_to_pathÚlxÚlyrÐr¬rªZpath_headingZxte_normalizedZxte_correctionZ heading_err_dZ
curv_scaleZheading_err_absZxte_absZ    xte_scaleZ
geom_scaleZ    min_scaleZblend_scalerZin_slow_zoneZin_final_approachZ
dist_scaleZdynamic_minrZspeed_changeZ
max_changeZlog_tZ pre_map_speedZpre_map_forwardZpre_map_turnZ forced_signalrÁrÄrr
r
rÚcompute_controlôs>
 
 
 
 
(ÿÿÿÿþþþ
þ
 
þ
 
(ÿÿÿÿþþþþ
þ
 
þ(ÿÿÿÿþþþ
þ
"(ÿÿÿÿþþþ
þ
"
&
 
(ÿþýýü
ü
 
(ÿÿþþý
ý
 
 
(ÿÿþþýýýýý
ý
 
ü(ÿþýüü
ü(ÿþýüüü
ü
 
ý
(ÿÿÿÿþ
þ
(ÿÿÿÿþ
þ
ÿ
þ
ý
ÿ
 
 
& 
(ÿÿÿÿþþý
ý
 

&&(ÿÿÿÿþþþþþ
ý
( (
 
(ÿÿÿÿþþþþý
ý
&ö
(ÿÿÿÿþþþþþý
ý
zMowerController.compute_control)Nr) Ú__name__Ú
__module__Ú__qualname__rrrrrr¢r¤r³rÀrÛr
r
r
rr #s

 
&H*,r Ú__main__r¥r7)rr§rérÂgY@rÃrGr)Ú__doc__ÚmathrrrrrrrÓrr rÜÚjsonrPÚcrrMrrrÛÚoutÚprintrÚfr\rZhdr[rrr
r
r
rÚ<module>s6     K
ò