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-07-05 14:12:01 +08:00
parent 7facae7a1a
commit 5d56543f64
5 changed files with 135 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

181
app/gl_barrier.c
View File

@@ -3,110 +3,113 @@
enum barrier_type_e barrier_type = BARRIER_NONE; enum barrier_type_e barrier_type = BARRIER_NONE;
enum s_type_e s_type = S_NONE; enum s_type_e s_type = S_NONE;
uint16 time_barrier; enum shiled_type_e shiled_type = SHILED_NONE;
float (*mid_track_s)[2]; float (*mid_track_s)[2];
int32_t mid_track_count_s; int32_t mid_track_count_s;
int temp_min_s; int temp_min_s;
int temp_max_s; int temp_max_s;
int barrier_begin_flag = 0; int barrier_begin_flag = 0;
int barrier_count = 0;
void CheckBarrier() void CheckBarrier()
{ {
if (barrier_type == BARRIER_NONE && Lpt0_found_barrier_in && !Lpt1_found_barrier_in && is_straight1) { if (barrier_type == BARRIER_NONE && Lpt0_found_barrier_in && !Lpt1_found_barrier_in && is_straight1) {
barrier_type = BARRIER_LEFT_BEGIN; barrier_type = BARRIER_LEFT_BEGIN;
} }
if (barrier_type == BARRIER_NONE && !Lpt0_found_barrier_in && Lpt1_found_barrier_in && is_straight0) { if (barrier_type == BARRIER_NONE && !Lpt0_found_barrier_in && Lpt1_found_barrier_in && is_straight0) {
barrier_type = BARRIER_RIGHT_BEGIN; barrier_type = BARRIER_RIGHT_BEGIN;
} }
} }
void RunBarrier() void RunBarrier()
{ {
if (barrier_type == BARRIER_LEFT_BEGIN) { if (barrier_type == BARRIER_LEFT_BEGIN) {
track_type = TRACK_RIGHT; track_type = TRACK_RIGHT;
if (barrier_begin_flag == 0) { // if (barrier_begin_flag == 0) {
timer_clear(TIM_3); // timer_clear(TIM_3);
timer_start(TIM_3); // timer_start(TIM_3);
barrier_begin_flag = 1; // barrier_begin_flag = 1;
// }
// }
// if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
// if (barrier_begin_flag == 1) {
// barrier_type = BARRIER_LEFT_BEGIN;
// time_barrier = timer_get(TIM_3);
// if (time_barrier >= 300 && time_barrier < 600) {
// barrier_type = BARRIER_LEFT_RUNNING;
// } else if (time_barrier >= 600) {
// barrier_type = BARRIER_NONE;
// barrier_count = 1;
// barrier_begin_flag = 0;
// timer_stop(TIM_3);
// timer_clear(TIM_3);
// }
// }
// }
if (Lpt0_found) {
Lpt0_found_count++;
} }
} if (Lpt0_found_count >= 1 && barrier_type == BARRIER_LEFT_BEGIN) {
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) { Lpt0_found_count = 0;
if (barrier_begin_flag == 1) { barrier_type = BARRIER_LEFT_RUNNING;
barrier_type = BARRIER_LEFT_BEGIN; track_type = TRACK_RIGHT;
time_barrier = timer_get(TIM_3); timer_clear(TIM_1);
if (time_barrier >= 300 && time_barrier < 600) { timer_start(TIM_1);
barrier_type = BARRIER_LEFT_RUNNING; } else if (barrier_type == BARRIER_LEFT_RUNNING) {
} else if (time_barrier >= 600) { track_type = TRACK_RIGHT;
barrier_type = BARRIER_NONE; if (timer_get(TIM_1) >= 220) {
barrier_begin_flag = 0; barrier_type = BARRIER_NONE;
timer_stop(TIM_3); track_type = TRACK_RIGHT;
timer_clear(TIM_3); timer_stop(TIM_1);
timer_clear(TIM_1);
} }
} }
} }
// if (Lpt0_found) {
// Lpt0_found_count++;
// }
// if (Lpt0_found_count >= 1) {
// Lpt0_found_count = 0;
// barrier_type = BARRIER_LEFT_RUNNING;
// track_type = TRACK_RIGHT;
// timer_clear(TIM_3);
// timer_start(TIM_3);
// }
// } else if (barrier_type == BARRIER_LEFT_RUNNING) {
// track_type = TRACK_RIGHT;
// time_barrier = timer_get(TIM_3);
// if (time_barrier >= 220) {
// barrier_type = BARRIER_NONE;
// track_type = TRACK_RIGHT;
// timer_start(TIM_3);
// timer_clear(TIM_3);
// }
if (barrier_type == BARRIER_RIGHT_BEGIN) { if (barrier_type == BARRIER_RIGHT_BEGIN) {
track_type = TRACK_LEFT; track_type = TRACK_LEFT;
if (barrier_begin_flag == 0) { // if (barrier_begin_flag == 0) {
timer_clear(TIM_3); // timer_clear(TIM_3);
timer_start(TIM_3); // timer_start(TIM_3);
barrier_begin_flag = 1; // barrier_begin_flag = 1;
// }
// }
// if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
// if (barrier_begin_flag == 1) {
// barrier_type = BARRIER_RIGHT_BEGIN;
// time_barrier = timer_get(TIM_3);
// if (time_barrier >= 300 && time_barrier < 600) {
// barrier_type = BARRIER_RIGHT_RUNNING;
// } else if (time_barrier >= 600) {
// barrier_type = BARRIER_NONE;
// barrier_count = 1;
// barrier_begin_flag = 0;
// timer_stop(TIM_3);
// timer_clear(TIM_3);
// }
// }
// }
if (Lpt1_found) {
Lpt1_found_count++;
} }
} if (Lpt1_found_count >= 1 && barrier_type == BARRIER_RIGHT_BEGIN) {
if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) { Lpt1_found_count = 0;
if (barrier_begin_flag == 1) { barrier_type = BARRIER_RIGHT_RUNNING;
barrier_type = BARRIER_RIGHT_BEGIN; track_type = TRACK_LEFT;
time_barrier = timer_get(TIM_3); timer_clear(TIM_1);
if (time_barrier >= 300 && time_barrier < 600) { timer_start(TIM_1);
barrier_type = BARRIER_RIGHT_RUNNING; } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
} else if (time_barrier >= 600) { track_type = TRACK_LEFT;
barrier_type = BARRIER_NONE; if (timer_get(TIM_1) >= 220) {
barrier_begin_flag = 0; timer_stop(TIM_1);
timer_stop(TIM_3); timer_clear(TIM_1);
timer_clear(TIM_3); barrier_type = BARRIER_NONE;
track_type = TRACK_LEFT;
} }
} }
} }
// if (Lpt1_found) {
// Lpt1_found_count++;
// }
// if (Lpt1_found_count >= 1) {
// Lpt1_found_count = 0;
// barrier_type = BARRIER_RIGHT_RUNNING;
// track_type = TRACK_LEFT;
// timer_clear(TIM_3);
// timer_start(TIM_3);
// }
// } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
// track_type = TRACK_LEFT;
// time_barrier = timer_get(TIM_3);
// if (time_barrier >= 220) {
// timer_start(TIM_3);
// timer_clear(TIM_3);
// barrier_type = BARRIER_NONE;
// track_type = TRACK_LEFT;
// }
} }
void Check_s() void Check_s()
@@ -144,4 +147,24 @@ void RunS()
if (temp_max_s - temp_min_s > 30 || (is_straight0 && is_straight1)) { if (temp_max_s - temp_min_s > 30 || (is_straight0 && is_straight1)) {
s_type = S_NONE; s_type = S_NONE;
} }
}
void Check_shiled(){
if (begin_flag == 1)
{
shiled_type = SHILED_BEGIN;
timer_clear(TIM_3);
timer_start(TIM_3);
begin_flag = 0;
}
if (timer_get(TIM_3) > 1000 && shiled_type == SHILED_BEGIN)
{
shiled_type = SHILED_NONE;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
} }

8
app/gl_barrier.h
View File

@@ -18,9 +18,14 @@ enum s_type_e {
S_OUT, S_OUT,
}; };
extern uint16 time_barrier; enum shiled_type_e {
SHILED_NONE,
SHILED_BEGIN,
};
extern enum barrier_type_e barrier_type; extern enum barrier_type_e barrier_type;
extern enum s_type_e s_type; extern enum s_type_e s_type;
extern enum shiled_type_e shiled_type;
extern float (*mid_track_s)[2]; extern float (*mid_track_s)[2];
extern int32_t mid_track_count_s; extern int32_t mid_track_count_s;
extern int temp_min_s; extern int temp_min_s;
@@ -29,6 +34,7 @@ extern int temp_max_s;
void CheckBarrier(void); void CheckBarrier(void);
void Check_s(void); void Check_s(void);
void RunBarrier(void); void RunBarrier(void);
void Check_shiled(void);
#endif /* COMMON_H_ */ #endif /* COMMON_H_ */

4
app/gl_data.c
View File

@@ -121,4 +121,6 @@ float aim_distance;
float aim_judge_far=0.3f; float aim_judge_far=0.3f;
float FIX_BINTHRESHOLD = 140; float FIX_BINTHRESHOLD = 140;
track_type_e track_type = TRACK_RIGHT; track_type_e track_type = TRACK_RIGHT;
int begin_flag = 0;

1
app/gl_data.h
View File

@@ -132,5 +132,6 @@ extern float aim_judge_far;
extern track_type_e track_type; extern track_type_e track_type;
extern float FIX_BINTHRESHOLD; extern float FIX_BINTHRESHOLD;
extern int begin_flag;
#endif #endif

42
app/gl_tracking.c
View File

@@ -112,23 +112,27 @@ void aim_distance_select(void)
void ElementJudge() void ElementJudge()
{ {
CheckGarage(); Check_shiled();
if (garage_type == GARAGE_NONE) { if (shiled_type == SHILED_NONE) {
CheckCross(); CheckGarage();
if (cross_type == CROSS_NONE) { if (garage_type == GARAGE_NONE) {
CheckBarrier(); CheckCross();
if (barrier_type == BARRIER_NONE) { if (cross_type == CROSS_NONE) {
CheckCircle(); CheckBarrier();
if (circle_type == CIRCLE_NONE) { if (barrier_type == BARRIER_NONE) {
Check_s(); CheckCircle();
if (circle_type == CIRCLE_NONE) {
Check_s();
}
} }
} }
} }
} }
if (garage_type != GARAGE_NONE) { if (garage_type != GARAGE_NONE) {
cross_type = CROSS_NONE; cross_type = CROSS_NONE;
circle_type = CIRCLE_NONE; circle_type = CIRCLE_NONE;
barrier_type = BARRIER_NONE;
} }
if (cross_type != CROSS_NONE) { if (cross_type != CROSS_NONE) {
circle_type = CIRCLE_NONE; circle_type = CIRCLE_NONE;
@@ -147,12 +151,10 @@ void ElementRun()
else if (cross_type != CROSS_NONE) { else if (cross_type != CROSS_NONE) {
RunCross(); RunCross();
}
else if (circle_type != CIRCLE_NONE) {
RunCircle();
} else if (barrier_type != BARRIER_NONE) { } else if (barrier_type != BARRIER_NONE) {
RunBarrier(); RunBarrier();
} else if (circle_type != CIRCLE_NONE) {
RunCircle();
} else if (s_type != S_NONE) { } else if (s_type != S_NONE) {
// RunS(); // RunS();
} }
@@ -274,17 +276,17 @@ void MidLineTrack()
switch (barrier_type) { switch (barrier_type) {
case BARRIER_LEFT_BEGIN: case BARRIER_LEFT_BEGIN:
// dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset; // dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -45.f; pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f - 20;
break; break;
case BARRIER_LEFT_RUNNING: case BARRIER_LEFT_RUNNING:
pure_angle = 25.f; pure_angle = 15.f;
break; break;
case BARRIER_RIGHT_BEGIN: case BARRIER_RIGHT_BEGIN:
pure_angle = 45.f; pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f + 20;
break; break;
case BARRIER_RIGHT_RUNNING: case BARRIER_RIGHT_RUNNING:
// dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset; // dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -25.f; pure_angle = -15.f;
default: default:
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f; // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
if (dy > 0) { if (dy > 0) {
@@ -326,7 +328,7 @@ void MidLineTrack()
// // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32; // // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
} }
if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_RIGHT_IN) { if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_LEFT_BEGIN || circle_type == CIRCLE_RIGHT_BEGIN) {
state_type = CIRCLE_STATE; state_type = CIRCLE_STATE;
timer_clear(TIM_3); timer_clear(TIM_3);
timer_start(TIM_3); timer_start(TIM_3);