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

27
app/jj_motion.c
View File

@@ -81,7 +81,8 @@ float in_pos;
float out_pos; float out_pos;
float set_pos = 0.0f; float set_pos = 0.0f;
// float set_gyro = 0.0f; // float set_gyro = 0.0f;
float rate_K = 0; float rate_K1 = 0;
float rate_K2 = 0;
float in_speed; float in_speed;
float out_speed; float out_speed;
float last_angle; float last_angle;
@@ -115,11 +116,15 @@ int32_t stop_cnt = 0;
int32_t last_lb = 0; int32_t last_lb = 0;
int32_t last_rb = 0; int32_t last_rb = 0;
float start_dis = 0; float start_dis = 0;
float set_dis=1.f; float set_dis = 1.f;
uint8_t go_cnt = 0; // 0 停车 1 测试侧向风扇 2 启动 3成功运行 uint8_t go_cnt = 0; // 0 停车 1 测试侧向风扇 2 启动 3成功运行
float bug_sw = 0; float bug_sw = 0;
float out_sw = 0; float out_sw = 0;
float sho_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:无状态 0:无状态
1:弯道 1:弯道
@@ -166,12 +171,14 @@ void sport_motion()
if (cnt7 >= 50 && (uint8_t)sho_sw == 1) { if (cnt7 >= 50 && (uint8_t)sho_sw == 1) {
bt_fly_flag = bt_run_flag = 0; bt_fly_flag = bt_run_flag = 0;
bt_printf("up"); bt_printf("up");
up_show_flag = 1;
go_cnt = 0; go_cnt = 0;
} }
// 異常值停车 // 異常值停车
if (fabs(in_angle - last_angle) > 45.f && (uint8_t)bug_sw == 1) { if (fabs(in_angle - last_angle) > 45.f && (uint8_t)bug_sw == 1) {
bt_fly_flag = bt_run_flag = 0; bt_fly_flag = bt_run_flag = 0;
bt_printf("bug"); bt_printf("bug");
bug_show_flag = 1;
go_cnt = 0; go_cnt = 0;
} }
// 斑马线停车,摄像头识别，屏蔽发车前的十秒钟 // 斑马线停车,摄像头识别，屏蔽发车前的十秒钟
@@ -180,12 +187,14 @@ void sport_motion()
} }
if (stop_flag == 1) { if (stop_flag == 1) {
bt_printf("finish"); bt_printf("finish");
finish_show_flag = 1;
bt_fly_flag = bt_run_flag = 0; bt_fly_flag = bt_run_flag = 0;
go_cnt = 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"); bt_printf("out");
out_show_flag = 1;
go_cnt = 0; go_cnt = 0;
bt_fly_flag = bt_run_flag = 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); PID_SetTunings(&pos_pid, po_Kp1, po_Ki1, po_Kd1);
} else if ((last_state != in_state) && in_state == 1) { // 弯道 } else if ((last_state != in_state) && in_state == 1) { // 弯道
bt_printf("to 入弯"); 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_angle_pid, an_Kp0, an_Ki0, an_Kd0);
PID_SetTunings(&far_gyro_pid, gy_Kp0, gy_Ki0, gy_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) { // } else if ((last_state != in_state) && in_state == 5) { //
bt_printf("to 障碍"); bt_printf("to 障碍");
set_speed = set_speed4; set_speed = set_speed4;
start_dis = 0;
PID_SetTunings(&far_angle_pid, za_Kp, za_Ki, za_Kd); PID_SetTunings(&far_angle_pid, za_Kp, za_Ki, za_Kd);
PID_SetTunings(&far_gyro_pid, zg_Kp, zg_Ki, zg_Kd); PID_SetTunings(&far_gyro_pid, zg_Kp, zg_Ki, zg_Kd);
PID_SetTunings(&pos_pid, zp_Kp, zp_Ki, zp_Kd); PID_SetTunings(&pos_pid, zp_Kp, zp_Ki, zp_Kd);
@@ -241,9 +254,9 @@ void sport_motion()
if (in_state == 1) { if (in_state == 1) {
if (fabs(in_pos) <= 12.0f) { 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 { } 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));
} }
} }
// 记录上一次状态 // 记录上一次状态

7
app/jj_motion.h
View File

@@ -59,7 +59,8 @@ extern float set_speed;
extern float set_speed4; extern float set_speed4;
extern float last_angle; extern float last_angle;
extern float yuc_Kp; extern float yuc_Kp;
extern float rate_K; extern float rate_K1;
extern float rate_K2;
extern float start_dis; extern float start_dis;
extern float set_dis; extern float set_dis;
extern int32_t pwm_duty_ls; extern int32_t pwm_duty_ls;
@@ -72,6 +73,10 @@ extern uint8_t in_stop;
extern float bug_sw; extern float bug_sw;
extern float out_sw; extern float out_sw;
extern float sho_sw; extern float sho_sw;
extern uint8_t bug_show_flag;
extern uint8_t out_show_flag;
extern uint8_t up_show_flag;
extern uint8_t finish_show_flag;
void sport_pid_init(); void sport_pid_init();
void sport_motion(void); void sport_motion(void);
#endif #endif

4
app/jj_param.c
View File

@@ -24,11 +24,11 @@ void jj_param_eeprom_init(void)
PARAM_REG(gyro_Kp0, &gy_Kp0, EFLOAT, 1, "gy_P0:"); // 注冊 PARAM_REG(gyro_Kp0, &gy_Kp0, EFLOAT, 1, "gy_P0:"); // 注冊
PARAM_REG(gyro_Ki0, &gy_Ki0, EFLOAT, 1, "gy_I0:"); // 注冊 PARAM_REG(gyro_Ki0, &gy_Ki0, EFLOAT, 1, "gy_I0:"); // 注冊
PARAM_REG(gyro_Kd0, &gy_Kd0, EFLOAT, 1, "gy_D0:"); PARAM_REG(gyro_Kd0, &rate_K2, EFLOAT, 1, "ra_K2:");
PARAM_REG(can_Kp1, &cn_Kp1, EFLOAT, 1, "cn_P1:"); // 注冊 PARAM_REG(can_Kp1, &cn_Kp1, EFLOAT, 1, "cn_P1:"); // 注冊
PARAM_REG(can_Ki1, &yuc_Kp, EFLOAT, 1, "yu_P1:"); // 注冊 PARAM_REG(can_Ki1, &yuc_Kp, EFLOAT, 1, "yu_P1:"); // 注冊
PARAM_REG(can_Kd1, &rate_K, EFLOAT, 1, "ra_K:"); PARAM_REG(can_Kd1, &rate_K1, EFLOAT, 1, "ra_K1:");
PARAM_REG(fly_pwm, &bt_fly, EFLOAT, 1, "fly:"); PARAM_REG(fly_pwm, &bt_fly, EFLOAT, 1, "fly:");

12
app/page/page_dparam.c
View File

@@ -66,7 +66,17 @@ static void Loop()
ips200_show_float(80, 200, set_speed, 4, 2); ips200_show_float(80, 200, set_speed, 4, 2);
ips200_show_string(0, 220, "dis"); ips200_show_string(0, 220, "dis");
ips200_show_float(80, 220, start_dis, 4, 2); ips200_show_float(80, 220, start_dis, 4, 2);
if (up_show_flag == 1) {
ips200_show_string(180, 200, "up ");
} else if (out_show_flag == 1) {
ips200_show_string(180, 200, "out ");
} else if (bug_show_flag == 1) {
ips200_show_string(180, 200, "bug ");
} else if (finish_show_flag == 1) {
ips200_show_string(180, 200, "finish");
} else {
ips200_show_string(180, 200, " ");
}
ips200_show_string(180, 0, "ls"); ips200_show_string(180, 0, "ls");
ips200_show_float(220, 0, pwm_duty_ls, 4, 1); ips200_show_float(220, 0, pwm_duty_ls, 4, 1);
ips200_show_string(180, 20, "rs"); ips200_show_string(180, 20, "rs");

3
app/page/page_gocar.c
View File

@@ -30,6 +30,7 @@ static void Setup()
{ {
ips200_clear(); ips200_clear();
ips200_show_string(0, 2, "gocar");
Print_Curser(Curser, Curser_Last, RGB565_PURPLE); Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
ips200_show_string(10, 200, "go_flag:"); ips200_show_string(10, 200, "go_flag:");
ips200_show_int(80, 200, go_cnt, 1); ips200_show_int(80, 200, go_cnt, 1);
@@ -103,7 +104,7 @@ static void Event(page_event event)
} }
if (Curser == 4 && bt_run_flag != 0) { if (Curser == 4 && bt_run_flag != 0) {
bt_run_flag = 0; bt_run_flag = 0;
go_cnt=0; go_cnt = 0;
} }
} else if (page_event_press_long == event) { } else if (page_event_press_long == event) {

2
app/page/page_stopcar.c
View File

@@ -26,7 +26,7 @@ static void jj_param_show();
static void Setup() static void Setup()
{ {
ips200_clear(); ips200_clear();
ips200_show_string(0, 2,"barrir"); ips200_show_string(0, 2,"stopsw");
Print_Curser(Curser, Curser_Last, RGB565_PURPLE); Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
for (int16 i = 0; i < LINE_END; i++) { for (int16 i = 0; i < LINE_END; i++) {
ips200_show_string(0, i * 18 + 20, Param_Data[i +Strat_param ].text); ips200_show_string(0, i * 18 + 20, Param_Data[i +Strat_param ].text);