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

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);
}
}
}