57 lines
1.9 KiB
C
57 lines
1.9 KiB
C
|
#include "zf_common_headfile.h"
|
||
|
|
#include "gl_headfile.h"
|
||
|
|
|
||
|
|
enum cross_type_e cross_type = CROSS_NONE;
|
||
|
|
bool far_Lpt0_found, far_Lpt1_found;
|
||
|
|
int32_t Both_Boder_None_Cross;
|
||
|
|
|
||
|
|
void CheckCross() {
|
||
|
|
bool Xfound = Lpt0_found && Lpt1_found;
|
||
|
|
if (cross_type == CROSS_NONE && Xfound){
|
||
|
|
cross_type = CROSS_BEGIN;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void RunCross() {
|
||
|
|
bool Xfound = Lpt0_found && Lpt1_found;
|
||
|
|
aim_distance = 0.4;
|
||
|
|
//检测到十字，先按照近线走
|
||
|
|
if (cross_type == CROSS_BEGIN) {
|
||
|
|
if (Lpt0_found) {
|
||
|
|
mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id;
|
||
|
|
track_type = TRACK_LEFT;
|
||
|
|
}
|
||
|
|
if (Lpt1_found) {
|
||
|
|
mid_right_count = pts_resample_right_count = Lpt1_rpts1s_id;
|
||
|
|
track_type = TRACK_RIGHT;
|
||
|
|
}
|
||
|
|
|
||
|
|
//近角点过少，进入远线控制
|
||
|
|
if ((Xfound && (Lpt0_rpts0s_id < 0.2 / RESAMPLEDIST || Lpt1_rpts1s_id < 0.2 / RESAMPLEDIST)) || (pts_resample_left_count <20 && pts_resample_right_count<20)) {
|
||
|
|
cross_type = CROSS_IN;
|
||
|
|
//cross_encoder = current_encoder;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
//远线控制进十字,begin_y渐变靠近防丢线
|
||
|
|
else if (cross_type == CROSS_IN) {
|
||
|
|
//寻远线,算法与近线相同
|
||
|
|
cross_farline();
|
||
|
|
|
||
|
|
if (pts_resample_left_count < 5 && pts_resample_right_count < 5) {
|
||
|
|
Both_Boder_None_Cross++;
|
||
|
|
}
|
||
|
|
if (Both_Boder_None_Cross > 2 && pts_resample_left_count > 20 && pts_resample_right_count > 20) {
|
||
|
|
cross_type = CROSS_NONE;
|
||
|
|
Both_Boder_None_Cross = 0;
|
||
|
|
}
|
||
|
|
if (far_Lpt1_found) { track_type = TRACK_RIGHT; }
|
||
|
|
else if (far_Lpt0_found) { track_type = TRACK_LEFT; }
|
||
|
|
else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) { track_type = TRACK_RIGHT; }
|
||
|
|
else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) { track_type = TRACK_LEFT; }
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void cross_farline(){}
|