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-30 15:55:49 +08:00
parent 2b5ea8434e
commit 0f45c981a6
1 changed files with 55 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
app/gl_tracking.c
View File

@@ -14,6 +14,7 @@ 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; int8_t turn_flag = 0;
int circle_flag = 1;
// 计算最小二乘法斜率的函数 // 计算最小二乘法斜率的函数
float leastSquaresSlope(float points[][2], int n) float leastSquaresSlope(float points[][2], int n)
{ {
@@ -293,45 +294,67 @@ void MidLineTrack()
// // 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;
} }
if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_RIGHT_IN ) {
state_type = CIRCLE_STATE;
}
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)
{
state_type = CIRCLE_RUNNING_STATE;
}
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;
}
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)) { if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_RIGHT_IN) {
turn_flag = 1; state_type = CIRCLE_STATE;
timer_clear(TIM_3); timer_clear(TIM_3);
timer_start(TIM_3); timer_start(TIM_3);
} }
if (turn_flag == 1) { 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) {
aim_distance = turn_aim; if (circle_flag == 1)
{
state_type = CIRCLE_STATE;
}
uint16 ti = timer_get(TIM_3); uint16 ti = timer_get(TIM_3);
if (ti >= 2000) { if (ti >= 1000) {
turn_flag = 0;
timer_stop(TIM_3); timer_stop(TIM_3);
timer_clear(TIM_3); timer_clear(TIM_3);
circle_flag = 0;
} }
if (circle_flag == 0)
{
state_type = CIRCLE_RUNNING_STATE;
}
} }
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;
}
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 = turn_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);
// }
// }
} }