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

最新一般有刷加无刷

Browse Source
This commit is contained in:
井明江
2024-07-06 21:54:16 +08:00
parent dda2649f6d
commit 9f9aa26197
4 changed files with 37 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
app/jj_motion.c
View File

@@ -104,9 +104,8 @@ int32_t pwm_duty_ls;
int32_t pwm_duty_rs;
int32_t pwm_duty_lb;
int32_t pwm_duty_rb;
uint32_t stop_have = 0;
float qd_down = 0.0f;
float last_gy = 0.0f;
float qd_down = 0.0f;
float last_gy = 0.0f;
/*
0:无状态
1:弯道
@@ -169,11 +168,7 @@ void sport_motion()
cnt1++;
cnt2++;
cnt5++;
// 屏蔽斑马线
if (stop_have == 0) {
stop_have = 1;
by_frame_send(&stop_have);
}
// pid参数切换
if ((last_state != in_state) && (in_state == 0 || in_state == 2)) { // 直道
cnt3_flag = 1;
@@ -255,9 +250,9 @@ void sport_motion()
cnt3 = 0;
}
if (in_speed > set_speed) {
qd_down = 50;
qd_down = 25;
} else if (in_speed > set_speed - 100) {
qd_down = 20;
qd_down = 10;
} else {
qd_down = 0;
}
@@ -268,35 +263,25 @@ void sport_motion()
cnt3_flag = 2;
cnt3 = 0;
}
if (in_speed > set_speed) {
qd_down = 4 * fabs(in_pos) + 20;
} else if (in_speed > set_speed - 100) {
qd_down = 2 * fabs(in_pos) + 10;
} else {
qd_down = 0;
}
} else if (in_state == 5) {
if (cnt3 >= 400) // 200ms
{
bt_printf("to 开张");
cnt3_flag = 2;
cnt3 = 0;
}
if (in_speed > set_speed - 100) {
qd_down = 150;
} else if (in_speed > set_speed - 200) {
qd_down = 75;
} else {
qd_down = 25;
}
if (in_speed > set_speed)
qd_down = (in_speed - set_speed) * 0.5 + 40;
else
qd_down = -10;
}
by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down);
} else {
if (in_state == 1) {
qd_down = 50;
} else if (in_state == 3) {
qd_down = 10;
if (in_state == 1) { // 消除s弯降气垫
if (in_speed >= set_speed / 1.2) {
qd_down = 40;
} else if (in_speed >= set_speed / 1.5) {
qd_down = 25;
} else if (in_speed >= set_speed / 2) {
qd_down = 10;
}else {
qd_down = 0;
}
} else {
qd_down = 0;
}
@@ -327,7 +312,7 @@ void sport_pid_init()
PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp1, gy_Ki1, gy_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); // m是增量
PID_SetMode(&far_gyro_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&far_gyro_pid, 20);
PID_SetOutputLimits(&far_gyro_pid, -6000.0f, 6000.0f);
PID_SetOutputLimits(&far_gyro_pid, -4800.0f, 4800.0f);
/* 速度控制 */
PID(&speed_pid, &in_speed, &out_speed, &set_speed, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_M, _PID_CD_DIRECT);