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

feat:发车靠积分屏蔽斑马线；发车起步先检查侧面风扇能否成功拖动；负压状态停车

Browse Source
This commit is contained in:
井明江
2024-08-11 20:08:38 +08:00
parent 53d06467d6
commit 92eaf372ff
4 changed files with 66 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
app/by_fan_control.c
View File

@@ -70,8 +70,8 @@ void by_pwm_update_duty(uint32_t update_pwm_duty1, uint32_t update_pwm_duty2)
*/
void by_pwm_power_duty(int32_t bpwm_duty_ls, int32_t bpwm_duty_rs, int32_t bpwm_duty_lb, int32_t bpwm_duty_rb)
{
bpwm_duty_ls = clip_s32(bpwm_duty_ls, 500, 980);
bpwm_duty_rs = clip_s32(bpwm_duty_rs, 500, 980);
bpwm_duty_ls = clip_s32(bpwm_duty_ls, 500, 1000);
bpwm_duty_rs = clip_s32(bpwm_duty_rs, 500, 1000);
bpwm_duty_lb = clip_s32(bpwm_duty_lb, 2500, 6000);
bpwm_duty_rb = clip_s32(bpwm_duty_rb, 2500, 6000);
pwm_set_duty(FAN_LS_PWM_PIN, bpwm_duty_ls);

75
app/jj_motion.c
View File

@@ -93,14 +93,14 @@ float set_speed3 = 666.f;
float set_speed4 = 720.f;
int cnt1 = 0;
int cnt2 = 0;
int cnt3 = 0;
int cnt4 = 0;
int cnt3 = 0; //
int cnt4 = 0; //
int cnt5 = 0;
int cnt6 = 0;
int cnt6 = 0; //
int cnt7 = 0;
int cnt8 = 0;
int cnt9 = 0;
int cnt10 = 0;
int cnt9 = 0; //
int cnt10 = 0; //
int if_start = 0;
float shock_stop = 0;
uint8_t in_state = 0;
@@ -111,11 +111,12 @@ int32_t pwm_duty_ls;
int32_t pwm_duty_rs;
int32_t pwm_duty_lb;
int32_t pwm_duty_rb;
float qd_down = 0.0f;
float last_gy = 0.0f;
int32_t stop_cnt = 0;
int32_t last_lb = 0;
int32_t last_rb = 0;
int32_t stop_cnt = 0;
int32_t last_lb = 0;
int32_t last_rb = 0;
float start_dis = 0;
int32_t game_over_flag = 0; // 任务结束
uint8_t go_flag = 0;
/*
0:无状态
1:弯道
@@ -175,18 +176,14 @@ void sport_motion()
if (stop_flag == 1) {
bt_printf("finish");
bt_fly_flag = bt_run_flag = 0;
game_over_flag = 1;
}
// 出界停车
if (2 == in_stop) {
stop_cnt++;
if (stop_cnt >= 5) {
bt_printf("out");
stop_cnt = 0;
bt_fly_flag = bt_run_flag = 0;
}
} else {
stop_cnt = 0;
// 出界停车,已经积分路过斑马线，保证斑马线不会触发
if (2 == in_stop && start_dis >= 1) {
bt_printf("out");
bt_fly_flag = bt_run_flag = 0;
}
//////////////////////////////////////////////// 动力风扇设置 */////////////////////////////////////////////////////////
if (1 == bt_run_flag) {
cnt1++;
@@ -194,6 +191,7 @@ void sport_motion()
cnt5++;
if (if_start == 0) {
cnt8++;
start_dis += in_speed * 0.000001; // 斑马线积分
if (cnt8 >= 5000) {
if_start = 1;
}
@@ -234,7 +232,7 @@ void sport_motion()
}
if (in_state == 1) {
set_speed = set_speed0 - fabs(rate_K * myclip_f(in_pos, -50.f, 50.f));
set_speed = set_speed0 - fabs(rate_K * myclip_f(in_pos, -100.f, 100.f));
}
last_state = in_state;
@@ -257,8 +255,14 @@ void sport_motion()
// 并级pid，限幅
pwm_duty_ls = (int32_t)myclip_f(180 - out_pos, 100.0f, 500.f);
pwm_duty_rs = (int32_t)myclip_f(180 + out_pos, 100.0f, 500.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 1000.0f, 3000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 1000.0f, 3000.f);
if (out_speed >= 1000) {
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 1000.0f, 3000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 1000.0f, 3000.f);
} else {
pwm_duty_lb = (int32_t)myclip_f(out_speed + 4 * out_gyro, 1000.0f, 3000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - 4 * out_gyro, 1000.0f, 3000.f);
}
if (pwm_duty_lb - last_lb > 1000) {
pwm_duty_lb = (pwm_duty_lb + last_lb) / 2;
}
@@ -275,13 +279,26 @@ void sport_motion()
}
///////////////////////////////////////////////////* 升力风扇设置 */ /////////////////////////////////////////////////////////////////////////////////////////////////
if (bt_fly_flag == 0) {
by_pwm_update_duty(500, 500);
} else {
by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
// 防止起步拉满
if (bt_run_flag == 0) {
if (game_over_flag == 0) {
by_pwm_update_duty(500, 500);
} else {
by_pwm_update_duty(700, 700);
}
by_pwm_power_duty(680, 680, 4000, 4000);
} else {
if (in_state == 1) {
by_pwm_update_duty(bt_fly + 500 + in_pos, bt_fly + 500 + in_pos);
} else {
by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
}
// 防止起步拉满
if (bt_run_flag == 0&&go_flag==1) {
by_pwm_power_duty(550, 550, 3000, 3000);
}
else if(bt_run_flag == 0&&go_flag==2)
{
by_pwm_power_duty(550, 550, 4000, 4000);
}
}
}

2
app/jj_motion.h
View File

@@ -65,7 +65,7 @@ extern int32_t pwm_duty_ls;
extern int32_t pwm_duty_rs;
extern int32_t pwm_duty_lb;
extern int32_t pwm_duty_rb;
extern uint8_t go_flag;
extern uint8_t in_state;
extern uint8_t in_stop;
void sport_pid_init();

27
app/page/page_menu.c
View File

@@ -3,6 +3,7 @@
#include "page.h"
#include "by_frame.h"
#include "jj_blueteeth.h"
#include "jj_motion.h"
#define LINE_HEAD 1
#define LINE_END 5
@@ -12,7 +13,7 @@ static char Text[] = "Menu";
static int8_t Curser = LINE_HEAD; // 定义光标位置
static int8_t Curser_Last = LINE_HEAD; // 定义光标位置
uint32_t cnt = 0;
uint8_t go_flag = 0;
uint32_t reset[3];
static void Print_Menu_p(void);
/***************************************************************************************
@@ -50,13 +51,14 @@ static void Exit()
*/
static void Loop()
{
ips200_show_string(0, 200, "go_flag:");
ips200_show_int(180, 200, go_flag, 2);
if (go_flag == 1) {
system_delay_ms(1000);
bt_fly_flag = 1;
system_delay_ms(1000);
bt_run_flag = 1;
go_flag = 0;
} else if (go_flag == 2) {
system_delay_ms(2000);
bt_run_flag = 1;
}
}
@@ -79,13 +81,18 @@ static void Event(page_event event)
Page_Shift(Curser); // 切换到光标选中的页面
}
} else if (page_event_press_long == event) {
go_flag = 1;
uint32_t temp = 0x5c;
for (int i = 0; i < 3; i++) {
by_frame_send(&temp);
if (go_flag == 0) {
go_flag = 1;
} else if (go_flag == 1) {
go_flag = 2;
}
ips200_clear();
// uint32_t temp = 0x5c;
// for (int i = 0; i < 3; i++) {
// by_frame_send(&temp);
// }
// ips200_clear();
}
if (Curser < LINE_HEAD) {