btl143/QDAC-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

增加弯道状态

Browse Source
This commit is contained in:
Glz
2024-03-26 21:39:39 +08:00
parent cbea30033a
commit f5654ba679
3 changed files with 34 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
app/gl_get_corners.c
View File

@@ -29,6 +29,11 @@ void get_corners() {
if(is_straight0){
state_type = STRAIGHT_STATE;
}
if (is_straight0 == false && Lpt0_found == false)
{
is_turn0 = is_curve(angle_left ,clip(angle_left_num - 10, 0,angle_left_num),0.05);
}
Lpt0_found_barrier = Lpt1_found_barrier = false;
@@ -77,6 +82,10 @@ void get_corners() {
if(is_straight1){
state_type = STRAIGHT_STATE;
}
if (is_straight1 == false && Lpt1_found == false)
{
is_turn1 = is_curve(angle_right ,clip(angle_right_num - 10, 0,angle_right_num),0.05);
}
for (int i = 0; i < pts_resample_right_count; i++) {
if (angle_new_right_barrier[i] == 0) continue;
@@ -98,6 +107,12 @@ void get_corners() {
Lpt1_found_barrier_in_id = Lpt1_rpts1s_id_barrier;
}
}
if (is_turn0 && is_turn1)
{
state_type = TURN_STATE;
}
}