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

避障

Browse Source
This commit is contained in:
井明江
2024-07-03 16:30:37 +08:00
parent 047a907eac
commit a94db790a6
4 changed files with 97 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
app/gl_barrier.c
View File

@@ -2,7 +2,7 @@
#include "gl_headfile.h"
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;
float (*mid_track_s)[2];
int32_t mid_track_count_s;
@@ -24,32 +24,27 @@ void RunBarrier()
{
if (barrier_type == BARRIER_LEFT_BEGIN) {
track_type = TRACK_RIGHT;
if (barrier_begin_flag == 0)
{
if (barrier_begin_flag == 0) {
timer_clear(TIM_3);
timer_start(TIM_3);
barrier_begin_flag = 1;
}
if (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 >= 1000 && time_barrier < 2000)
{
if (time_barrier >= 300 && time_barrier < 600) {
barrier_type = BARRIER_LEFT_RUNNING;
} else if (time_barrier >= 600) {
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
else if (time_barrier >= 2000)
{
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
}
}
// if (Lpt0_found) {
// Lpt0_found_count++;
// }
@@ -69,35 +64,29 @@ void RunBarrier()
// timer_start(TIM_3);
// timer_clear(TIM_3);
// }
}
if (barrier_type == BARRIER_RIGHT_BEGIN) {
track_type = TRACK_LEFT;
if (barrier_begin_flag == 0)
{
if (barrier_begin_flag == 0) {
timer_clear(TIM_3);
timer_start(TIM_3);
barrier_begin_flag = 1;
}
if (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 >= 1000)
{
if (time_barrier >= 300 && time_barrier < 600) {
barrier_type = BARRIER_RIGHT_RUNNING;
if (time_barrier >= 2000)
{
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
} else if (time_barrier >= 600) {
barrier_type = BARRIER_NONE;
barrier_begin_flag = 0;
timer_stop(TIM_3);
timer_clear(TIM_3);
}
}
}
// if (Lpt1_found) {
// Lpt1_found_count++;
@@ -118,11 +107,10 @@ void RunBarrier()
// barrier_type = BARRIER_NONE;
// track_type = TRACK_LEFT;
// }
}
}
void Check_s(){
void Check_s()
{
if (track_type == TRACK_LEFT) {
mid_track_s = mid_left;
mid_track_count_s = mid_left_count;
@@ -134,35 +122,26 @@ void Check_s(){
int min_s = 1000;
int max_s = 1;
for (int i = 0; i < mid_track_count_s; i++)
{
if (mid_track_s[i][1] < min_s)
{
min_s = mid_track_s[i][1];
for (int i = 0; i < mid_track_count_s; i++) {
if (mid_track_s[i][1] < min_s) {
min_s = mid_track_s[i][1];
temp_min_s = min_s;
}
if (mid_track_s[i][1] > max_s)
{
max_s = mid_track_s[i][1];
if (mid_track_s[i][1] > max_s) {
max_s = mid_track_s[i][1];
temp_max_s = max_s;
}
}
if (is_straight0 == false && is_straight1 == false && (max_s - min_s) <= 30 && (max_s - min_s) >= 5)
{
if (is_straight0 == false && is_straight1 == false && (max_s - min_s) <= 30 && (max_s - min_s) >= 5) {
s_type = S_BEGIN;
}
}
void RunS()
{
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;
}
}