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

避障

Browse Source
This commit is contained in:
井明江
2024-07-03 16:30:37 +08:00
parent 047a907eac
commit a94db790a6
4 changed files with 97 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
app/gl_tracking.c
View File

@@ -14,7 +14,7 @@ float (*rpts)[2];
int rpts_num;
float last_pure_angle = 0.0f;
int8_t turn_flag = 0;
int circle_flag = 1;
int circle_flag = 1;
// 计算最小二乘法斜率的函数
float leastSquaresSlope(float points[][2], int n)
{
@@ -270,31 +270,55 @@ void MidLineTrack()
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) {
//dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -25.f;
}else if (barrier_type == BARRIER_LEFT_RUNNING)
{
pure_angle = 25.f;
}else if (barrier_type == BARRIER_RIGHT_BEGIN)
{
pure_angle = 25.f;
}
else if (barrier_type == BARRIER_RIGHT_RUNNING) {
//dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -25.f;
} else {
// 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;
last_pure_angle = pure_angle;
// last_pure_angle_half = pure_angle_half;
} else {
pure_angle = last_pure_angle;
// pure_angle_half = last_pure_angle_half;
}
switch (barrier_type) {
case BARRIER_LEFT_BEGIN:
// dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
pure_angle = -45.f;
break;
case BARRIER_LEFT_RUNNING:
pure_angle = 25.f;
break;
case BARRIER_RIGHT_BEGIN:
pure_angle = 45.f;
break;
case BARRIER_RIGHT_RUNNING:
// dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
pure_angle = -25.f;
default:
// 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;
last_pure_angle = pure_angle;
// last_pure_angle_half = pure_angle_half;
} else {
pure_angle = last_pure_angle;
// pure_angle_half = last_pure_angle_half;
}
}
// if (barrier_type == BARRIER_LEFT_BEGIN) {
// // dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset;
// pure_angle = -25.f;
// } else if (barrier_type == BARRIER_LEFT_RUNNING) {
// pure_angle = 25.f;
// } else if (barrier_type == BARRIER_RIGHT_BEGIN) {
// pure_angle = 25.f;
// } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
// // dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset;
// pure_angle = -25.f;
// } else {
// // 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;
// last_pure_angle = pure_angle;
// // last_pure_angle_half = pure_angle_half;
// } else {
// pure_angle = last_pure_angle;
// // pure_angle_half = last_pure_angle_half;
// }
// }
// // 计算近锚点偏差值
// dx_near = rptsn[aim_idx_near][1] - cx;
// // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
@@ -308,32 +332,25 @@ void MidLineTrack()
timer_start(TIM_3);
}
if (circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_LEFT_END || circle_type == CIRCLE_RIGHT_END) {
if (circle_flag == 1)
{
if (circle_flag == 1) {
state_type = CIRCLE_STATE;
}
uint16 ti = timer_get(TIM_3);
if (ti >= 1000) {
timer_stop(TIM_3);
timer_clear(TIM_3);
circle_flag = 0;
}
if (circle_flag == 0)
{
if (circle_flag == 0) {
state_type = CIRCLE_RUNNING_STATE;
}
}
if (circle_type == CIRCLE_NONE)
{
if (circle_type == CIRCLE_NONE) {
circle_flag = 1;
}
if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING || barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
state_type = BARRIER_STATE;