diff --git a/app/jj_motion.c b/app/jj_motion.c index 249e3d0..c3f0c58 100644 --- a/app/jj_motion.c +++ b/app/jj_motion.c @@ -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)); } } // 记录上一次状态 diff --git a/app/jj_motion.h b/app/jj_motion.h index e8f4d2d..07b41f3 100644 --- a/app/jj_motion.h +++ b/app/jj_motion.h @@ -59,7 +59,8 @@ extern float set_speed; extern float set_speed4; extern float last_angle; extern float yuc_Kp; -extern float rate_K; +extern float rate_K1; +extern float rate_K2; extern float start_dis; extern float set_dis; extern int32_t pwm_duty_ls; @@ -72,6 +73,10 @@ extern uint8_t in_stop; extern float bug_sw; extern float out_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_motion(void); #endif \ No newline at end of file diff --git a/app/jj_param.c b/app/jj_param.c index ac6debe..411fc24 100644 --- a/app/jj_param.c +++ b/app/jj_param.c @@ -24,11 +24,11 @@ void jj_param_eeprom_init(void) PARAM_REG(gyro_Kp0, &gy_Kp0, EFLOAT, 1, "gy_P0:"); // 注冊 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_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:"); diff --git a/app/page/page_dparam.c b/app/page/page_dparam.c index ab0aaff..09fe971 100644 --- a/app/page/page_dparam.c +++ b/app/page/page_dparam.c @@ -66,7 +66,17 @@ static void Loop() ips200_show_float(80, 200, set_speed, 4, 2); ips200_show_string(0, 220, "dis"); 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_float(220, 0, pwm_duty_ls, 4, 1); ips200_show_string(180, 20, "rs"); diff --git a/app/page/page_gocar.c b/app/page/page_gocar.c index 9b39e4e..ce85a3c 100644 --- a/app/page/page_gocar.c +++ b/app/page/page_gocar.c @@ -30,6 +30,7 @@ static void Setup() { ips200_clear(); + ips200_show_string(0, 2, "gocar"); Print_Curser(Curser, Curser_Last, RGB565_PURPLE); ips200_show_string(10, 200, "go_flag:"); 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) { bt_run_flag = 0; - go_cnt=0; + go_cnt = 0; } } else if (page_event_press_long == event) { diff --git a/app/page/page_stopcar.c b/app/page/page_stopcar.c index 786cfb2..c7d2a1e 100644 --- a/app/page/page_stopcar.c +++ b/app/page/page_stopcar.c @@ -26,7 +26,7 @@ static void jj_param_show(); static void Setup() { ips200_clear(); - ips200_show_string(0, 2,"barrir"); + ips200_show_string(0, 2,"stopsw"); Print_Curser(Curser, Curser_Last, RGB565_PURPLE); for (int16 i = 0; i < LINE_END; i++) { ips200_show_string(0, i * 18 + 20, Param_Data[i +Strat_param ].text);