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

日常更新

Browse Source
This commit is contained in:
Glz
2024-06-16 21:52:42 +08:00
parent 3fe85dc7db
commit b155d3ce14
18 changed files with 205 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

132
app/gl_tracking.c
View File

@@ -5,10 +5,11 @@ float (*mid_track)[2];
int32_t mid_track_count;
float pure_angle;
float dx_near;
float curvature;
float (*rpts)[2];
int rpts_num;
float last_pure_angle = 0.0f;
int8_t turn_flag = 0;
// 计算最小二乘法斜率的函数
float leastSquaresSlope(float points[][2], int n)
{
@@ -40,7 +41,8 @@ float leastSquaresSlope(float points[][2], int n)
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;
return b_x;
}
@@ -62,13 +64,11 @@ void tracking()
void aim_distance_select(void)
{
if (cross_type != CROSS_NONE) {
aim_distance = cricle_aim;
} else if (circle_type != CIRCLE_NONE) {
aim_distance = cross_aim;
} else if (circle_type != CIRCLE_NONE) {
aim_distance = cricle_aim;
} else if (barrier_type != BARRIER_NONE) {
aim_distance = barrier_aim;
} else {
aim_distance = common_aim;
}
}
@@ -81,6 +81,9 @@ void ElementJudge()
CheckBarrier();
if (barrier_type == BARRIER_NONE) {
CheckCircle();
if (circle_type == CIRCLE_NONE) {
Check_s();
}
}
}
}
@@ -112,6 +115,8 @@ void ElementRun()
RunCircle();
} else if (barrier_type != BARRIER_NONE) {
RunBarrier();
} else if (s_type != S_NONE) {
RunS();
}
}
@@ -141,59 +146,8 @@ void MidLineTrack()
float cx = InverseMapW[(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 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;
}
}
}
}
// 找最近点 (起始点中线归一化)
@@ -226,7 +180,7 @@ void MidLineTrack()
float dx1 = mid_track[3 * (mid_track_count / 4)][1] - mid_track[aim_idx_judge][1];
float dy1 = mid_track[3 * (mid_track_count / 4)][0] - mid_track[aim_idx_judge][0];
float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
float dx2 = mid_track[aim_idx_judge][1] - nearx;
float dy2 = mid_track[aim_idx_judge][0] - neary;
float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2);
@@ -235,6 +189,7 @@ void MidLineTrack()
float c2 = dx2 / dn2;
float s2 = dy2 / dn2;
float angle_1 = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
if (angle_1 >= 0.2f || angle_1 <= -0.2f) {
state_type = TURN_STATE;
} else {
@@ -242,58 +197,63 @@ void MidLineTrack()
}
// 计算远锚点偏差值
float dx = rptsn[aim_idx][1] - cx;
float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER;
float dn = (dx * dx + dy * dy);
float dx = rptsn[aim_idx][1] - cx;
float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER;
float dn = (dx * dx + dy * dy);
float temp_near = 0;
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
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;
} 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;
pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f + 20;
} else {
if (fabs(cx - near_x1) > fabs(cx - near_x2))
{
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;
// pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
if (dy > 0) {
pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
pure_angle = -atanf(dx / dy) / PI32 * 180.0f;
last_pure_angle = pure_angle;
} else {
pure_angle = last_pure_angle;
}
}
// // 计算近锚点偏差值
// dx_near = rptsn[aim_idx_near][1] - cx;
// // 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 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;
}
if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) {
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);
}
}
}