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

fix: 減小側面中間值

Browse Source
This commit is contained in:
井明江
2024-07-08 21:02:14 +08:00
parent 0313c90c28
commit 73a30fc187
1 changed files with 14 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
app/jj_motion.c
View File

@@ -81,7 +81,6 @@ float out_pos;
float set_pos = 0.0f;
// float set_gyro = 0.0f;
float in_speed;
float out_speed;
@@ -100,7 +99,7 @@ int cnt6 = 0;
int cnt7 = 0;
int cnt8 = 0;
int if_start = 0;
float shock_stop=0;
float shock_stop = 0;
uint8_t in_state = 0;
uint8_t in_stop = 0;
uint8_t last_state = 0;
@@ -148,13 +147,17 @@ void sport_motion()
get_vol();
in_gyro = imu660ra_gyro_z;
// 抖动停车
if (fabs(out_gyro) < 700) {
PID_SetOutputLimits(&speed_pid, -0.0f, 2800.0f);
} else {
PID_SetOutputLimits(&speed_pid, -0.0f, 2100.0f);
}
if (imu660ra_acc_z <= 800) {
cnt7++;
} else {
cnt7 = 0;
}
if (cnt7 >= 100&&shock_stop==1) {
if (cnt7 >= 100 && shock_stop == 1) {
bt_fly_flag = bt_run_flag = 0;
bt_printf("ting");
}
@@ -163,10 +166,8 @@ void sport_motion()
stop_flag = 1;
}
if (stop_flag == 1) {
cnt4++;
if (cnt4 >= 100) {
bt_fly_flag = bt_run_flag = 0;
}
bt_fly_flag = bt_run_flag = 0;
}
/* 动力风扇设置 */
if (1 == bt_run_flag) {
@@ -233,8 +234,8 @@ void sport_motion()
cnt5 = 0;
}
// 并级pid，限幅
pwm_duty_ls = (int32_t)myclip_f(300 - out_pos, 100.0f, 500.f);
pwm_duty_rs = (int32_t)myclip_f(300 + out_pos, 100.0f, 500.f);
pwm_duty_ls = (int32_t)myclip_f(250 - out_pos, 50.0f, 500.f);
pwm_duty_rs = (int32_t)myclip_f(250 + out_pos, 50.0f, 500.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 300.0f, 3000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 300.0f, 3000.f);
@@ -275,17 +276,17 @@ void sport_pid_init()
PID(&pos_pid, &in_angle, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&pos_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&pos_pid, 20);
PID_SetOutputLimits(&pos_pid, -200.0f, 200.0f);
PID_SetOutputLimits(&pos_pid, -250.0f, 250.0f);
/* 角速度控制 */
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, -1200.0f, 1200.0f);
PID_SetOutputLimits(&far_gyro_pid, -1500.0f, 1500.0f);
/* 速度控制 */
PID(&speed_pid, &in_speed, &out_speed, &set_speed, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_M, _PID_CD_DIRECT);
PID_SetMode(&speed_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&speed_pid, 20);
PID_SetOutputLimits(&speed_pid, -0.0f, 1800.0f);
PID_SetOutputLimits(&speed_pid, -0.0f, 2100.0f);
}