244 lines
8.0 KiB
C
244 lines
8.0 KiB
C
#include "zf_common_headfile.h"
|
|
#include "gl_headfile.h"
|
|
|
|
enum barrier_type_e barrier_type = BARRIER_NONE;
|
|
enum s_type_e s_type = S_NONE;
|
|
enum shield_type_e shield_type = SHIELD_NONE;
|
|
float (*mid_track_s)[2];
|
|
int32_t mid_track_count_s;
|
|
int temp_min_s;
|
|
int temp_max_s;
|
|
int barrier_begin_flag = 0;
|
|
int barrier_count = 0;
|
|
float barrirer_time=500.f;
|
|
void CheckBarrier()
|
|
{
|
|
if (barrier_type == BARRIER_NONE && Lpt0_found_barrier_in && !Lpt1_found_barrier_in && is_straight1) {
|
|
barrier_type = BARRIER_LEFT_BEGIN;
|
|
}
|
|
if (barrier_type == BARRIER_NONE && !Lpt0_found_barrier_in && Lpt1_found_barrier_in && is_straight0) {
|
|
barrier_type = BARRIER_RIGHT_BEGIN;
|
|
}
|
|
}
|
|
|
|
void RunBarrier()
|
|
{
|
|
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
|
|
track_type = TRACK_RIGHT;
|
|
if (barrier_type == BARRIER_LEFT_BEGIN)
|
|
{
|
|
timer_clear(TIM_2);
|
|
timer_start(TIM_2);
|
|
barrier_type = BARRIER_LEFT_RUNNING;
|
|
}
|
|
if (barrier_type == BARRIER_LEFT_RUNNING)
|
|
{
|
|
if (timer_get(TIM_2) >= barrirer_time)
|
|
{
|
|
barrier_type = BARRIER_NONE;
|
|
timer_stop(TIM_2);
|
|
timer_clear(TIM_2);
|
|
}
|
|
else{
|
|
barrier_type = BARRIER_LEFT_RUNNING;
|
|
}
|
|
}
|
|
|
|
|
|
// if (barrier_begin_flag == 0) {
|
|
// timer_clear(TIM_3);
|
|
// timer_start(TIM_3);
|
|
// 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 (pts_resample_left_count >= 5) {
|
|
// if (Lpt0_found) {
|
|
// Lpt0_found_count++;
|
|
// }
|
|
// if (Lpt0_found_count >= 1 && barrier_type == BARRIER_LEFT_BEGIN) {
|
|
// Lpt0_found_count = 0;
|
|
// barrier_type = BARRIER_LEFT_RUNNING;
|
|
// track_type = TRACK_RIGHT;
|
|
// timer_clear(TIM_2);
|
|
// timer_start(TIM_2);
|
|
// } else if (barrier_type == BARRIER_LEFT_RUNNING) {
|
|
// track_type = TRACK_RIGHT;
|
|
// if (timer_get(TIM_2) >= 500) {
|
|
// barrier_type = BARRIER_NONE;
|
|
// track_type = TRACK_RIGHT;
|
|
// timer_stop(TIM_2);
|
|
// timer_clear(TIM_2);
|
|
// }
|
|
// }
|
|
// } else {
|
|
// track_type = TRACK_RIGHT;
|
|
// barrier_type = BARRIER_NONE;
|
|
// }
|
|
|
|
// if (barrier_type == BARRIER_LEFT_RUNNING) {
|
|
// if (pts_resample_left_count >= 5 && pts_resample_right_count >= 5) {
|
|
// barrier_type = BARRIER_NONE;
|
|
// }
|
|
// }
|
|
|
|
// if (Lpt0_found) {
|
|
// Lpt0_found_count++;
|
|
// }
|
|
// if (Lpt0_found_count >= 1 && barrier_type == BARRIER_LEFT_BEGIN) {
|
|
// Lpt0_found_count = 0;
|
|
// barrier_type = BARRIER_LEFT_RUNNING;
|
|
// track_type = TRACK_RIGHT;
|
|
// timer_clear(TIM_2);
|
|
// timer_start(TIM_2);
|
|
// } else if (barrier_type == BARRIER_LEFT_RUNNING) {
|
|
// track_type = TRACK_RIGHT;
|
|
// if (timer_get(TIM_2) >= 500) {
|
|
// barrier_type = BARRIER_NONE;
|
|
// track_type = TRACK_RIGHT;
|
|
// timer_stop(TIM_2);
|
|
// timer_clear(TIM_2);
|
|
// }
|
|
// }
|
|
}
|
|
|
|
if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
|
|
track_type = TRACK_LEFT;
|
|
if (barrier_type == BARRIER_RIGHT_BEGIN)
|
|
{
|
|
timer_clear(TIM_2);
|
|
timer_start(TIM_2);
|
|
barrier_type = BARRIER_RIGHT_RUNNING;
|
|
}
|
|
if (barrier_type == BARRIER_RIGHT_RUNNING)
|
|
{
|
|
if (timer_get(TIM_2) >= barrirer_time)
|
|
{
|
|
barrier_type = BARRIER_NONE;
|
|
timer_stop(TIM_2);
|
|
timer_clear(TIM_2);
|
|
}
|
|
else{
|
|
barrier_type = BARRIER_RIGHT_RUNNING;
|
|
}
|
|
}
|
|
// if (barrier_begin_flag == 0) {
|
|
// timer_clear(TIM_3);
|
|
// timer_start(TIM_3);
|
|
// 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 (pts_resample_right_count >= 5) {
|
|
// if (Lpt1_found) {
|
|
// Lpt1_found_count++;
|
|
// }
|
|
// if (Lpt1_found_count >= 1 && barrier_type == BARRIER_RIGHT_BEGIN) {
|
|
// Lpt1_found_count = 0;
|
|
// barrier_type = BARRIER_RIGHT_RUNNING;
|
|
// track_type = TRACK_LEFT;
|
|
// timer_clear(TIM_2);
|
|
// timer_start(TIM_2);
|
|
// } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
|
|
// track_type = TRACK_LEFT;
|
|
// if (timer_get(TIM_2) >= 500) {
|
|
// timer_stop(TIM_2);
|
|
// timer_clear(TIM_2);
|
|
// barrier_type = BARRIER_NONE;
|
|
// track_type = TRACK_LEFT;
|
|
// }
|
|
// }
|
|
// } else {
|
|
// track_type = TRACK_LEFT;
|
|
// barrier_type = BARRIER_NONE;
|
|
// }
|
|
|
|
// if (barrier_type == BARRIER_RIGHT_RUNNING) {
|
|
// if (pts_resample_left_count >= 5 && pts_resample_right_count >= 5) {
|
|
// barrier_type = BARRIER_NONE;
|
|
// }
|
|
// }
|
|
}
|
|
}
|
|
|
|
void Check_s()
|
|
{
|
|
if (track_type == TRACK_LEFT) {
|
|
mid_track_s = mid_left;
|
|
mid_track_count_s = mid_left_count;
|
|
} else {
|
|
mid_track_s = mid_right;
|
|
mid_track_count_s = mid_right_count;
|
|
}
|
|
|
|
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];
|
|
temp_min_s = min_s;
|
|
}
|
|
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) {
|
|
s_type = S_BEGIN;
|
|
}
|
|
}
|
|
|
|
void RunS()
|
|
{
|
|
|
|
if (temp_max_s - temp_min_s > 30 || (is_straight0 && is_straight1)) {
|
|
s_type = S_NONE;
|
|
}
|
|
}
|
|
|
|
// void Check_shield(){
|
|
// if (begin_flag == 1)
|
|
// {
|
|
// shield_type = SHIELD_BEGIN;
|
|
// timer_clear(TIM_3);
|
|
// timer_start(TIM_3);
|
|
// begin_flag = 0;
|
|
// }
|
|
// if (timer_get(TIM_3) > 1000 && shield_type == SHIELD_BEGIN)
|
|
// {
|
|
// shield_type = SHIELD_NONE;
|
|
// timer_stop(TIM_3);
|
|
// timer_clear(TIM_3);
|
|
// }
|
|
|
|
// }
|