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

feat：增加避障积分屏蔽出界停车，增加出界原因屏幕显示，注册分段速度系数eeprom

Browse Source
This commit is contained in:
井明江
2024-08-15 17:03:42 +08:00
parent c825014599
commit 9a4caf7b7c
6 changed files with 53 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
app/jj_motion.c
View File

@@ -81,7 +81,8 @@ float in_pos;
float out_pos;
float set_pos = 0.0f;
// float set_gyro = 0.0f;
float rate_K = 0;
float rate_K1 = 0;
float rate_K2 = 0;
float in_speed;
float out_speed;
float last_angle;
@@ -111,15 +112,19 @@ int32_t pwm_duty_ls;
int32_t pwm_duty_rs;
int32_t pwm_duty_lb;
int32_t pwm_duty_rb;
int32_t stop_cnt = 0;
int32_t last_lb = 0;
int32_t last_rb = 0;
float start_dis = 0;
float set_dis=1.f;
uint8_t go_cnt = 0; // 0 停车 1 测试侧向风扇 2 启动 3成功运行
float bug_sw = 0;
float out_sw = 0;
float sho_sw = 0;
int32_t stop_cnt = 0;
int32_t last_lb = 0;
int32_t last_rb = 0;
float start_dis = 0;
float set_dis = 1.f;
uint8_t go_cnt = 0; // 0 停车 1 测试侧向风扇 2 启动 3成功运行
float bug_sw = 0;
float out_sw = 0;
float sho_sw = 0;
uint8_t bug_show_flag = 0;
uint8_t out_show_flag = 0;
uint8_t up_show_flag = 0;
uint8_t finish_show_flag = 0;
/*
0:无状态
1:弯道
@@ -166,13 +171,15 @@ void sport_motion()
if (cnt7 >= 50 && (uint8_t)sho_sw == 1) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("up");
go_cnt = 0;
up_show_flag = 1;
go_cnt = 0;
}
// 異常值停车
if (fabs(in_angle - last_angle) > 45.f && (uint8_t)bug_sw == 1) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("bug");
go_cnt = 0;
bug_show_flag = 1;
go_cnt = 0;
}
// 斑马线停车,摄像头识别，屏蔽发车前的十秒钟
if (1 == in_stop && if_start == 1) {
@@ -180,13 +187,15 @@ void sport_motion()
}
if (stop_flag == 1) {
bt_printf("finish");
finish_show_flag = 1;
bt_fly_flag = bt_run_flag = 0;
go_cnt = 0;
}
// 出界停车,已经积分路过斑马线，保证斑马线不会触发
if (2 == in_stop && (start_dis >= set_dis|| bt_run_flag == 0) && (uint8_t)out_sw == 1) {
if (2 == in_stop && ((start_dis >= set_dis) || bt_run_flag == 0) && (uint8_t)out_sw == 1) {
bt_printf("out");
go_cnt = 0;
out_show_flag = 1;
go_cnt = 0;
bt_fly_flag = bt_run_flag = 0;
}
@@ -215,7 +224,11 @@ void sport_motion()
PID_SetTunings(&pos_pid, po_Kp1, po_Ki1, po_Kd1);
} else if ((last_state != in_state) && in_state == 1) { // 弯道
bt_printf("to 入弯");
set_speed = set_speed0 - fabs(rate_K * myclip_f(in_pos, -50.f, 50.f));
if (fabs(in_pos) <= 12.0f) {
set_speed = set_speed0 - fabs(rate_K1 * myclip_f(in_pos, -50.f, 50.f));
} else {
set_speed = set_speed0 - fabs(rate_K2 * myclip_f(in_pos, -50.f, 50.f));
}
PID_SetTunings(&far_angle_pid, an_Kp0, an_Ki0, an_Kd0);
PID_SetTunings(&far_gyro_pid, gy_Kp0, gy_Ki0, gy_Kd0);
@@ -233,7 +246,7 @@ void sport_motion()
} else if ((last_state != in_state) && in_state == 5) { //
bt_printf("to 障碍");
set_speed = set_speed4;
start_dis = 0;
PID_SetTunings(&far_angle_pid, za_Kp, za_Ki, za_Kd);
PID_SetTunings(&far_gyro_pid, zg_Kp, zg_Ki, zg_Kd);
PID_SetTunings(&pos_pid, zp_Kp, zp_Ki, zp_Kd);
@@ -241,9 +254,9 @@ void sport_motion()
if (in_state == 1) {
if (fabs(in_pos) <= 12.0f) {
set_speed = set_speed0 - fabs(rate_K / 3 * myclip_f(in_pos, -50.f, 50.f));
set_speed = set_speed0 - fabs(rate_K1 * myclip_f(in_pos, -50.f, 50.f));
} else {
set_speed = set_speed0 - fabs(rate_K * myclip_f(in_pos, -50.f, 50.f));
set_speed = set_speed0 - fabs(rate_K2 * myclip_f(in_pos, -50.f, 50.f));
}
}
// 记录上一次状态