121 lines
3.1 KiB
C
121 lines
3.1 KiB
C
#include "zf_common_headfile.h"
|
|
#include "gl_headfile.h"
|
|
|
|
float (*mid_track)[2];
|
|
int32_t mid_track_count;
|
|
float near_angle;
|
|
float pure_angle;
|
|
float (*rpts)[2];
|
|
int rpts_num;
|
|
|
|
|
|
|
|
void tracking(){
|
|
|
|
if (pts_resample_left_count < pts_resample_right_count / 2 && pts_resample_left_count < 50) {
|
|
track_type = TRACK_RIGHT;
|
|
} else if (pts_resample_right_count < pts_resample_left_count / 2 && pts_resample_right_count < 58) {
|
|
track_type = TRACK_LEFT;
|
|
} else if (pts_resample_left_count < 20 && pts_resample_right_count > pts_resample_left_count) {
|
|
track_type = TRACK_RIGHT;
|
|
} else if (pts_resample_right_count < 20 && pts_resample_left_count > pts_resample_right_count) {
|
|
track_type = TRACK_LEFT;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void ElementJudge() {
|
|
CheckGarage();
|
|
if (garage_type == GARAGE_NONE) {
|
|
CheckCross();
|
|
if (cross_type == CROSS_NONE) {
|
|
CheckCircle();
|
|
}
|
|
}
|
|
}
|
|
|
|
void ElementRun() {
|
|
if (garage_type != GARAGE_NONE) {
|
|
RunGarage();
|
|
}
|
|
|
|
else if (cross_type != CROSS_NONE) {
|
|
RunCross();
|
|
}
|
|
|
|
else if (circle_type != CIRCLE_NONE) {
|
|
RunCircle();
|
|
}
|
|
}
|
|
|
|
void MidLineTrack() {
|
|
if (cross_type == CROSS_IN) {
|
|
if (track_type == TRACK_LEFT) {
|
|
}
|
|
}
|
|
else {
|
|
if (track_type == TRACK_LEFT) {
|
|
mid_track = mid_left;
|
|
mid_track_count = mid_left_count;
|
|
} else {
|
|
mid_track = mid_right;
|
|
mid_track_count = mid_right_count;
|
|
}
|
|
}
|
|
|
|
// 车轮对应点 (纯跟踪起始点)
|
|
float cx = InverseMapW[(int) (IMAGE_H * 0.85f)][94];
|
|
float cy = InverseMapH[(int) (IMAGE_H * 0.85f)][94];
|
|
|
|
// 找最近点 (起始点中线归一化)
|
|
float min_dist = 1e10;
|
|
|
|
int begin_id = -1;
|
|
for (int i = 0; i < mid_track_count; i++) {
|
|
float dx = mid_track[i][1] - cx;
|
|
float dy = mid_track[i][0] - cy;
|
|
float dist = sqrt(dx * dx + dy * dy);
|
|
if (dist < min_dist) {
|
|
min_dist = dist;
|
|
begin_id = i;
|
|
}
|
|
}
|
|
|
|
if (begin_id >= 0 && mid_track_count - begin_id >= 3){
|
|
// 归一化中线
|
|
mid_track[begin_id][0] = cy;
|
|
mid_track[begin_id][1] = cx;
|
|
rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]);
|
|
GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER );
|
|
|
|
// 远预锚点位置
|
|
int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
|
|
|
|
//近锚点位置
|
|
int aim_idx_near = clip(round(0.25 / RESAMPLEDIST), 0, rptsn_num - 1);
|
|
|
|
// 计算远锚点偏差值
|
|
float dx = rptsn[aim_idx][1] - cx;
|
|
float dy = cy - rptsn[aim_idx][0] + 0.2 * PIXPERMETER;
|
|
float dn = sqrt(dx * dx + dy * dy);
|
|
//float error = -atan2f(dx, dy) * 180 / PI;
|
|
|
|
// 计算近锚点偏差值
|
|
float dx_near = rptsn[aim_idx_near][1] - cx;
|
|
float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
|
|
float dn_near = sqrt(dx_near * dx_near + dy_near * dy_near);
|
|
//float error_near = -atan2f(dx_near, dy_near) * 180 / PI;
|
|
|
|
//考虑近点
|
|
near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI * 180 ;
|
|
//考虑远点
|
|
pure_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx/ dn / dn) / PI * 180 ;
|
|
|
|
|
|
|
|
}
|
|
}
|
|
|