日常更新
This commit is contained in:
@@ -2,7 +2,13 @@
|
|||||||
#include "gl_headfile.h"
|
#include "gl_headfile.h"
|
||||||
|
|
||||||
enum barrier_type_e barrier_type = BARRIER_NONE;
|
enum barrier_type_e barrier_type = BARRIER_NONE;
|
||||||
|
enum s_type_e s_type = S_NONE;
|
||||||
uint16 time_barrier;
|
uint16 time_barrier;
|
||||||
|
float (*mid_track_s)[2];
|
||||||
|
int32_t mid_track_count_s;
|
||||||
|
int temp_min_s;
|
||||||
|
int temp_max_s;
|
||||||
|
|
||||||
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) {
|
||||||
@@ -61,3 +67,49 @@ void RunBarrier()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@@ -10,10 +10,24 @@ enum barrier_type_e {
|
|||||||
BARRIER_LEFT_OUT,
|
BARRIER_LEFT_OUT,
|
||||||
BARRIER_RIGHT_OUT,
|
BARRIER_RIGHT_OUT,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
enum s_type_e {
|
||||||
|
S_NONE,
|
||||||
|
S_BEGIN,
|
||||||
|
S_RUNNING,
|
||||||
|
S_OUT,
|
||||||
|
};
|
||||||
|
|
||||||
extern uint16 time_barrier;
|
extern uint16 time_barrier;
|
||||||
extern enum barrier_type_e barrier_type;
|
extern enum barrier_type_e barrier_type;
|
||||||
|
extern enum s_type_e s_type;
|
||||||
|
extern float (*mid_track_s)[2];
|
||||||
|
extern int32_t mid_track_count_s;
|
||||||
|
extern int temp_min_s;
|
||||||
|
extern int temp_max_s;
|
||||||
|
|
||||||
void CheckBarrier(void);
|
void CheckBarrier(void);
|
||||||
|
void Check_s(void);
|
||||||
void RunBarrier(void);
|
void RunBarrier(void);
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -4,12 +4,13 @@
|
|||||||
#define IMAGE_H (MT9V03X_H)
|
#define IMAGE_H (MT9V03X_H)
|
||||||
#define IMAGE_W (MT9V03X_W)
|
#define IMAGE_W (MT9V03X_W)
|
||||||
#define BEGINH_L (61)
|
#define BEGINH_L (61)
|
||||||
|
#define BEGINH_L_curvature (75)
|
||||||
#define BEGINH_R (61)
|
#define BEGINH_R (61)
|
||||||
|
#define BEGINH_R_curvature (75)
|
||||||
#define BEGINW_L (-18)
|
#define BEGINW_L (-18)
|
||||||
#define BEGINW_R (-12)
|
#define BEGINW_R (-12)
|
||||||
#define PT_MAXLEN (75)
|
#define PT_MAXLEN (75)
|
||||||
#define GET_PIX_1C(IMG, H, W) (IMG[(H) * MT9V03X_W + (W)]) // 获取像素点的值
|
#define GET_PIX_1C(IMG, H, W) (IMG[(H) * MT9V03X_W + (W)]) // 获取像素点的值
|
||||||
#define FIX_BINTHRESHOLD (140) // 设置最开始的阈值
|
|
||||||
#define SELFADAPT_KERNELSIZE (7) // 巡线区域核大小
|
#define SELFADAPT_KERNELSIZE (7) // 巡线区域核大小
|
||||||
#define FILTER_KERNELSIZE (7) // 滤波核大小
|
#define FILTER_KERNELSIZE (7) // 滤波核大小
|
||||||
#define SELFADAPT_OFFSET (8) // 适应性块大小
|
#define SELFADAPT_OFFSET (8) // 适应性块大小
|
||||||
|
|||||||
@@ -6,6 +6,11 @@ int32_t pts_left[PT_MAXLEN][2];
|
|||||||
int32_t pts_right[PT_MAXLEN][2];
|
int32_t pts_right[PT_MAXLEN][2];
|
||||||
int32_t pts_left_count;
|
int32_t pts_left_count;
|
||||||
int32_t pts_right_count;
|
int32_t pts_right_count;
|
||||||
|
int32_t pts_left_curvature[PT_MAXLEN][2];
|
||||||
|
int32_t pts_right_curvature[PT_MAXLEN][2];
|
||||||
|
int32_t pts_left_count_curvature;
|
||||||
|
int32_t pts_right_count_curvature;
|
||||||
|
int32_t mid_s_count;
|
||||||
int32_t pts_far_left[PT_MAXLEN][2];
|
int32_t pts_far_left[PT_MAXLEN][2];
|
||||||
int32_t pts_far_right[PT_MAXLEN][2];
|
int32_t pts_far_right[PT_MAXLEN][2];
|
||||||
int32_t pts_far_left_count;
|
int32_t pts_far_left_count;
|
||||||
@@ -43,6 +48,8 @@ int32_t pts_far_resample_right_count;
|
|||||||
float mid_left[PT_MAXLEN][2];
|
float mid_left[PT_MAXLEN][2];
|
||||||
float mid_right[PT_MAXLEN][2];
|
float mid_right[PT_MAXLEN][2];
|
||||||
|
|
||||||
|
float mid_s[PT_MAXLEN][2];
|
||||||
|
|
||||||
int32_t mid_left_count;
|
int32_t mid_left_count;
|
||||||
int32_t mid_right_count;
|
int32_t mid_right_count;
|
||||||
|
|
||||||
@@ -112,5 +119,6 @@ float rptsn[PT_MAXLEN][2];
|
|||||||
int32_t rptsn_num;
|
int32_t rptsn_num;
|
||||||
float aim_distance;
|
float aim_distance;
|
||||||
float aim_judge_far=0.3f;
|
float aim_judge_far=0.3f;
|
||||||
|
float FIX_BINTHRESHOLD = 140;
|
||||||
|
|
||||||
track_type_e track_type = TRACK_RIGHT;
|
track_type_e track_type = TRACK_RIGHT;
|
||||||
@@ -16,10 +16,15 @@ extern int32_t pts_left[PT_MAXLEN][2];
|
|||||||
extern int32_t pts_right[PT_MAXLEN][2];
|
extern int32_t pts_right[PT_MAXLEN][2];
|
||||||
extern int32_t pts_left_count;
|
extern int32_t pts_left_count;
|
||||||
extern int32_t pts_right_count;
|
extern int32_t pts_right_count;
|
||||||
|
extern int32_t pts_left_curvature[PT_MAXLEN][2];
|
||||||
|
extern int32_t pts_right_curvature[PT_MAXLEN][2];
|
||||||
|
extern int32_t pts_left_count_curvature;
|
||||||
|
extern int32_t pts_right_count_curvature;
|
||||||
extern int32_t pts_far_left[PT_MAXLEN][2];
|
extern int32_t pts_far_left[PT_MAXLEN][2];
|
||||||
extern int32_t pts_far_right[PT_MAXLEN][2];
|
extern int32_t pts_far_right[PT_MAXLEN][2];
|
||||||
extern int32_t pts_far_left_count;
|
extern int32_t pts_far_left_count;
|
||||||
extern int32_t pts_far_right_count;
|
extern int32_t pts_far_right_count;
|
||||||
|
extern int32_t mid_s_count;
|
||||||
//逆透视后边线数组
|
//逆透视后边线数组
|
||||||
extern float pts_inv_l[PT_MAXLEN][2];
|
extern float pts_inv_l[PT_MAXLEN][2];
|
||||||
extern float pts_inv_r[PT_MAXLEN][2];
|
extern float pts_inv_r[PT_MAXLEN][2];
|
||||||
@@ -51,6 +56,7 @@ extern int32_t pts_far_resample_right_count;
|
|||||||
|
|
||||||
extern float mid_left[PT_MAXLEN][2];
|
extern float mid_left[PT_MAXLEN][2];
|
||||||
extern float mid_right[PT_MAXLEN][2];
|
extern float mid_right[PT_MAXLEN][2];
|
||||||
|
extern float mid_s[PT_MAXLEN][2];
|
||||||
|
|
||||||
extern int32_t mid_left_count;
|
extern int32_t mid_left_count;
|
||||||
extern int32_t mid_right_count;
|
extern int32_t mid_right_count;
|
||||||
@@ -125,5 +131,6 @@ extern float aim_distance;
|
|||||||
extern float aim_judge_far;
|
extern float aim_judge_far;
|
||||||
|
|
||||||
extern track_type_e track_type;
|
extern track_type_e track_type;
|
||||||
|
extern float FIX_BINTHRESHOLD;
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -237,3 +237,4 @@ int is_curve(float angle[], int n, float threshold) {
|
|||||||
return 0; // 不是弯道
|
return 0; // 不是弯道
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -11,7 +11,7 @@
|
|||||||
|
|
||||||
void img_processing() {
|
void img_processing() {
|
||||||
|
|
||||||
|
//
|
||||||
int w1 = IMAGE_W / 2 - BEGINW_R, h1 = BEGINH_L;
|
int w1 = IMAGE_W / 2 - BEGINW_R, h1 = BEGINH_L;
|
||||||
pts_left_count = sizeof(pts_left) / sizeof(pts_left[0]);
|
pts_left_count = sizeof(pts_left) / sizeof(pts_left[0]);
|
||||||
for (; w1 > 0; w1--) {
|
for (; w1 > 0; w1--) {
|
||||||
@@ -23,6 +23,20 @@ void img_processing() {
|
|||||||
SearchLineAdaptive_Left(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, h1, w1, pts_left, &pts_left_count);
|
SearchLineAdaptive_Left(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, h1, w1, pts_left, &pts_left_count);
|
||||||
}
|
}
|
||||||
else pts_left_count = 0;
|
else pts_left_count = 0;
|
||||||
|
//算曲率
|
||||||
|
int w1_curvature = IMAGE_W / 2 - BEGINW_R, h1_curvature = BEGINH_L_curvature;
|
||||||
|
pts_left_count_curvature = sizeof(pts_left_curvature) / sizeof(pts_left_curvature[0]);
|
||||||
|
for (; w1_curvature > 0; w1_curvature--) {
|
||||||
|
if (GET_PIX_1C(mt9v03x_image_copy[0], h1_curvature, w1_curvature - 1) < FIX_BINTHRESHOLD)
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (GET_PIX_1C(mt9v03x_image_copy[0], h1_curvature, w1_curvature) >= FIX_BINTHRESHOLD){
|
||||||
|
SearchLineAdaptive_Left(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, h1_curvature, w1_curvature, pts_left_curvature, &pts_left_count_curvature);
|
||||||
|
}
|
||||||
|
else pts_left_count_curvature = 0;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int w2 = IMAGE_W / 2 + BEGINW_L, h2 = BEGINH_R;
|
int w2 = IMAGE_W / 2 + BEGINW_L, h2 = BEGINH_R;
|
||||||
pts_right_count = sizeof(pts_right) / sizeof(pts_right[0]);
|
pts_right_count = sizeof(pts_right) / sizeof(pts_right[0]);
|
||||||
@@ -35,6 +49,18 @@ void img_processing() {
|
|||||||
SearchLineAdaptive_Right(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, h2, w2, pts_right, &pts_right_count);
|
SearchLineAdaptive_Right(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, h2, w2, pts_right, &pts_right_count);
|
||||||
}
|
}
|
||||||
else pts_right_count = 0;
|
else pts_right_count = 0;
|
||||||
|
//算曲率
|
||||||
|
int w2_curvature = IMAGE_W / 2 + BEGINW_L, h2_curvature = BEGINH_R_curvature;
|
||||||
|
pts_right_count_curvature = sizeof(pts_right_curvature) / sizeof(pts_right_curvature[0]);
|
||||||
|
for (; w2_curvature < IMAGE_W - 1; w2_curvature++) {
|
||||||
|
if (GET_PIX_1C(mt9v03x_image_copy[0], h2_curvature, w2_curvature + 1) < FIX_BINTHRESHOLD)
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (GET_PIX_1C(mt9v03x_image_copy[0], h2_curvature, w2_curvature) >= FIX_BINTHRESHOLD){
|
||||||
|
SearchLineAdaptive_Right(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, h2_curvature, w2_curvature, pts_right_curvature, &pts_right_count_curvature);
|
||||||
|
}
|
||||||
|
else pts_right_count_curvature = 0;
|
||||||
|
|
||||||
//透视变换
|
//透视变换
|
||||||
for (int i = 0; i < pts_left_count; i++) {
|
for (int i = 0; i < pts_left_count; i++) {
|
||||||
@@ -87,6 +113,7 @@ void img_processing() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
pts_resample_right_count = valid_points_count;
|
pts_resample_right_count = valid_points_count;
|
||||||
|
|
||||||
// 大距离边线角度变化率
|
// 大距离边线角度变化率
|
||||||
|
|||||||
@@ -2,8 +2,12 @@
|
|||||||
#include "gl_headfile.h"
|
#include "gl_headfile.h"
|
||||||
|
|
||||||
enum state_type_e state_type =0;
|
enum state_type_e state_type =0;
|
||||||
|
enum state_type_e last_state =0;
|
||||||
float cricle_aim=0.2f;
|
float cricle_aim=0.2f;
|
||||||
float cross_aim=0.49f;
|
float cross_aim=0.49f;
|
||||||
float common_aim=0.35f;
|
float common_aim=0.35f;
|
||||||
|
float straight_aim=0.35f;
|
||||||
|
float turn_aim=0.35f;
|
||||||
|
float mid_aim=0.5f;
|
||||||
float barrier_aim=0.5f;
|
float barrier_aim=0.5f;
|
||||||
float barrier_offset=17.0f;
|
float barrier_offset=17.0f;
|
||||||
@@ -7,11 +7,14 @@ enum state_type_e {
|
|||||||
STRAIGHT_STATE,
|
STRAIGHT_STATE,
|
||||||
CIRCLE_STATE,
|
CIRCLE_STATE,
|
||||||
};
|
};
|
||||||
|
extern enum state_type_e last_state;
|
||||||
extern enum state_type_e state_type;
|
extern enum state_type_e state_type;
|
||||||
extern float cricle_aim;
|
extern float cricle_aim;
|
||||||
extern float cross_aim;
|
extern float cross_aim;
|
||||||
extern float common_aim;
|
extern float common_aim;
|
||||||
|
extern float straight_aim;
|
||||||
|
extern float turn_aim;
|
||||||
|
extern float mid_aim;
|
||||||
extern float barrier_aim;
|
extern float barrier_aim;
|
||||||
extern float barrier_offset;
|
extern float barrier_offset;
|
||||||
|
|
||||||
|
|||||||
@@ -5,10 +5,11 @@ float (*mid_track)[2];
|
|||||||
int32_t mid_track_count;
|
int32_t mid_track_count;
|
||||||
float pure_angle;
|
float pure_angle;
|
||||||
float dx_near;
|
float dx_near;
|
||||||
|
float curvature;
|
||||||
float (*rpts)[2];
|
float (*rpts)[2];
|
||||||
int rpts_num;
|
int rpts_num;
|
||||||
float last_pure_angle = 0.0f;
|
float last_pure_angle = 0.0f;
|
||||||
|
int8_t turn_flag = 0;
|
||||||
// 计算最小二乘法斜率的函数
|
// 计算最小二乘法斜率的函数
|
||||||
float leastSquaresSlope(float points[][2], int n)
|
float leastSquaresSlope(float points[][2], int n)
|
||||||
{
|
{
|
||||||
@@ -40,7 +41,8 @@ float leastSquaresSlope(float points[][2], int n)
|
|||||||
return temp;
|
return temp;
|
||||||
}
|
}
|
||||||
|
|
||||||
float calculateX(float a_x, float a_y, float slope, float b_y) {
|
float calculateX(float a_x, float a_y, float slope, float b_y)
|
||||||
|
{
|
||||||
float b_x = a_x - (b_y - a_y) * slope;
|
float b_x = a_x - (b_y - a_y) * slope;
|
||||||
return b_x;
|
return b_x;
|
||||||
}
|
}
|
||||||
@@ -62,13 +64,11 @@ void tracking()
|
|||||||
void aim_distance_select(void)
|
void aim_distance_select(void)
|
||||||
{
|
{
|
||||||
if (cross_type != CROSS_NONE) {
|
if (cross_type != CROSS_NONE) {
|
||||||
aim_distance = cricle_aim;
|
|
||||||
} else if (circle_type != CIRCLE_NONE) {
|
|
||||||
aim_distance = cross_aim;
|
aim_distance = cross_aim;
|
||||||
|
} else if (circle_type != CIRCLE_NONE) {
|
||||||
|
aim_distance = cricle_aim;
|
||||||
} else if (barrier_type != BARRIER_NONE) {
|
} else if (barrier_type != BARRIER_NONE) {
|
||||||
aim_distance = barrier_aim;
|
aim_distance = barrier_aim;
|
||||||
} else {
|
|
||||||
aim_distance = common_aim;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -81,6 +81,9 @@ void ElementJudge()
|
|||||||
CheckBarrier();
|
CheckBarrier();
|
||||||
if (barrier_type == BARRIER_NONE) {
|
if (barrier_type == BARRIER_NONE) {
|
||||||
CheckCircle();
|
CheckCircle();
|
||||||
|
if (circle_type == CIRCLE_NONE) {
|
||||||
|
Check_s();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -112,6 +115,8 @@ void ElementRun()
|
|||||||
RunCircle();
|
RunCircle();
|
||||||
} else if (barrier_type != BARRIER_NONE) {
|
} else if (barrier_type != BARRIER_NONE) {
|
||||||
RunBarrier();
|
RunBarrier();
|
||||||
|
} else if (s_type != S_NONE) {
|
||||||
|
RunS();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -141,59 +146,8 @@ void MidLineTrack()
|
|||||||
float cx = InverseMapW[(int)(IMAGE_H * 0.8f)][70];
|
float cx = InverseMapW[(int)(IMAGE_H * 0.8f)][70];
|
||||||
float cy = InverseMapH[(int)(IMAGE_H * 0.8f)][70];
|
float cy = InverseMapH[(int)(IMAGE_H * 0.8f)][70];
|
||||||
|
|
||||||
|
|
||||||
// float slope = leastSquaresSlope(mid_track, mid_track_count);
|
|
||||||
int neary = mid_track[0][0];
|
int neary = mid_track[0][0];
|
||||||
int nearx = mid_track[0][1];
|
int nearx = mid_track[0][1];
|
||||||
// float near_distance = calculateX(a_x, a_y, slope, cy);
|
|
||||||
int w1 = (int)cx;
|
|
||||||
int h1 = (int)cy;
|
|
||||||
int near_x1 = 0;
|
|
||||||
int near_x2 = 0;
|
|
||||||
if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1) >= FIX_BINTHRESHOLD)
|
|
||||||
{
|
|
||||||
for(;w1>10; w1--)
|
|
||||||
{
|
|
||||||
if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) < FIX_BINTHRESHOLD) {
|
|
||||||
near_x1 = w1;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
for(;w1<130; w1++)
|
|
||||||
{
|
|
||||||
if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + 1) < FIX_BINTHRESHOLD) {
|
|
||||||
near_x2 = w1;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - BEGINW_R) < FIX_BINTHRESHOLD)
|
|
||||||
{
|
|
||||||
for(;w1>10; w1--)
|
|
||||||
{
|
|
||||||
if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 2) > FIX_BINTHRESHOLD) {
|
|
||||||
near_x1 = w1;
|
|
||||||
near_x2 = cx;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + BEGINW_R) < FIX_BINTHRESHOLD)
|
|
||||||
{
|
|
||||||
for(;w1<130; w1++)
|
|
||||||
{
|
|
||||||
if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + 1) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 + 2) > FIX_BINTHRESHOLD) {
|
|
||||||
near_x1 = cx;
|
|
||||||
near_x2 = w1;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 找最近点 (起始点中线归一化)
|
// 找最近点 (起始点中线归一化)
|
||||||
@@ -235,6 +189,7 @@ void MidLineTrack()
|
|||||||
float c2 = dx2 / dn2;
|
float c2 = dx2 / dn2;
|
||||||
float s2 = dy2 / dn2;
|
float s2 = dy2 / dn2;
|
||||||
float angle_1 = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
|
float angle_1 = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
|
||||||
|
|
||||||
if (angle_1 >= 0.2f || angle_1 <= -0.2f) {
|
if (angle_1 >= 0.2f || angle_1 <= -0.2f) {
|
||||||
state_type = TURN_STATE;
|
state_type = TURN_STATE;
|
||||||
} else {
|
} else {
|
||||||
@@ -249,28 +204,11 @@ void MidLineTrack()
|
|||||||
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
|
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_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 = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f - 20;
|
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f - 20;
|
||||||
} else if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING ) {
|
} else if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == 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 = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f + 20;
|
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f + 20;
|
||||||
} else {
|
} else {
|
||||||
if (fabs(cx - near_x1) > fabs(cx - near_x2))
|
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
|
||||||
{
|
|
||||||
dx_near = near_x2 - cx - 45;
|
|
||||||
temp_near = dx_near;
|
|
||||||
}
|
|
||||||
if (fabs(cx - near_x1) < fabs(cx - near_x2))
|
|
||||||
{
|
|
||||||
dx_near = near_x1 - cx + 45;
|
|
||||||
temp_near = dx_near;
|
|
||||||
}
|
|
||||||
if (fabs(cx - near_x1) == fabs(cx - near_x2))
|
|
||||||
{
|
|
||||||
dx_near = 0;
|
|
||||||
temp_near = dx_near;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
|
|
||||||
if (dy > 0) {
|
if (dy > 0) {
|
||||||
pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
|
pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
|
||||||
last_pure_angle = pure_angle;
|
last_pure_angle = pure_angle;
|
||||||
@@ -279,21 +217,43 @@ void MidLineTrack()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// // 计算近锚点偏差值
|
// // 计算近锚点偏差值
|
||||||
// dx_near = rptsn[aim_idx_near][1] - cx;
|
// dx_near = rptsn[aim_idx_near][1] - cx;
|
||||||
// // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
|
// // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
|
||||||
// // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
|
// // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
|
||||||
// // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
|
// // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
|
||||||
|
|
||||||
// // //考虑近点
|
|
||||||
// // near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI32 * 180 ;
|
|
||||||
// // //考虑远点
|
|
||||||
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
|
|
||||||
}
|
}
|
||||||
if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING) {
|
if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_LEFT_BEGIN || circle_type == CIRCLE_RIGHT_BEGIN) {
|
||||||
state_type = CIRCLE_STATE;
|
state_type = CIRCLE_STATE;
|
||||||
}
|
}
|
||||||
if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) {
|
if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) {
|
||||||
state_type = STRAIGHT_STATE;
|
state_type = STRAIGHT_STATE;
|
||||||
}
|
}
|
||||||
|
last_state = state_type;
|
||||||
|
if (state_type == STRAIGHT_STATE) {
|
||||||
|
aim_distance = straight_aim;
|
||||||
|
} else if (state_type == TURN_STATE) {
|
||||||
|
aim_distance = mid_aim;
|
||||||
|
}
|
||||||
|
// }else{
|
||||||
|
// aim_distance = common_aim;
|
||||||
|
// }
|
||||||
|
|
||||||
|
if (last_state == STRAIGHT_STATE && (state_type == TURN_STATE || state_type == CIRCLE_STATE)) {
|
||||||
|
turn_flag = 1;
|
||||||
|
timer_clear(TIM_3);
|
||||||
|
timer_start(TIM_3);
|
||||||
|
}
|
||||||
|
if (turn_flag == 1) {
|
||||||
|
aim_distance = turn_aim;
|
||||||
|
|
||||||
|
uint16 ti = timer_get(TIM_3);
|
||||||
|
if (ti >= 2000) {
|
||||||
|
turn_flag = 0;
|
||||||
|
timer_stop(TIM_3);
|
||||||
|
timer_clear(TIM_3);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -6,6 +6,8 @@ extern int32_t mid_track_count;
|
|||||||
extern float pure_angle;
|
extern float pure_angle;
|
||||||
extern float dx_near;
|
extern float dx_near;
|
||||||
|
|
||||||
|
extern int8_t turn_flag ;
|
||||||
|
extern float curvature;
|
||||||
void aim_distance_select(void);
|
void aim_distance_select(void);
|
||||||
void tracking(void);
|
void tracking(void);
|
||||||
void ElementJudge(void);
|
void ElementJudge(void);
|
||||||
|
|||||||
@@ -92,6 +92,10 @@ void USART1_IRQHandler(void)
|
|||||||
void USART2_IRQHandler(void)
|
void USART2_IRQHandler(void)
|
||||||
{
|
{
|
||||||
if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) {
|
if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) {
|
||||||
|
// uint8_t data = (uint8_t)USART_ReceiveData(USART2);
|
||||||
|
// by_frame_parse_uart_handle(data);
|
||||||
|
USART_ReceiveData(USART2);
|
||||||
|
NVIC_SystemReset();
|
||||||
USART_ClearITPendingBit(USART2, USART_IT_RXNE);
|
USART_ClearITPendingBit(USART2, USART_IT_RXNE);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -3,6 +3,7 @@
|
|||||||
#include "./page/page.h"
|
#include "./page/page.h"
|
||||||
#include "gl_tracking.h"
|
#include "gl_tracking.h"
|
||||||
#include "gl_state.h"
|
#include "gl_state.h"
|
||||||
|
#include "gl_data.h"
|
||||||
#include "zf_common_headfile.h"
|
#include "zf_common_headfile.h"
|
||||||
PARAM_INFO Param_Data[DATA_NUM];
|
PARAM_INFO Param_Data[DATA_NUM];
|
||||||
soft_iic_info_struct eeprom_param;
|
soft_iic_info_struct eeprom_param;
|
||||||
@@ -20,10 +21,13 @@ void jj_param_eeprom_init(void)
|
|||||||
PARAM_REG(aim_cricle, &cricle_aim, EFLOAT, 1, "cric:"); // 注冊
|
PARAM_REG(aim_cricle, &cricle_aim, EFLOAT, 1, "cric:"); // 注冊
|
||||||
PARAM_REG(aim_common, &common_aim, EFLOAT, 1, "comm:"); // 注冊
|
PARAM_REG(aim_common, &common_aim, EFLOAT, 1, "comm:"); // 注冊
|
||||||
PARAM_REG(aim_barrier, &barrier_aim, EFLOAT, 1, "barr:");
|
PARAM_REG(aim_barrier, &barrier_aim, EFLOAT, 1, "barr:");
|
||||||
|
PARAM_REG(aim_mid, &mid_aim, EFLOAT, 1, "mid:");
|
||||||
PARAM_REG(offset_barrier, &barrier_offset, EFLOAT, 1, "offs:");
|
PARAM_REG(offset_barrier, &barrier_offset, EFLOAT, 1, "offs:");
|
||||||
PARAM_REG(delta_x, &pure_angle, EFLOAT, 2, "far:");
|
PARAM_REG(delta_x, &pure_angle, EFLOAT, 2, "far:");
|
||||||
PARAM_REG(delta_y, &dx_near, EFLOAT, 2, "near:");
|
PARAM_REG(delta_y, &dx_near, EFLOAT, 2, "near:");
|
||||||
|
PARAM_REG(aim_straight, &straight_aim, EFLOAT, 1, "str:");
|
||||||
|
PARAM_REG(aim_turn, &turn_aim, EFLOAT, 1, "tur:");
|
||||||
|
PARAM_REG(baoguang, &FIX_BINTHRESHOLD, EFLOAT, 1, "bao:");
|
||||||
jj_param_read(); // 注冊
|
jj_param_read(); // 注冊
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -23,11 +23,12 @@ typedef enum {
|
|||||||
aim_cricle,
|
aim_cricle,
|
||||||
aim_barrier,
|
aim_barrier,
|
||||||
offset_barrier,
|
offset_barrier,
|
||||||
|
aim_straight,
|
||||||
|
aim_turn,
|
||||||
|
aim_mid,
|
||||||
Page2_head,
|
Page2_head,
|
||||||
// 第二页参数
|
// 第二页参数
|
||||||
imgax_Kp = Page2_head,
|
baoguang = Page2_head,
|
||||||
imgax_Ki,
|
|
||||||
imgax_Kd,
|
|
||||||
|
|
||||||
Page3_head,
|
Page3_head,
|
||||||
DATA_IN_FLASH_NUM,
|
DATA_IN_FLASH_NUM,
|
||||||
|
|||||||
@@ -73,7 +73,7 @@ int main(void)
|
|||||||
ElementJudge();
|
ElementJudge();
|
||||||
ElementRun();
|
ElementRun();
|
||||||
MidLineTrack();
|
MidLineTrack();
|
||||||
bt_printf("type= %d", circle_type);
|
//bt_printf("type= %d", circle_type);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -49,11 +49,12 @@ static void Loop()
|
|||||||
{
|
{
|
||||||
// 刷新参数数值
|
// 刷新参数数值
|
||||||
ips200_show_float(90, 18 + 2, *((float *)(Param_Data[delta_x].p_data)), 4, 5);
|
ips200_show_float(90, 18 + 2, *((float *)(Param_Data[delta_x].p_data)), 4, 5);
|
||||||
ips200_show_float(90, 18 + 20, *((float *)(Param_Data[delta_y].p_data)), 4, 5);
|
ips200_show_float(90, 18 + 20, FIX_BINTHRESHOLD, 4, 5);
|
||||||
ips200_show_int(90, 18 + 38, track_type, 1);
|
ips200_show_int(90, 18 + 38, track_type, 1);
|
||||||
ips200_show_int(90, 18 + 56, garage_type, 1);
|
ips200_show_int(90, 18 + 56, garage_type, 1);
|
||||||
ips200_show_int(90, 18 + 74, cross_type, 1);
|
ips200_show_int(90, 18 + 74, cross_type, 1);
|
||||||
ips200_show_int(90, 18 + 92, circle_type, 1);
|
ips200_show_int(90, 18 + 92, circle_type, 1);
|
||||||
|
ips200_show_int(90, 18 + 110, s_type, 1);
|
||||||
}
|
}
|
||||||
/**
|
/**
|
||||||
* @brief 页面事件
|
* @brief 页面事件
|
||||||
|
|||||||
@@ -83,9 +83,9 @@ static void Event(page_event event)
|
|||||||
|
|
||||||
Curser_Last = Curser;
|
Curser_Last = Curser;
|
||||||
if (page_event_forward == event) {
|
if (page_event_forward == event) {
|
||||||
Curser++; // 光标上移
|
Curser--; // 光标上移
|
||||||
} else if (page_event_backward == event) {
|
} else if (page_event_backward == event) {
|
||||||
Curser--; // 光标下移
|
Curser++; // 光标下移
|
||||||
} else if (page_event_press_short == event) {
|
} else if (page_event_press_short == event) {
|
||||||
event_flag = 1; // 选中参数
|
event_flag = 1; // 选中参数
|
||||||
Print_Curser(Curser, Curser_Last, RGB565_RED);
|
Print_Curser(Curser, Curser_Last, RGB565_RED);
|
||||||
|
|||||||
@@ -45,11 +45,19 @@ static void Setup()
|
|||||||
Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
|
Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
|
||||||
for (int16 i = 0; i < palong; i++) {
|
for (int16 i = 0; i < palong; i++) {
|
||||||
ips200_show_string(20, i * 18 + 20, Param_Data[i + pafrist].text);
|
ips200_show_string(20, i * 18 + 20, Param_Data[i + pafrist].text);
|
||||||
if (Param_Data[i].type == EINT32)
|
if (Param_Data[i+ pafrist].type == EINT32)
|
||||||
ips200_show_int(60, i * 18 + 20, *((int32 *)(Param_Data[i + pafrist].p_data)), 5);
|
{
|
||||||
else if (Param_Data[i].type == EFLOAT)
|
ips200_show_int(60, i * 18 + 20, *((int32_t *)(Param_Data[i + pafrist].p_data)), 5);
|
||||||
|
}
|
||||||
|
else if (Param_Data[i+ pafrist].type == EFLOAT)
|
||||||
|
{
|
||||||
ips200_show_float(60, i * 18 + 20, *((float *)(Param_Data[i + pafrist].p_data)), 4, 5);
|
ips200_show_float(60, i * 18 + 20, *((float *)(Param_Data[i + pafrist].p_data)), 4, 5);
|
||||||
}
|
}
|
||||||
|
else if (Param_Data[i+ pafrist].type == EUINT32)
|
||||||
|
{
|
||||||
|
ips200_show_uint(60, i * 18 + 20, *((uint32_t *)(Param_Data[i + pafrist].p_data)), 4);
|
||||||
|
}
|
||||||
|
}
|
||||||
ips200_show_int(50, palong * 18 + 20, index_power, 5);
|
ips200_show_int(50, palong * 18 + 20, index_power, 5);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -83,9 +91,9 @@ static void Event(page_event event)
|
|||||||
|
|
||||||
Curser_Last = Curser;
|
Curser_Last = Curser;
|
||||||
if (page_event_forward == event) {
|
if (page_event_forward == event) {
|
||||||
Curser++; // 光标上移
|
Curser--; // 光标上移
|
||||||
} else if (page_event_backward == event) {
|
} else if (page_event_backward == event) {
|
||||||
Curser--; // 光标下移
|
Curser++; // 光标下移
|
||||||
} else if (page_event_press_short == event) {
|
} else if (page_event_press_short == event) {
|
||||||
event_flag = 1; // 选中参数
|
event_flag = 1; // 选中参数
|
||||||
Print_Curser(Curser, Curser_Last, RGB565_RED);
|
Print_Curser(Curser, Curser_Last, RGB565_RED);
|
||||||
@@ -105,13 +113,13 @@ static void Event(page_event event)
|
|||||||
if (page_event_forward == event) {
|
if (page_event_forward == event) {
|
||||||
switch (Param_Data[Curser + pafrist - 1].type) {
|
switch (Param_Data[Curser + pafrist - 1].type) {
|
||||||
case EFLOAT:
|
case EFLOAT:
|
||||||
*((float *)(Param_Data[Curser + pafrist - 1].p_data)) += powf(10, index_power);
|
*((float *)(Param_Data[Curser + pafrist - 1].p_data)) += powf(10.0f, (float)index_power);
|
||||||
break;
|
break;
|
||||||
case EINT32:
|
case EINT32:
|
||||||
*((int32 *)(Param_Data[Curser + pafrist - 1].p_data)) += 1;
|
*((int32 *)(Param_Data[Curser + pafrist - 1].p_data)) += 1;
|
||||||
break;
|
break;
|
||||||
case EUINT32:
|
case EUINT32:
|
||||||
*((uint32 *)(Param_Data[Curser + pafrist - 1].p_data)) += 1;
|
*((uint32 *)(Param_Data[Curser + pafrist - 1].p_data)) +=(uint32_t)powf(10, index_power);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
@@ -125,14 +133,14 @@ static void Event(page_event event)
|
|||||||
*((int32 *)(Param_Data[Curser + pafrist - 1].p_data)) -= 1;
|
*((int32 *)(Param_Data[Curser + pafrist - 1].p_data)) -= 1;
|
||||||
break;
|
break;
|
||||||
case EUINT32:
|
case EUINT32:
|
||||||
*((uint32 *)(Param_Data[Curser + pafrist - 1].p_data)) -= 1;
|
*((uint32 *)(Param_Data[Curser + pafrist - 1].p_data)) -= (uint32_t)powf(10, index_power);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
} else if (page_event_press_short == event) {
|
} else if (page_event_press_short == event) {
|
||||||
index_power++;
|
index_power++;
|
||||||
if (index_power > 2) {
|
if (index_power > 5) {
|
||||||
index_power = -2;
|
index_power = -2;
|
||||||
}
|
}
|
||||||
ips200_show_int(50, palong * 18 + 20, index_power, 5);
|
ips200_show_int(50, palong * 18 + 20, index_power, 5);
|
||||||
@@ -143,7 +151,7 @@ static void Event(page_event event)
|
|||||||
if (EINT32 == Param_Data[Curser + pafrist - 1].type)
|
if (EINT32 == Param_Data[Curser + pafrist - 1].type)
|
||||||
ips200_show_int(60, Curser * 18 + 2, *((int32 *)(Param_Data[Curser + pafrist - 1].p_data)), 5);
|
ips200_show_int(60, Curser * 18 + 2, *((int32 *)(Param_Data[Curser + pafrist - 1].p_data)), 5);
|
||||||
else if (EUINT32 == Param_Data[Curser + pafrist - 1].type)
|
else if (EUINT32 == Param_Data[Curser + pafrist - 1].type)
|
||||||
ips200_show_uint(60, Curser * 18 + 2, *((int32 *)(Param_Data[Curser + pafrist - 1].p_data)), 5);
|
ips200_show_uint(60, Curser * 18 + 2, *((uint32 *)(Param_Data[Curser + pafrist - 1].p_data)), 5);
|
||||||
else if (EFLOAT == Param_Data[Curser + pafrist - 1].type)
|
else if (EFLOAT == Param_Data[Curser + pafrist - 1].type)
|
||||||
ips200_show_float(60, Curser * 18 + 2, *((float *)(Param_Data[Curser + pafrist - 1].p_data)), 4, 5);
|
ips200_show_float(60, Curser * 18 + 2, *((float *)(Param_Data[Curser + pafrist - 1].p_data)), 4, 5);
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user